Major rewrite moves state variables into state.

Rather than maintain X and P in the main function they are moved into
the body and state classes, respectively. This will become much more
important when features are added and the accounting becomes more
complicated.

6 files changed, 249 insertions, 155 deletions

diff --git a/src/body.cpp b/src/body.cpp
index c4d8a82..2832ea4 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -15,10 +15,71 @@
  *
  * =====================================================================================
  */
-
-#include <Eigen/Dense>
 #include "body.h"
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: update
+ * Description:  Increments the state by dx after a Kalman update.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::update ( const Matrix<double,9,1> &dx )
+{
+    X += dx;
+    return ;
+}		/* -----  end of method Body::update  ----- */
+
+void
+Body::vel ( const Matrix<double,3,1> &v )
+{
+    X.segment(3,3) = v;
+    return ;
+}		/* -----  end of method Body::vel  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: S
+ * Description:  Returns the matrix S=HPH.T+R. HPH.T is trivial, so Body::H() is
+ * not called explicitly.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,1,1>
+Body::S ( const Matrix<double,9,9> &P )
+{
+    Matrix<double,1,1> S;
+    S << P(2,2);
+    return S+R();
+}		/* -----  end of method Body::S  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: enu
+ * Description:  Stores the position input as ENU. Internally stored as NED.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::enu ( const UTM &utm )
+{
+    utm_c = utm.zone_c;
+    utm_i = utm.zone_i;
+    X[0] = utm.northing;
+    X[1] = utm.easting;
+    X[2] = -utm.up;
+    return ;
+}		/* -----  end of method Body::enu  ----- */
+
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: R
+ * Description:  Returns R matrix of measurement noise for height measurement.
+ *--------------------------------------------------------------------------------------
+ */
 Matrix<double,1,1> 
 Body::R()
 {
@@ -27,6 +88,13 @@ Body::R()
     return R;
 }
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: Q
+ * Description:  Returns Q matrix of process noise for the body.
+ *--------------------------------------------------------------------------------------
+ */
 Matrix<double,9,9>
 Body::Q (double dt)
 {
@@ -45,15 +113,16 @@ Body::Q (double dt)
  *--------------------------------------------------------------------------------------
  *       Class:  Body
  *      Method:  Body :: H
- * Description:  Writes the Jacobian of the measurement model into H.
+ * Description:  Returns the Jacobian of the measurement model, H.
  *--------------------------------------------------------------------------------------
  */
-void
-Body::H ( Matrix<double,1,9> &H )
+Matrix<double,1,9>
+Body::H ( )
 {
+    Matrix<double,1,9> H;
     H = Matrix<double,1,9>::Zero();
     H[0,2] = 1;
-    return ;
+    return H ;
 }		/* -----  end of method Body::H  ----- */
 
 
@@ -61,11 +130,11 @@ Body::H ( Matrix<double,1,9> &H )
  *--------------------------------------------------------------------------------------
  *       Class:  Body
  *      Method:  Body :: h
- * Description:  Writes the predicted measurement vector into h.
+ * Description:  Returns the predicted measurement vector, h.
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,1,1>
-Body::h ( const Matrix<double,9,1> &X)
+Body::h ( )
 {
     Matrix<double,1,1> h;
     h[0] = X[2];
@@ -76,11 +145,11 @@ Body::h ( const Matrix<double,9,1> &X)
  *--------------------------------------------------------------------------------------
  *       Class:  Body
  *      Method:  Body :: F
- * Description:  Computes the Jacobian of the motion model of the body.
+ * Description:  Returns the Jacobian of the motion model of the body.
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,9,9>
-Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
+Body::F ( const Vector3d &ang, const Quaternion<double> &q )
 {
     Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
     Matrix<double,3,3> Rbw(q.toRotationMatrix());
@@ -100,7 +169,7 @@ Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<dou
  *--------------------------------------------------------------------------------------
  */
 void
-Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
+Body::motionModel ( 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);
@@ -136,3 +205,20 @@ Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
 }		/* -----  end of method Body::skewSymmetric  ----- */
 
 
+void
+Body::unicsv ( )
+{
+    printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", utm_i, utm_c,
+            X[1], X[0], -X[2],
+            X[3], X[4], X[5],
+            X[6], X[7], X[8]);
+    return ;
+}		/* -----  end of method Body::unicsv  ----- */
+
+    void
+Body::accelerometer_bias ( const Vector3d &b )
+{
+    X.segment(6,3) = b;
+    return ;
+}		/* -----  end of method State::accelerometer_bias  ----- */
+
diff --git a/src/body.h b/src/body.h
index 04196d2..f201683 100644
--- a/src/body.h
+++ b/src/body.h
@@ -2,6 +2,8 @@
 #define  body_INC
 #include <Eigen/Dense>
 #include <iostream>
+#include "types.h"
+
 using Eigen::Matrix;
 using Eigen::Vector3d;
 using Eigen::Quaternion;
@@ -21,16 +23,22 @@ class Body
         /* ====================  ACCESSORS     ======================================= */
 
         /* ====================  MUTATORS      ======================================= */
+        void accelerometer_bias( const Vector3d &b);
+        void enu( const UTM &utm);
+        void update(const Matrix<double,9,1> &dx);
+        void vel(const Matrix<double,3,1> &v);
 
         /* ====================  OPERATORS     ======================================= */
-        void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, 
-                const Vector3d &ang, const Quaternion<double> &q, const double dt);
-        Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
-        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
-        void H(Matrix<double, 1,9> &H);
-        Matrix<double,1,1> h(const Matrix<double,9,1> &X);
+        Matrix<double,9,9> F(const Vector3d &ang, const Quaternion<double> &q);
+        Matrix<double,1,9> H();
+        Matrix<double,1,1> h();
+        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
+                const Quaternion<double> &q, const double dt);
         Matrix<double,9,9> Q(double dt);
         Matrix<double,1,1> R();
+        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
+        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
+        void unicsv();
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -41,6 +49,9 @@ class Body
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
+        Matrix<double,9,1> X;
+        char utm_c;
+        int utm_i;
 
 }; /* -----  end of class Body  ----- */
 
diff --git a/src/main.cpp b/src/main.cpp
index c3c6fa9..6cc4317 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,6 +22,7 @@
 #include "main.h"
 bool seenutm=false;
 bool seenpva=false;
+bool seencov=false;
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -30,15 +31,15 @@ using std::cerr;
 
 #else      /* -----  not USE_ROS  ----- */
 void
-covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q)
+covCallback(const message &msg, State &mu, const Quaternion<double> &q)
 {
-    if (seenutm==false || seenpva==false) {
-        Matrix3d Rbw(q.toRotationMatrix());
-        Matrix3d Renuned;
-        Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
-        P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
-        P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
-    }
+    if (seencov && seenutm && seenpva) return;
+    seencov=true;
+    Matrix3d Rbw(q.toRotationMatrix());
+    Matrix3d Renuned;
+    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
+    mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose());
+    mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
 }
 
 void
@@ -48,28 +49,21 @@ imgCallback(const message *msg)
 }
 
 void 
-imuCallback(const message &msg, Matrix<double,9,1> &X, 
-        Matrix<double,9,9> &P, const Quaternion<double> &q,
-        const timestamp dt)
+imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt)
 {
-    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
+    if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return;
     Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
     Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
     // WARNING: This is the bias for 1112-1
     Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03);
-    ang-=ang_bias;
-    Body body;
-    State mu;
+    ang -= ang_bias;
     double dtf = dt.secs+dt.nsecs*1e-9;
 
-    Matrix<double,9,9> F;
-    F = body.F(X,ang,q);
-    mu.Pkk1(P,F,body.Q(dtf));
-    body.motionModel(X,acc,ang,q,dtf);
+    mu.motionModel(acc,ang,q,dtf);
 }
 
 void
-pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
+pvaCallback(const message &msg, State &mu, Quaternion<double> &q)
 {
     // Update quaternion
     using Eigen::AngleAxisd;
@@ -85,31 +79,24 @@ pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
     q = Quaternion<double>(R);
 
     // Set body velocity
-    if (!seenpva) {
+    if (!seenpva || !seencov || !seenutm) {
         Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
-        X.segment(3,3) = R.transpose()*vw;
+        mu.vel(R.transpose()*vw);
         seenpva=true;
     } 
     return;
 }
 
 void
-utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P)
+utmCallback(const message &msg, State &mu)
 {
-    if (!seenutm) {
+    if (!seenutm || !seencov || !seenpva) {
         seenutm=true;
-        X[0] = msg.utm.northing;
-        X[1] = msg.utm.easting;
-        X[2] = -msg.utm.up;
+        mu.enu(msg.utm);
     } else { // Perform Z measurement
-        State mu;
-        Body body;
-        Matrix<double,1,1> z,h;
-        Matrix<double,1,9> H;
-        z(0) = -msg.utm.up;
-        h = body.h(X);
-        body.H(H);
-        mu.update(X,P,H,h,z,body.R());
+        Matrix<double,1,1> z;
+        z << -msg.utm.up;
+        mu.update(z);
     }
     return;
 }
@@ -229,11 +216,12 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
+    State mu;
     Quaternion<double> qbw;
-    Matrix<double,9,1> bodyStateVector;
-    Matrix<double,9,9> P;
     // bias in FRD coordinates
-    bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
+    Vector3d acc_bias;
+    acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
+    mu.accelerometer_bias(acc_bias);
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
@@ -249,8 +237,7 @@ while (scanf("%s", line)!=EOF) {
     
     switch ( msg.msg_type ) {
         case BESTUTM:	
-            dt = update_dt(msg.stamp, &t_old);
-            utmCallback(msg,bodyStateVector,P);
+            utmCallback(msg, mu);
             break;
 
         case IMG:	
@@ -258,23 +245,19 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case INSPVAS:	
-            dt = update_dt(msg.stamp, &t_old);
-            pvaCallback(msg, bodyStateVector, qbw);
-            //std::cout << bodyStateVector.transpose() << std::endl;
-            if (seenutm)
-                printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,
-                        bodyStateVector[0],bodyStateVector[1],-bodyStateVector[2],
-                        bodyStateVector[3],bodyStateVector[4],bodyStateVector[5],
-                        bodyStateVector[6],bodyStateVector[7],bodyStateVector[8]);
+            pvaCallback(msg, mu, qbw);
+            if (seenutm && seencov && seenpva)
+                mu.unicsv();
             break;
 
         case RAWIMUS:	
             dt = update_dt(msg.stamp, &t_old);
-            imuCallback(msg, bodyStateVector, P, qbw, dt);
+            imuCallback(msg, mu, qbw, dt);
             break;
 
         case INSCOVS:
-            covCallback(msg, P, qbw);
+            covCallback(msg, mu, qbw);
+            break;
 
         default:	
             break;
diff --git a/src/main.h b/src/main.h
index 36802fd..2fef291 100644
--- a/src/main.h
+++ b/src/main.h
@@ -1,99 +1,29 @@
 #ifndef  main_INC
 #define  main_INC
 
-//#define USE_ROS            /* Uncomment to use with ROS */
-
-#ifndef  USE_ROS
+#include <cmath>
 #include <cstdio>
 #include <cstring>
-#endif     /* -----  USE_ROS  ----- */
-
-#include <cmath>
 #include <Eigen/Dense>
 #include <iostream>
 
 #include "body.h"
 #include "state.h"
+#include "types.h"
 
-#define MAXLINE 8192
-#define MAXFILENAME 1024
 
 using Eigen::Matrix;
 using Eigen::Matrix3d;
 using Eigen::Quaternion;
 using Eigen::Vector3d;
-// A struct for storing PVA covariance.
-typedef struct {
-    Matrix3d position,velocity,attitude;
-} covariance_t;
-
-// A struct for storing x,y,z data.
-typedef struct {
-    double x,y,z;
-} tuple;
-
-// A struct for storing velocity in world frame
-typedef struct {
-    double east, north, up;
-} velocity_t;
-
-// A struct for storing attitude data.
-typedef struct {
-    double roll,pitch,yaw;
-} attitude_t;
-
-// A struct for storing UTM data.
-typedef struct {
-    double northing,easting,up;
-    int zone_i;
-    char zone_c;
-} UTM;
-
-// A struct for storing GPS data.
-typedef struct {
-    double latitude, longitude, altitude;
-} gps;
-
-// Message types
-typedef enum {BESTUTM,IMG,INSCOVS,INSPVAS,RAWIMUS} message_type;
-
-/*
- * The message struct is a general container for all message types. Not all
- * members are used for all messages, so care must be taken not to use garbage
- * values. 
- */
-typedef struct {
-    int secs,nsecs;
-} timestamp;
-
-typedef struct {
-    // These should always be set.
-    timestamp stamp;
-    message_type msg_type;
-
-    // Only the members needed by the msg_type are stored.
-    tuple angular_velocity;
-    attitude_t attitude;
-    char image_names[2][MAXFILENAME];
-    tuple linear_acceleration;
-    gps position;
-    UTM utm;
-    velocity_t velocity;
-    covariance_t covariance;
-} 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 covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q);
 void imgCallback(const message *msg);
-void imuCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P,
-        const Quaternion<double> &q, const timestamp dt);
+void imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt);
 void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q);
-void utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P);
-#endif     /* -----  not USE_ROS  ----- */
+void utmCallback(const message &msg, State &mu);
 
 #endif   /* ----- #ifndef main_INC  ----- */
diff --git a/src/state.cpp b/src/state.cpp
index 294b522..304e5ed 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -17,18 +17,46 @@
  */
 #include "state.h"
 
+State::State ( )
+{
+    body = new Body;
+    P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
+    P.block(6,6,3,3) = 1e-6*Matrix3d::Identity();
+    return ;
+}		/* -----  end of method State::State  ----- */
+
+void
+State::vel ( const Matrix<double,3,1> &v )
+{
+    body->vel(v);
+    return ;
+}		/* -----  end of method State::vel  ----- */
+
+void
+State::enu ( const UTM &utm )
+{
+    utm_c = utm.zone_c;
+    utm_i = utm.zone_i;
+    body->enu(utm);
+    return ;
+}		/* -----  end of method State::enu  ----- */
+
 void 
-State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, 
-        const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
-        const Matrix<double,1,1> &z, const Matrix<double,1,1> &R)
+State::update ( const Matrix<double,1,1> &z )
 {
+    Matrix<double,1,9> H;
+    H=body->H();
     Matrix<double,1,1> S;
+    S=body->S(P);
     Matrix<double,9,1> K;
-    S = H*P*H.transpose() + R;
     K = P*H.transpose()*S.inverse();
     P = (Matrix<double,9,9>::Identity()-K*H)*P;
     P = 0.5*(P+P.transpose());
-    X += K*(z-h);
+    Matrix<double,9,1> dx;
+    Matrix<double,1,1> h;
+    h = body->h();
+    dx = K*(z-h);
+    body->update(dx);
 }
 
 /*
@@ -38,13 +66,52 @@ State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P,
  * Description:  Updates P_k|k-1
  *--------------------------------------------------------------------------------------
  */
-    void
-State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
-        const Matrix<double,9,9> &Q )
+void
+State::Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt)
 {
+    Matrix<double,9,9> F,Q;
+    Q = body->Q(dt);
+    F = body->F(ang,q);
     P = F*P*F.transpose()+Q;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
     return ;
 }		/* -----  end of method State::Pkk1  ----- */
 
+void
+State::motionModel ( const Vector3d &acc, const Vector3d &ang,
+    const Quaternion<double> &q, const double dt)
+{
+    Pkk1(ang,q,dt);
+    body->motionModel(acc,ang,q,dt);
+    return ;
+}		/* -----  end of method State::motionModel  ----- */
+
+void
+State::position_covariance ( const Matrix<double,3,3> &p )
+{
+    P.block(0,0,3,3) = p;
+    return ;
+}		/* -----  end of method State::position_covariance  ----- */
+
+void
+State::velocity_covariance ( const Matrix<double,3,3> &p )
+{
+    P.block(3,3,3,3) = p;
+    return ;
+}		/* -----  end of method State::velocity_covariance  ----- */
+
+void
+State::unicsv ( )
+{
+    body->unicsv();
+    return ;
+}		/* -----  end of method State::unicsv  ----- */
+
+void
+State::accelerometer_bias ( const Vector3d &b)
+{
+    body->accelerometer_bias(b);
+    return ;
+}		/* -----  end of method State::accelerometer_bias  ----- */
+
diff --git a/src/state.h b/src/state.h
index 743f5d8..1efb0c2 100644
--- a/src/state.h
+++ b/src/state.h
@@ -1,7 +1,14 @@
 #ifndef  state_INC
 #define  state_INC
 #include <Eigen/Dense>
+#include "body.h"
+#include "types.h"
+
+#define MAXFEATURES 30
 using Eigen::Matrix;
+using Eigen::Dynamic;
+using Eigen::Vector3d;
+using Eigen::Quaternion;
 /*
  * =====================================================================================
  *        Class:  State
@@ -12,18 +19,23 @@ class State
 {
     public:
         /* ====================  LIFECYCLE     ======================================= */
-        State (){};                             /* constructor */
+        State ();                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
 
         /* ====================  MUTATORS      ======================================= */
+        void accelerometer_bias(const Vector3d &b);
+        void enu(const UTM &utm);
+        void motionModel(const Vector3d &acc, const Vector3d &ang,
+                const Quaternion<double> &q, const double dt);
+        void position_covariance(const Matrix<double,3,3> &p);
+        void velocity_covariance(const Matrix<double,3,3> &p);
+        void vel(const Matrix<double,3,1> &v);
+        void Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt);
 
         /* ====================  OPERATORS     ======================================= */
-        void Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
-            const Matrix<double,9,9> &Q );
-        void update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, 
-                const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
-                const Matrix<double,1,1> &z, const Matrix<double,1,1> &R);
+        void update ( const Matrix<double,1,1> &z);
+        void unicsv();
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -34,6 +46,11 @@ class State
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
+        Body *body;
+        Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
+        char utm_c;
+        int utm_i;
+
 
 }; /* -----  end of class State  ----- */