Fix Jacobian of motion model.

This fixes a bug that was present in the pyslam code. The Jacobian of the motion
model needs to be replaced with:
F = I + F*dt

1 files changed, 4 insertions, 2 deletions

diff --git a/src/body.cpp b/src/body.cpp
index f27a62f..e6bfb6c 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -105,7 +105,7 @@ Body::Q (double dt)
     Q.block<3,3>(0,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<3,3>(6,6) = dt*dt*5e-8*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(6,6) = dt*dt*5e-3*Matrix<double,3,3>::Identity();
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
@@ -149,7 +149,7 @@ Body::h ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,9,9>
-Body::F ( const Vector3d &ang, const Quaterniond &q )
+Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
 {
     Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
     Matrix<double,3,3> Rbw(q.toRotationMatrix());
@@ -158,6 +158,8 @@ Body::F ( const Vector3d &ang, const Quaterniond &q )
     F.block<3,3>(0,3) = Rbw;
     F.block<3,3>(3,3) = -W;
     F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
+    F *= dt;
+    F += Matrix<double,9,9>::Identity();
     return F;
 }		/* -----  end of method Body::F  ----- */