From 0f25fa35e063d3df3c652153b2d6f1b9f94ce82e Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 20 Mar 2017 11:57:09 -0500
Subject: 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
---
 src/state.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/state.cpp')

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());
-- 
cgit v1.1