Fix transition function Jacobian.

diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index b5764d4..9889afe 100644 (file)
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -115,6 +115,7 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
        Eigen::MatrixXd F(7,7);
        F <<    T_/2.0*Omega, -T_/2.0*Xi(),
                Eigen::MatrixXd::Zero(3,4), Eigen::MatrixXd::Zero(3,3);
+       F+=Eigen::MatrixXd::Identity(7,7);
 
        return F;
 }