From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 6 Oct 2019 05:02:53 +0000 (-0300)
Subject: Fix transition function Jacobian.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=22a8ddd15be86302bd7ed2057c91ef3d0a9e4740;p=imufusion.git

Fix transition function Jacobian.
---

diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index b5764d4..9889afe 100644
--- 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;
 }