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.

1 files changed, 97 insertions, 11 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  ----- */
+