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

diff --git a/src/state.h b/src/state.h
index 743f5d8..1efb0c2 100644
--- a/src/state.h
+++ b/src/state.h
@@ -1,7 +1,14 @@
 #ifndef  state_INC
 #define  state_INC
 #include <Eigen/Dense>
+#include "body.h"
+#include "types.h"
+
+#define MAXFEATURES 30
 using Eigen::Matrix;
+using Eigen::Dynamic;
+using Eigen::Vector3d;
+using Eigen::Quaternion;
 /*
  * =====================================================================================
  *        Class:  State
@@ -12,18 +19,23 @@ class State
 {
     public:
         /* ====================  LIFECYCLE     ======================================= */
-        State (){};                             /* constructor */
+        State ();                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
 
         /* ====================  MUTATORS      ======================================= */
+        void accelerometer_bias(const Vector3d &b);
+        void enu(const UTM &utm);
+        void motionModel(const Vector3d &acc, const Vector3d &ang,
+                const Quaternion<double> &q, const double dt);
+        void position_covariance(const Matrix<double,3,3> &p);
+        void velocity_covariance(const Matrix<double,3,3> &p);
+        void vel(const Matrix<double,3,1> &v);
+        void Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt);
 
         /* ====================  OPERATORS     ======================================= */
-        void Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
-            const Matrix<double,9,9> &Q );
-        void update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, 
-                const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
-                const Matrix<double,1,1> &z, const Matrix<double,1,1> &R);
+        void update ( const Matrix<double,1,1> &z);
+        void unicsv();
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -34,6 +46,11 @@ class State
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
+        Body *body;
+        Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
+        char utm_c;
+        int utm_i;
+
 
 }; /* -----  end of class State  ----- */