Update Body to use STATESIZE

1 files changed, 16 insertions, 22 deletions

diff --git a/src/body.h b/src/body.h
index 5544bba..97df1e9 100644
--- a/src/body.h
+++ b/src/body.h
@@ -9,7 +9,7 @@
 //#define DOCLAMP
 #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
 #define MINHEIGHT -0.3            /*  */
-#define FULLSTATE /* Uncomment to estimate quaternion */
+#define STATESIZE 13
 
 
 using Eigen::Matrix;
@@ -29,27 +29,21 @@ class Body
         Body (){ gravity_world << 0.,0.,9.80655; };                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
-#ifdef FULLSTATE
-        Matrix<double,13,1> asVector();
-        void asVector(const Matrix<double,13,1> &m);
-        void dx( const Matrix<double,13,1> &del);
-        Matrix<double,13,13> F(const Vector3d &ang, const Quaterniond &q, double dt);
-        Matrix<double,1,13> H();
-        Matrix<double,13,13> Q(double dt);
-        Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx);
+#if STATESIZE==13
+        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
         Quaterniond qhat();
 #else
-        Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt);
-        Matrix<double,9,1> asVector();
-        void asVector(const Matrix<double,9,1> &m);
-        void dx( const Matrix<double,9,1> &del);
-        Matrix<double,1,9> H();
-        Matrix<double,9,9> Q(double dt);
-        Matrix<double,1,1> S(const Matrix<double,9,9> &Pxx);
+        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
 #endif
+        Matrix<double,STATESIZE,1> asVector();
+        void asVector(const Matrix<double,STATESIZE,1> &m);
+        void dx( const Matrix<double,STATESIZE,1> &del);
+        Matrix<double,1,STATESIZE> H();
+        Matrix<double,STATESIZE,STATESIZE> Q(double dt);
+        Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx);
         Vector3d ned();
         Vector3d vel();
-
+        Vector3d accelerometer_bias( );
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
         void clamp();
@@ -58,8 +52,12 @@ class Body
 
         /* ====================  OPERATORS     ======================================= */
         Matrix<double,1,1> h();
+#if STATESIZE==13
+        void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt);
+#else
         void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                 const Quaterniond &q, const double dt);
+#endif
         Matrix<double,1,1> R();
         Matrix<double,3,3> skewSymmetric(const Vector3d &x);
         Matrix<double,4,4> omega(const Vector3d &x);
@@ -74,11 +72,7 @@ class Body
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
-#ifdef FULLSTATE
-        Matrix<double,13,1> X;
-#else
-        Matrix<double,9,1> X;
-#endif
+        Matrix<double,STATESIZE,1> X;
         char utm_c;
         int utm_i;
         Vector3d gravity_world;