Update body to use full state.

The changes add quaternion estimation to the body state. The quaternion is still
provided as an input, but it is ignored.

1 files changed, 24 insertions, 7 deletions

diff --git a/src/body.h b/src/body.h
index 7bbd50d..b973b00 100644
--- a/src/body.h
+++ b/src/body.h
@@ -9,6 +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 */
 
 
 using Eigen::Matrix;
@@ -25,31 +26,42 @@ class Body
 {
     public:
         /* ====================  LIFECYCLE     ======================================= */
-        Body (){};                             /* constructor */
+        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);
+#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);
+#endif
         Vector3d ned();
         Vector3d vel();
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
-        void asVector(const Matrix<double,9,1> &m);
         void clamp();
         void pos( const UTM &utm);
-        void dx( const Matrix<double,9,1> &del);
         void vel(const Matrix<double,3,1> &v);
 
         /* ====================  OPERATORS     ======================================= */
-        Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt);
-        Matrix<double,1,9> H();
         Matrix<double,1,1> h();
         void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                 const Quaterniond &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> &Pxx);
         Matrix<double,3,3> skewSymmetric(const Vector3d &x);
+        Matrix<double,4,4> omega(const Vector3d &x);
         void unicsv();
 
     protected:
@@ -61,9 +73,14 @@ class Body
         /* ====================  METHODS       ======================================= */
 
         /* ====================  DATA MEMBERS  ======================================= */
+#ifdef FULLSTATE
+        Matrix<double,13,1> X;
+#else
         Matrix<double,9,1> X;
+#endif
         char utm_c;
         int utm_i;
+        Vector3d gravity_world;
 
 }; /* -----  end of class Body  ----- */