From 2c0ce68acd031b67fdffaea3b2e50cca4f487633 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 4 Apr 2017 16:10:26 -0500
Subject: Full state mostly implemented.

STATESIZE 9 works just as it does for experiment04032017 tag. The
quaternion covariance is not being set correctly yet.
---
 src/main.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 55 insertions(+)

(limited to 'src/main.cpp')

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:	
-- 
cgit v1.1