Update P during imuCallback

4 files changed, 40 insertions, 14 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 330c0a8..4741911 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -19,6 +19,20 @@
 #include <Eigen/Dense>
 #include "body.h"
 
+Matrix<double,9,9>
+Body::Q (double dt)
+{
+    Matrix<double,9,9> Q;
+    Q = Matrix<double,9,9>::Zero();
+    Q.block(0,0,3,3) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block(3,0,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block(0,3,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block(3,3,3,3) = dt*dt*Matrix<double,3,3>::Identity();
+    Q *= 800e-6;
+    Q.block(6,6,3,3) = dt*dt*5e-8*Matrix<double,3,3>::Identity();
+    return Q;
+}		/* -----  end of method Body::q  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
@@ -56,7 +70,7 @@ Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
  * Description:  Computes the Jacobian of the motion model of the body.
  *--------------------------------------------------------------------------------------
  */
-void
+Matrix<double,9,9>
 Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
 {
     Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
@@ -66,7 +80,7 @@ Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<dou
     F.block(0,3,3,3) = Rbw;
     F.block(3,3,3,3) = -W;
     F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
-    return ;
+    return F;
 }		/* -----  end of method Body::F  ----- */
 
 /*
diff --git a/src/body.h b/src/body.h
index 1fc88ff..b5b2aae 100644
--- a/src/body.h
+++ b/src/body.h
@@ -25,10 +25,11 @@ class Body
         /* ====================  OPERATORS     ======================================= */
         void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, 
                 const Vector3d &ang, const Quaternion<double> &q, const double dt);
-        void F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
+        Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
         void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
         void H(Matrix<double, 1,9> &H);
         void h(const Matrix<double,9,1> &X, Matrix<double,1,1> &h);
+        Matrix<double,9,9> Q(double dt);
 
     protected:
         /* ====================  METHODS       ======================================= */
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;
diff --git a/src/main.h b/src/main.h
index aa97995..d1932c8 100644
--- a/src/main.h
+++ b/src/main.h
@@ -13,6 +13,7 @@
 #include <iostream>
 
 #include "body.h"
+#include "state.h"
 
 #define MAXLINE 8192
 #define MAXFILENAME 1024
@@ -83,10 +84,14 @@ timestamp update_dt(const timestamp t, timestamp *t_old);
 #ifdef  USE_ROS
 void imuCallback();
 #else      /* -----  not USE_ROS  ----- */
-void covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q);
+void covCallback(const message &msg, Eigen::Matrix<double,9,9> &P,
+        const Eigen::Quaternion<double> &q);
 void imgCallback(const message *msg);
-void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt);
-void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);
+void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
+        Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q,
+        const timestamp dt);
+void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
+        Eigen::Quaternion<double> &q);
 void utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X);
 #endif     /* -----  not USE_ROS  ----- */