Update body to use full state.

The changes add quaternion estimation to the body state. The quaternion is still
provided as an input, but it is ignored.

1 files changed, 112 insertions, 1 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 1b07361..99ea1ed 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -38,7 +38,11 @@ Body::clamp()
  *--------------------------------------------------------------------------------------
  */
 void
+#ifdef FULLSTATE
+Body::dx ( const Matrix<double,13,1> &del )
+#else
 Body::dx ( const Matrix<double,9,1> &del )
+#endif
 {
     X += del;
     clamp();
@@ -61,7 +65,11 @@ 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
 {
     Matrix<double,1,1> S;
     S << Pxx(2,2);
@@ -109,17 +117,31 @@ Body::R()
  * Description:  Returns Q matrix of process noise for the body.
  *--------------------------------------------------------------------------------------
  */
+#ifdef FULLSTATE
+Matrix<double,13,13>
+#else
 Matrix<double,9,9>
+#endif
 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
     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
+    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
     Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
+#endif
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
@@ -130,11 +152,20 @@ 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
 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
     H(2) = 1.;
     return H ;
 }		/* -----  end of method Body::H  ----- */
@@ -162,24 +193,84 @@ Body::h ( )
  * Description:  Returns the Jacobian of the motion model of the body.
  *--------------------------------------------------------------------------------------
  */
+#ifdef FULLSTATE
+Matrix<double,13,13>
+#else
 Matrix<double,9,9>
+#endif
 Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
 {
+#ifdef FULLSTATE
+    Matrix<double,13,13> F = Matrix<double,13,13>::Zero();
+    double qbw1,qbw2,qbw3,qbw4;
+    qbw1 = X(6);
+    qbw2 = X(7);
+    qbw3 = X(8);
+    qbw4 = X(9);
+    Quaterniond qbw(qbw4,qbw1,qbw2,qbw3);
+    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());
+#endif
     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
+    /* Jacobian generated using symbolic matlab */
+    double v1,v2,v3;
+    v1 = X(3);
+    v2 = X(4);
+    v3 = X(5);
+    F.block<1,4>(0,6) << 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, 
+                         2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3, 
+                         2*qbw1*v3 - 2*qbw3*v1 - 2*qbw4*v2, 
+                         2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1;
+    F.block<1,4>(1,6) << 2*qbw2*v1 - 2*qbw1*v2 - 2*qbw4*v3,
+                         2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3,
+                         2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1,
+                         2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2;
+    F.block<1,4>(2,6) << 2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2,
+                         2*qbw3*v2 - 2*qbw2*v3 - 2*qbw4*v1,
+                         2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3,
+                         2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3;
+    F.block<3,4>(3,6) << -2*gravity_world*qbw3,  2*gravity_world*qbw4, -2*gravity_world*qbw1,  2*gravity_world*qbw2,
+                         -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();
+#else
     F *= dt;
     F += Matrix<double,9,9>::Identity();
+#endif
     return F;
 }		/* -----  end of method Body::F  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
+ *      Method:  Body :: omega
+ * Description:  Returns the Omega(w) used for the motion model of the
+ * quaternion.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,4,4>
+Body::omega ( const Vector3d &x )
+{
+    Matrix<double,4,4> Omega;
+    Omega.block<3,3>(0,0) = -skewSymmetric(x);
+    Omega.block<3,1>(0,3) = x;
+    Omega.block<1,3>(3,0) = x.transpose();
+    Omega(3,3) = 0;
+    return Omega;
+}		/* -----  end of method Body::omega  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
  *      Method:  Body :: motionModel
  * Description:  Propagates the motion model of the body into vector X.
  *--------------------------------------------------------------------------------------
@@ -187,7 +278,11 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
 void
 Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
 {
-    Vector3d bias(X.segment(6,3));
+#ifdef FULLSTATE
+    Vector3d bias(X.segment<3>(10));
+#else
+    Vector3d bias(X.segment<3>(6));
+#endif
     Vector3d gravity_world(0.,0.,9.80655);
 
     Matrix<double,6,3> A;
@@ -195,7 +290,12 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     A = Matrix<double,6,3>::Zero();
     b = Matrix<double,6,1>::Zero();
 
+#ifdef FULLSTATE
+    Quaterniond qbw(X(9),X(6),X(7),X(8));
+    Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
+#else
     Matrix<double,3,3> Rbw(q.toRotationMatrix());
+#endif
     A.block<3,3>(0,0) = Rbw;
     Matrix<double,3,3> W;
     W = skewSymmetric(ang);
@@ -203,6 +303,9 @@ 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
+    X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
+#endif
     clamp();
     return ;
 }		/* -----  end of method Body::motionModel  ----- */
@@ -258,14 +361,22 @@ Body::vel (  )
     return X.segment<3>(3);
 }		/* -----  end of method Body::vel  ----- */
 
+#ifdef FULLSTATE
+Matrix<double,13,1>
+#else
 Matrix<double,9,1>
+#endif
 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
 {
     X = m;
     return ;