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, 1 insertions, 1 deletions

diff --git a/src/state.cpp b/src/state.cpp
index f12ec77..84aa565 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -71,7 +71,7 @@ State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 {
     Matrix<double,9,9> F,Q;
     Q = body->Q(dt);
-    F = body->F(ang,q);
+    F = body->F(ang,q,dt);
     P = F*P*F.transpose()+Q;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());