Add support for INSCOVS.

The position and velocity covariances are rotated into the correct
frames: NED and FRD.

1 files changed, 67 insertions, 2 deletions

diff --git a/src/body.cpp b/src/body.cpp
index daf4e69..330c0a8 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -19,7 +19,64 @@
 #include <Eigen/Dense>
 #include "body.h"
 
-    void
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: H
+ * Description:  Writes the Jacobian of the measurement model into H.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::H ( Matrix<double,1,9> &H )
+{
+    H = Matrix<double,1,9>::Zero();
+    H[0,2] = 1;
+    return ;
+}		/* -----  end of method Body::H  ----- */
+
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: h
+ * Description:  Writes the predicted measurement vector into h.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
+{
+    h[0] = X[2];
+    return ;
+}		/* -----  end of method Body::h_hat  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: F
+ * Description:  Computes the Jacobian of the motion model of the body.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
+{
+    Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
+    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+    Matrix<double,3,3> W;
+    skewSymmetric(ang,W);
+    F.block(0,3,3,3) = Rbw;
+    F.block(3,3,3,3) = -W;
+    F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
+    return ;
+}		/* -----  end of method Body::F  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: motionModel
+ * Description:  Propagates the motion model of the body into vector X.
+ *--------------------------------------------------------------------------------------
+ */
+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));
@@ -41,10 +98,18 @@ Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &
     return ;
 }		/* -----  end of method Body::motionModel  ----- */
 
-    void
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: skewSymmetric
+ * Description:  Create the skew symmetric matrix y from the vector x.
+ *--------------------------------------------------------------------------------------
+ */
+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  ----- */
 
+