Full state mostly implemented.

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

1 files changed, 8 insertions, 2 deletions

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  ----- */