From 623f94f85a49f1b720a025c680769ba2ba028ba0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 4 Apr 2017 13:59:03 -0500
Subject: Update Body to use STATESIZE

---
 src/body.cpp | 141 ++++++++++++++++++++++++++---------------------------------
 1 file changed, 61 insertions(+), 80 deletions(-)

(limited to 'src/body.cpp')

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  ----- */
 
-- 
cgit v1.1