Update P during imuCallback

1 files changed, 16 insertions, 2 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  ----- */
 
 /*