Full state mostly implemented.

STATESIZE 9 works just as it does for experiment04032017 tag. The
quaternion covariance is not being set correctly yet.

9 files changed, 107 insertions, 13 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 819bd01..f909a39 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -366,15 +366,24 @@ Body::asVector ( const Matrix<double,STATESIZE,1> &m )
  * Description:  Returns the current quaternion estimate.
  *--------------------------------------------------------------------------------------
  */
+#if STATESIZE==13
 Quaterniond
 Body::qhat ( )
 {
-#if STATESIZE==13
     Quaterniond qbw(X(9),X(6),X(7),X(8));
-#else
-    fprintf(stderr, "Quaternion is not being estimated, quitting.\n");
-    exit(1);
-#endif
     return qbw;
 }		/* -----  end of method Body::qhat  ----- */
 
+void
+Body::qhat ( const Quaterniond &q )
+{
+    X[6] = q.x();
+    X[7] = q.y();
+    X[8] = q.z();
+    X[9] = q.w();
+    return ;
+}		/* -----  end of method Body::qhat  ----- */
+
+#endif
+
+
diff --git a/src/body.h b/src/body.h
index 97df1e9..23ed2ca 100644
--- a/src/body.h
+++ b/src/body.h
@@ -9,7 +9,6 @@
 //#define DOCLAMP
 #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
 #define MINHEIGHT -0.3            /*  */
-#define STATESIZE 13
 
 
 using Eigen::Matrix;
@@ -32,6 +31,7 @@ class Body
 #if STATESIZE==13
         Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
         Quaterniond qhat();
+        void qhat(const Quaterniond &q);
 #else
         Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
 #endif
diff --git a/src/feature.cpp b/src/feature.cpp
index 9c6bf84..85396e3 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -350,7 +350,7 @@ Feature::Fx ( double dt )
     y2 = X[2];
     Matrix<double,3,STATESIZE> F;
     F = Matrix<double,3,STATESIZE>::Zero();
-    F.block<3,3>(0,3) << y0*y2,-y2, 
+    F.block<3,3>(0,3) << y0*y2,-y2, 0.,
                          y1*y2, 0.,-y2, 
                          y2*y2,  0,   0;
     F *= dt;
diff --git a/src/feature.h b/src/feature.h
index 53eef2d..82a48d3 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -17,7 +17,6 @@
 #define RANSAC_LI_THRESHOLD 4e-5            /*  */
 #define RANSAC_HI_THRESHOLD 5e-2            /*  */
 #define INITDEPTH
-#define STATESIZE 13            /*  */
 
 using Eigen::Dynamic;
 using Eigen::Matrix;
diff --git a/src/main.cpp b/src/main.cpp
index 914aa26..11bf6dd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -50,11 +50,18 @@ aboveWater(const Quaterniond &q)
  * =====================================================================================
  */
 void
+#if STATESIZE==13
+covCallback(const message &msg, State &mu)
+#else
 covCallback(const message &msg, State &mu, const Quaterniond &q)
+#endif
 {
     if (seencov && seenutm && seenpva) return;
     seencov=true;
     // Rotation from body to world
+#if STATESIZE==13
+    Quaterniond q = mu.qhat();
+#endif
     Matrix3d Rbw(q.toRotationMatrix());
     // Rotation from ENU to NED
     Matrix3d Renuned;
@@ -63,6 +70,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
     pcov(2,2) *= 10;
     mu.position_covariance(pcov);
     mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
+#if STATESIZE==13
+    mu.quaternion_covariance();
+#endif
 }
 
 
@@ -74,9 +84,16 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
  * =====================================================================================
  */
 void
+#if STATESIZE==13
+imgCallback(message &msg, State &mu, Camera &cam)
+#else
 imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
+#endif
 {
     if (seenutm && seenpva && seencov) {
+#if STATESIZE==13
+        Quaterniond q = mu.qhat();
+#endif
         std::vector<measurement_t> z;
 #ifdef MEASURE_HEIGHT
         measurement_t height;
@@ -112,7 +129,11 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
         if (fclose(fin)==EOF) {
             err_sys("fclose");
         }
+#if STATESIZE==13
+        mu.handle_measurements(z);
+#else
         mu.handle_measurements(z,q);
+#endif
     }
 
     return;
@@ -126,7 +147,11 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
  * =====================================================================================
  */
 void 
+#if STATESIZE==13
+imuCallback(const message &msg, State &mu, const timestamp dt)
+#else
 imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt)
+#endif
 {
     if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return;
     Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
@@ -136,7 +161,11 @@ imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp
     ang -= ang_bias;
     double dtf = dt.secs+dt.nsecs*1e-9;
 
+#if STATESIZE==13
+    mu.motionModel(acc,ang,dtf);
+#else
     mu.motionModel(acc,ang,q,dtf);
+#endif
 }
 
 /* 
@@ -166,6 +195,9 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
     if (!seenpva || !seencov || !seenutm) {
         Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
         mu.vel(R.transpose()*vw);
+#if STATESIZE==13
+        mu.qhat(q);
+#endif
         seenpva=true;
     } 
     return;
@@ -179,8 +211,15 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
  * =====================================================================================
  */
 void
+#if STATESIZE==13
+utmCallback(const message &msg, State &mu)
+#else
 utmCallback(const message &msg, State &mu, const Quaterniond &q)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = mu.qhat();
+#endif
     static int i=0;
     if ((!seenutm || !seencov) && seenpva) {
         seenutm=true;
@@ -368,13 +407,21 @@ main(int argc, char **argv)
         
         switch ( msg.msg_type ) {
             case BESTUTM:	
+#if STATESIZE==13
+                utmCallback(msg, mu);
+#else
                 utmCallback(msg, mu,qbw);
+#endif
                 printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                 mu.unicsv();
                 break;
 
             case IMG:	
+#if STATESIZE==13
+                imgCallback(msg, mu, cam);
+#else
                 imgCallback(msg, mu, cam, qbw);
+#endif
                 break;
 
             case INSPVAS:	
@@ -384,7 +431,11 @@ main(int argc, char **argv)
             case RAWIMUS:	
                 if (i++%DOWNSAMPLE==0) {
                     dt = update_dt(msg.stamp, &t_old);
+#if STATESIZE==13
+                    imuCallback(msg, mu, dt);
+#else
                     imuCallback(msg, mu, qbw, dt);
+#endif
                     if (seenutm && seencov && seenpva) {
 #ifdef  ROS_PUBLISH
                         geometry_msgs::PoseStamped msg;
@@ -397,7 +448,11 @@ main(int argc, char **argv)
                 break;
 
             case INSCOVS:
+#if STATESIZE==13
+                covCallback(msg, mu);
+#else
                 covCallback(msg, mu, qbw);
+#endif
                 break;
 
             default:	
diff --git a/src/main.h b/src/main.h
index 6e11875..0cbe4d0 100644
--- a/src/main.h
+++ b/src/main.h
@@ -40,7 +40,6 @@
 #define DOWNSAMPLE 1            /*  */
 #define HEIGHT_FROM_ATTITUDE    /* use the attitude to measure the height */
 #define MEASURE_HEIGHT
-
 #if  ROS_PUBLISH
 #include <ros/ros.h>
 #include <ros/geometry_msgs.h>
@@ -55,11 +54,18 @@ double aboveWater(const Quaterniond &q);
 int parseLine(char *line, message *msg);
 timestamp update_dt(const timestamp t, timestamp *t_old);
 
+#if STATESIZE==13
+void covCallback(const message &msg, State &mu);
+void imgCallback(message &msg, State &mu, Camera &cam);
+void imuCallback(const message &msg, State &mu, const timestamp dt);
+void utmCallback(const message &msg, State &mu);
+#else
 void covCallback(const message &msg, State &mu, const Quaterniond &q);
 void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q);
 void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt);
-void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q);
 void utmCallback(const message &msg, State &mu, const Quaterniond &q);
+#endif
+void pvaCallback(const message &msg, Matrix<double,STATESIZE,1> &X, Quaterniond &q);
 
 
 #endif   /* ----- #ifndef main_INC  ----- */
diff --git a/src/state.cpp b/src/state.cpp
index dfe41fa..e53ad29 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -464,7 +464,7 @@ void
 #if STATESIZE==13
 State::addFeatures ( std::vector<measurement_t> &F, double z)
 #else
-State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
+State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z)
 #endif
 {
 #if STATESIZE==13
@@ -1008,3 +1008,26 @@ State::asVector ( const Matrix<double,Dynamic,1> &m )
     return ;
 }		/* -----  end of method State::asVector  ----- */
 
+#if STATESIZE==13
+Quaterniond
+State::qhat ( )
+{
+    return body->qhat() ;
+}		/* -----  end of method State::qhat  ----- */
+
+void
+State::qhat ( const Quaterniond &q )
+{
+    body->qhat(q);
+    return ;
+}		/* -----  end of method State::qhat  ----- */
+
+
+void
+State::quaternion_covariance (  )
+{
+    P.block<4,4>(6,6) = 1e-5*Matrix<double,4,4>::Identity();
+    return ;
+}		/* -----  end of method State::quaternion_covariance  ----- */
+
+#endif
diff --git a/src/state.h b/src/state.h
index f330b67..b04ae9c 100644
--- a/src/state.h
+++ b/src/state.h
@@ -15,7 +15,6 @@
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define DORANSAC            /*  */
 //#define INLIERTEST            /*  */
-#define STATESIZE 13            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
 using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -48,6 +47,7 @@ class State
         MatrixXd F(const Vector3d &w, double dt);
         MatrixXd H ( const std::vector<measurement_t> &z );
         MatrixXd blockSI( const std::vector<measurement_t> &z );
+        Quaterniond qhat();
 #else
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
@@ -74,6 +74,8 @@ class State
                 const std::vector<measurement_t> &z);
         void Pkk1 ( const Vector3d &ang, const double dt);
         void ransacUpdate(std::vector<measurement_t> &z);
+        void quaternion_covariance();
+        void qhat(const Quaterniond &q);
 #else
         void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
         void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
diff --git a/src/types.h b/src/types.h
index c284d4a..3c1a962 100644
--- a/src/types.h
+++ b/src/types.h
@@ -4,7 +4,7 @@
 #include <Eigen/Dense>
 #define MAXLINE 8192
 #define MAXFILENAME 1024
-
+#define STATESIZE 9            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
 using Eigen::Matrix;
 using Eigen::Matrix3d;
 using Eigen::Quaterniond;