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, 17 insertions, 6 deletions

diff --git a/src/body.h b/src/body.h
index 04196d2..f201683 100644
--- a/src/body.h
+++ b/src/body.h
@@ -2,6 +2,8 @@
 #define  body_INC
 #include <Eigen/Dense>
 #include <iostream>
+#include "types.h"
+
 using Eigen::Matrix;
 using Eigen::Vector3d;
 using Eigen::Quaternion;
@@ -21,16 +23,22 @@ class Body
         /* ====================  ACCESSORS     ======================================= */
 
         /* ====================  MUTATORS      ======================================= */
+        void accelerometer_bias( const Vector3d &b);
+        void enu( const UTM &utm);
+        void update(const Matrix<double,9,1> &dx);
+        void vel(const Matrix<double,3,1> &v);
 
         /* ====================  OPERATORS     ======================================= */
-        void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, 
-                const Vector3d &ang, const Quaternion<double> &q, const double dt);
-        Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
-        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
-        void H(Matrix<double, 1,9> &H);
-        Matrix<double,1,1> h(const Matrix<double,9,1> &X);
+        Matrix<double,9,9> F(const Vector3d &ang, const Quaternion<double> &q);
+        Matrix<double,1,9> H();
+        Matrix<double,1,1> h();
+        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
+                const Quaternion<double> &q, const double dt);
         Matrix<double,9,9> Q(double dt);
         Matrix<double,1,1> R();
+        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
+        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
+        void unicsv();
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -41,6 +49,9 @@ class Body
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
+        Matrix<double,9,1> X;
+        char utm_c;
+        int utm_i;
 
 }; /* -----  end of class Body  ----- */