Integration is implemented. But there are problems.

The coordinates are not working correctly. When I run integration on a
known dataset the body seems to turn in the opposite direction of
reality. It's probably best to change it to NED and FRD. Currenty it is
ENU and RFU.

4 files changed, 139 insertions, 10 deletions

diff --git a/src/body.cpp b/src/body.cpp
new file mode 100644
index 0000000..b9ea683
--- /dev/null
+++ b/src/body.cpp
@@ -0,0 +1,50 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  body.cpp
+ *
+ *    Description:  Method definitions for body class.
+ *
+ *        Version:  1.0
+ *        Created:  03/17/2017 08:07:35 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+
+#include <Eigen/Dense>
+#include "body.h"
+
+    void
+Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
+{
+    Vector3d bias(X.segment(6,3));
+    Vector3d gravity_world(0.,0.,-9.80655);
+
+    Matrix<double,6,3> A;
+    Matrix<double,6,1> b;
+    A = Matrix<double,6,3>::Zero();
+    b = Matrix<double,6,1>::Zero();
+
+    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+    A.block(0,0,3,3) = Rbw;
+    Matrix<double,3,3> W;
+    skewSymmetric(ang,W);
+    A.block(3,0,3,3) = -W;
+    b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
+
+    X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
+    return ;
+}		/* -----  end of method Body::motionModel  ----- */
+
+    void
+Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
+{
+    y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
+    return ;
+}		/* -----  end of method Body::skewSymmetric  ----- */
+
diff --git a/src/body.h b/src/body.h
new file mode 100644
index 0000000..d668957
--- /dev/null
+++ b/src/body.h
@@ -0,0 +1,42 @@
+#ifndef  body_INC
+#define  body_INC
+#include <Eigen/Dense>
+#include <iostream>
+using Eigen::Matrix;
+using Eigen::Vector3d;
+using Eigen::Quaternion;
+
+/*
+ * =====================================================================================
+ *        Class:  Body
+ *  Description:  State representation of body.
+ * =====================================================================================
+ */
+class Body
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        Body (){};                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+        void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt);
+        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+}; /* -----  end of class Body  ----- */
+
+
+#endif   /* ----- #ifndef body_INC  ----- */
diff --git a/src/main.cpp b/src/main.cpp
index 150a141..44e09f3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -32,12 +32,15 @@ imgCallback(const message *msg)
     return;
 }
 void 
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
 {
-    if (!seenutm || !seenpva) return;
+    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
     using Eigen::Vector3d;
     Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z);
     Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z);
+    Body mu;
+    double dtf = dt.secs+dt.nsecs*1e-9;
+    mu.motionModel(X,acc,ang,q,dtf);
 }
 
 void
@@ -66,10 +69,12 @@ pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<
 void
 utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
 {
-    seenutm=true;
-    X[0] = msg.utm.easting;
-    X[1] = msg.utm.northing;
-    X[2] = msg.utm.up;
+    if (!seenutm) {
+        seenutm=true;
+        X[0] = msg.utm.easting;
+        X[1] = msg.utm.northing;
+        X[2] = msg.utm.up;
+    }
     return;
 }
 #endif     /* -----  not USE_ROS  ----- */
@@ -136,10 +141,34 @@ parseLine(char *line, message *data)
     }
 }
 
-int main(int argc, char **argv)
+timestamp 
+update_dt(const timestamp t, timestamp *t_old)
+{
+    double dtf=0;
+    if (t_old->secs!=0 || t_old->nsecs!=0) {
+        double tf = t.secs+1e-9*t.nsecs;
+        double t_oldf = t_old->secs+1e-9*t_old->nsecs;
+        dtf = tf-t_oldf;
+    }
+    t_old->secs=t.secs;
+    t_old->nsecs=t.nsecs;
+
+    timestamp dt;
+    dt.secs = (int) dtf;
+    dt.nsecs = (int)((dtf-(double)dt.secs)*1e9);
+    
+    return dt;
+}
+
+int 
+main(int argc, char **argv)
 {
     Eigen::Quaternion<double> qbw;
     Eigen::Matrix<double,9,1> bodyStateVector;
+    bodyStateVector.segment(6,3) << 0.01465135, -0.03713532, -0.00709229;
+    timestamp dt{0,0};
+    timestamp t_old{0,0};
+
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
@@ -151,6 +180,7 @@ while (scanf("%s", line)!=EOF) {
     
     switch ( msg.msg_type ) {
         case BESTUTM:	
+            dt = update_dt(msg.stamp, &t_old);
             utmCallback(msg,bodyStateVector);
             break;
 
@@ -159,12 +189,16 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case INSPVAS:	
+            dt = update_dt(msg.stamp, &t_old);
             pvaCallback(msg, bodyStateVector, qbw);
-            std::cout << bodyStateVector << std::endl;
+            //std::cout << bodyStateVector.transpose() << std::endl;
+            if (seenutm)
+                printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[1],bodyStateVector[0]);
             break;
 
         case RAWIMUS:	
-            imuCallback(msg, bodyStateVector, qbw);
+            dt = update_dt(msg.stamp, &t_old);
+            imuCallback(msg, bodyStateVector, qbw, dt);
             break;
 
         default:	
diff --git a/src/main.h b/src/main.h
index e3dd911..2139248 100644
--- a/src/main.h
+++ b/src/main.h
@@ -12,6 +12,8 @@
 #include <Eigen/Dense>
 #include <iostream>
 
+#include "body.h"
+
 #define MAXLINE 8192
 #define MAXFILENAME 1024
 
@@ -70,12 +72,13 @@ typedef struct {
 } message;
 
 int parseLine(char *line, message *msg);
+timestamp update_dt(const timestamp t, timestamp *t_old);
 
 #ifdef  USE_ROS
 void imuCallback();
 #else      /* -----  not USE_ROS  ----- */
 void imgCallback(const message *msg);
-void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);
+void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt);
 void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);
 void utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X);
 #endif     /* -----  not USE_ROS  ----- */