Update Body to use STATESIZE

3 files changed, 81 insertions, 105 deletions

diff --git a/src/body.cpp b/src/body.cpp
index f7440b5..819bd01 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -38,11 +38,7 @@ Body::clamp()
  *--------------------------------------------------------------------------------------
  */
 void
-#ifdef FULLSTATE
-Body::dx ( const Matrix<double,13,1> &del )
-#else
-Body::dx ( const Matrix<double,9,1> &del )
-#endif
+Body::dx ( const Matrix<double,STATESIZE,1> &del )
 {
     X += del;
     clamp();
@@ -65,11 +61,7 @@ Body::vel ( const Matrix<double,3,1> &v )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,1,1>
-#ifdef FULLSTATE
-Body::S ( const Matrix<double,13,13> &Pxx )
-#else
-Body::S ( const Matrix<double,9,9> &Pxx )
-#endif
+Body::S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx )
 {
     Matrix<double,1,1> S;
     S << Pxx(2,2);
@@ -117,26 +109,17 @@ Body::R()
  * Description:  Returns Q matrix of process noise for the body.
  *--------------------------------------------------------------------------------------
  */
-#ifdef FULLSTATE
-Matrix<double,13,13>
-#else
-Matrix<double,9,9>
-#endif
+Matrix<double,STATESIZE,STATESIZE>
 Body::Q (double dt)
 {
-#ifdef FULLSTATE
-    Matrix<double,13,13> Q;
-    Q = Matrix<double,13,13>::Zero();
-#else
-    Matrix<double,9,9> Q;
-    Q = Matrix<double,9,9>::Zero();
-#endif
+    Matrix<double,STATESIZE,STATESIZE> Q;
+    Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
     Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
     Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
     Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
     Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
     Q *= IMU_NOISE;
-#ifdef FULLSTATE
+#if STATESIZE==13
     Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
     Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
 #else
@@ -152,20 +135,11 @@ Body::Q (double dt)
  * Description:  Returns the Jacobian of the measurement model, H.
  *--------------------------------------------------------------------------------------
  */
-#ifdef FULLSTATE
-Matrix<double,1,13>
-#else
-Matrix<double,1,9>
-#endif
+Matrix<double,1,STATESIZE>
 Body::H ( )
 {
-#ifdef FULLSTATE
-    Matrix<double,1,13> H;
-    H = Matrix<double,1,13>::Zero();
-#else
-    Matrix<double,1,9> H;
-    H = Matrix<double,1,9>::Zero();
-#endif
+    Matrix<double,1,STATESIZE> H;
+    H = Matrix<double,1,STATESIZE>::Zero();
     H(2) = 1.;
     return H ;
 }		/* -----  end of method Body::H  ----- */
@@ -193,32 +167,29 @@ Body::h ( )
  * Description:  Returns the Jacobian of the motion model of the body.
  *--------------------------------------------------------------------------------------
  */
-#ifdef FULLSTATE
-Matrix<double,13,13>
+Matrix<double,STATESIZE,STATESIZE>
+#if STATESIZE==13
+Body::F ( const Vector3d &ang, double dt )
 #else
-Matrix<double,9,9>
-#endif
 Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
+#endif
 {
-#ifdef FULLSTATE
-    Matrix<double,13,13> F = Matrix<double,13,13>::Zero();
-    double qbw1,qbw2,qbw3,qbw4;
-    Quaterniond qbw = qhat();
-    qbw1 = qbw.x();
-    qbw2 = qbw.y();
-    qbw3 = qbw.z();
-    qbw4 = qbw.w();
-    Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
-#else
-    Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
-    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+    Matrix<double,STATESIZE,STATESIZE> F = Matrix<double,STATESIZE,STATESIZE>::Zero();
+#if STATESIZE==13
+    Quaterniond q = qhat();
 #endif
+    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+
     Matrix<double,3,3> W;
     W = skewSymmetric(ang);
     F.block<3,3>(0,3) = Rbw;
     F.block<3,3>(3,3) = -W;
-    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
-#ifdef FULLSTATE
+#if STATESIZE==13
+    double qbw1,qbw2,qbw3,qbw4;
+    qbw1 = q.x();
+    qbw2 = q.y();
+    qbw3 = q.z();
+    qbw4 = q.w();
     /* Jacobian generated using symbolic matlab */
     double v1,v2,v3;
     v1 = X(3);
@@ -240,12 +211,12 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
                          -2*gravity_world*qbw4, -2*gravity_world*qbw3, -2*gravity_world*qbw2, -2*gravity_world*qbw1,
                           2*gravity_world*qbw1,  2*gravity_world*qbw2, -2*gravity_world*qbw3, -2*gravity_world*qbw4;
     F.block<4,4>(6,6) =  omega(ang);
-    F *= dt;
-    F += Matrix<double,13,13>::Identity();
+    F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
 #else
-    F *= dt;
-    F += Matrix<double,9,9>::Identity();
+    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
 #endif
+    F *= dt;
+    F += Matrix<double,STATESIZE,STATESIZE>::Identity();
     return F;
 }		/* -----  end of method Body::F  ----- */
 
@@ -276,26 +247,23 @@ Body::omega ( const Vector3d &x )
  *--------------------------------------------------------------------------------------
  */
 void
-Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
-{
-#ifdef FULLSTATE
-    Vector3d bias(X.segment<3>(10));
+#if STATESIZE==13
+Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt)
 #else
-    Vector3d bias(X.segment<3>(6));
+Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
 #endif
-    Vector3d gravity_world(0.,0.,9.80655);
+{
+    Vector3d bias = accelerometer_bias();
 
     Matrix<double,6,3> A;
     Matrix<double,6,1> b;
     A = Matrix<double,6,3>::Zero();
     b = Matrix<double,6,1>::Zero();
 
-#ifdef FULLSTATE
-    Quaterniond qbw qhat();
-    Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
-#else
-    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+#if STATESIZE==13
+    Quaterniond q = qhat();
 #endif
+    Matrix<double,3,3> Rbw(q.toRotationMatrix());
     A.block<3,3>(0,0) = Rbw;
     Matrix<double,3,3> W;
     W = skewSymmetric(ang);
@@ -303,7 +271,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
 
     X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
-#ifdef FULLSTATE
+#if STATESIZE==13
     X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
 #endif
     clamp();
@@ -335,13 +303,29 @@ Body::unicsv ( )
     return ;
 }		/* -----  end of method Body::unicsv  ----- */
 
-    void
+void
 Body::accelerometer_bias ( const Vector3d &b )
 {
+#if STATESIZE==13
+    X.segment<3>(10) = b;
+#else
     X.segment<3>(6) = b;
+#endif
     return ;
 }		/* -----  end of method State::accelerometer_bias  ----- */
 
+Vector3d
+Body::accelerometer_bias (  )
+{
+    Vector3d bs;
+#if STATESIZE==13
+    bs = X.segment<3>(10);
+#else
+    bs = X.segment<3>(6);
+#endif
+    return bs ;
+}		/* -----  end of method State::accelerometer_bias  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
@@ -361,22 +345,14 @@ Body::vel (  )
     return X.segment<3>(3);
 }		/* -----  end of method Body::vel  ----- */
 
-#ifdef FULLSTATE
-Matrix<double,13,1>
-#else
-Matrix<double,9,1>
-#endif
+Matrix<double,STATESIZE,1>
 Body::asVector (  )
 {
     return X;
 }		/* -----  end of method Body::asVector  ----- */
 
 void
-#ifdef FULLSTATE
-Body::asVector ( const Matrix<double,13,1> &m )
-#else
-Body::asVector ( const Matrix<double,9,1> &m )
-#endif
+Body::asVector ( const Matrix<double,STATESIZE,1> &m )
 {
     X = m;
     return ;
@@ -393,7 +369,12 @@ Body::asVector ( const Matrix<double,9,1> &m )
 Quaterniond
 Body::qhat ( )
 {
+#if STATESIZE==13
     Quaterniond qbw(X(9),X(6),X(7),X(8));
+#else
+    fprintf(stderr, "Quaternion is not being estimated, quitting.\n");
+    exit(1);
+#endif
     return qbw;
 }		/* -----  end of method Body::qhat  ----- */
 
diff --git a/src/body.h b/src/body.h
index 5544bba..97df1e9 100644
--- a/src/body.h
+++ b/src/body.h
@@ -9,7 +9,7 @@
 //#define DOCLAMP
 #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
 #define MINHEIGHT -0.3            /*  */
-#define FULLSTATE /* Uncomment to estimate quaternion */
+#define STATESIZE 13
 
 
 using Eigen::Matrix;
@@ -29,27 +29,21 @@ class Body
         Body (){ gravity_world << 0.,0.,9.80655; };                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
-#ifdef FULLSTATE
-        Matrix<double,13,1> asVector();
-        void asVector(const Matrix<double,13,1> &m);
-        void dx( const Matrix<double,13,1> &del);
-        Matrix<double,13,13> F(const Vector3d &ang, const Quaterniond &q, double dt);
-        Matrix<double,1,13> H();
-        Matrix<double,13,13> Q(double dt);
-        Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx);
+#if STATESIZE==13
+        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
         Quaterniond qhat();
 #else
-        Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt);
-        Matrix<double,9,1> asVector();
-        void asVector(const Matrix<double,9,1> &m);
-        void dx( const Matrix<double,9,1> &del);
-        Matrix<double,1,9> H();
-        Matrix<double,9,9> Q(double dt);
-        Matrix<double,1,1> S(const Matrix<double,9,9> &Pxx);
+        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
 #endif
+        Matrix<double,STATESIZE,1> asVector();
+        void asVector(const Matrix<double,STATESIZE,1> &m);
+        void dx( const Matrix<double,STATESIZE,1> &del);
+        Matrix<double,1,STATESIZE> H();
+        Matrix<double,STATESIZE,STATESIZE> Q(double dt);
+        Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx);
         Vector3d ned();
         Vector3d vel();
-
+        Vector3d accelerometer_bias( );
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
         void clamp();
@@ -58,8 +52,12 @@ class Body
 
         /* ====================  OPERATORS     ======================================= */
         Matrix<double,1,1> h();
+#if STATESIZE==13
+        void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt);
+#else
         void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                 const Quaterniond &q, const double dt);
+#endif
         Matrix<double,1,1> R();
         Matrix<double,3,3> skewSymmetric(const Vector3d &x);
         Matrix<double,4,4> omega(const Vector3d &x);
@@ -74,11 +72,7 @@ class Body
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
-#ifdef FULLSTATE
-        Matrix<double,13,1> X;
-#else
-        Matrix<double,9,1> X;
-#endif
+        Matrix<double,STATESIZE,1> X;
         char utm_c;
         int utm_i;
         Vector3d gravity_world;
diff --git a/src/state.cpp b/src/state.cpp
index 5253425..dfe41fa 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -553,9 +553,6 @@ State::F ( const Vector3d &w, double dt )
 State::F ( const Quaterniond &q, const Vector3d &w, double dt )
 #endif
 {
-#if STATESIZE==13
-    Quaterniond q = body->qhat();
-#endif
     Vector3d v;
     v = body->vel();
     // Allocate matrix F
@@ -564,7 +561,11 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt )
     f = MatrixXd::Zero(rows,rows);
 
     // Set body F
+#if STATESIZE==13
+    f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,dt);
+#else
     f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,q,dt);
+#endif
     // Set Fxi Fyi
     { // limit i's scope
         auto i = features.begin();