Update P during imuCallback

1 files changed, 14 insertions, 8 deletions

diff --git a/src/main.cpp b/src/main.cpp
index a30ca6c..838bab8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -23,6 +23,9 @@
 bool seenutm=false;
 bool seenpva=false;
 Eigen::Matrix<double,9,9> P;
+using std::cout;
+using std::endl;
+using std::cerr;
 
 #ifdef  USE_ROS
 
@@ -44,7 +47,9 @@ imgCallback(const message *msg)
 }
 
 void 
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
+imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, 
+        Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q,
+        const timestamp dt)
 {
     if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
     using Eigen::Vector3d;
@@ -53,9 +58,14 @@ imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quate
     // WARNING: This is the bias for 1112-1
     Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03);
     ang-=ang_bias;
-    Body mu;
+    Body body;
+    State mu;
     double dtf = dt.secs+dt.nsecs*1e-9;
-    mu.motionModel(X,acc,ang,q,dtf);
+
+    Eigen::Matrix<double,9,9> F;
+    F = body.F(X,ang,q);
+    mu.Pkk1(P,F,body.Q(dtf));
+    body.motionModel(X,acc,ang,q,dtf);
 }
 
 void
@@ -221,9 +231,6 @@ main(int argc, char **argv)
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
-using std::cout;
-using std::endl;
-using std::cerr;
 printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
 // Read sensors from file
 char line[MAXLINE];
@@ -251,12 +258,11 @@ while (scanf("%s", line)!=EOF) {
 
         case RAWIMUS:	
             dt = update_dt(msg.stamp, &t_old);
-            imuCallback(msg, bodyStateVector, qbw, dt);
+            imuCallback(msg, bodyStateVector, P, qbw, dt);
             break;
 
         case INSCOVS:
             covCallback(msg, P, qbw);
-            cout << P << endl;
 
         default:	
             break;