From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 16 Aug 2019 20:04:38 +0000 (-0300)
Subject: Fix bias in f() and F().
X-Git-Tag: f1~7
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=8a38a7c268b7e4ad9572827046640ac3e3b3bd98;p=imufusion.git

Fix bias in f() and F().
---

diff --git a/include/ekf.hpp b/include/ekf.hpp
index 5f6bbc0..88bfd1e 100644
--- a/include/ekf.hpp
+++ b/include/ekf.hpp
@@ -49,13 +49,13 @@ class Ekf
 		@param u Process input (gyro)
 		@return Predicted state
 	*/
-	Eigen::VectorXd f(Eigen::Vector3d u);
+	Eigen::VectorXd f(const Eigen::Vector3d u);
 	
 	/** @brief Jacobian of process fuction
 		@param u Process input (gyro)
 		@return Jacobian of process function
 	*/
-	Eigen::MatrixXd F(Eigen::Vector3d u);
+	Eigen::MatrixXd F(const Eigen::Vector3d u);
 
 	
 	/** @brief Observation fuction
diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index ab78532..e757879 100644
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -20,11 +20,11 @@
 *******************************************************************************/
 
 #include <ekf.hpp>
-
+#include <iostream>
 namespace imufusion
 {
 
-Ekf::Ekf(const Eigen::Quaterniond &q0,const Eigen::MatrixXd &P0,const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T): Gz_(9.81), T_(T), x_(7), P_(P0), Pw_(Pw),Pv_(Pv)
+Ekf::Ekf(const Eigen::Quaterniond &q0,const Eigen::MatrixXd &P0,const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T): Gz_(-9.81), T_(T), x_(7), P_(P0), Pw_(Pw),Pv_(Pv)
 {
 	Eigen::Vector3d gyroBias;
 	gyroBias.setZero();
@@ -56,11 +56,12 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig
 	// Covariance a posteriori
 	Eigen::MatrixXd P=(Eigen::MatrixXd::Identity(7,7)-K*H())*P_;
 
-	// State prediction
-	x_=f(gyro);
+	// Covariance prediction
+	Eigen::Vector3d u=gyro-x_.tail<3>();
+	P_=F(u)*P*F(u).transpose()+Pw_;
 
-	// Covariance propagation
-	P_=F(gyro)*P*F(gyro).transpose()+Pw_;
+	// State prediction
+	x_=f(u);	// Should be the last operation becuse changes x_
 }
 
 Eigen::Quaterniond Ekf::quaternion(void)
@@ -78,7 +79,7 @@ Eigen::EulerAnglesZYXd Ekf::euler(void)
 	return euler;
 }
 
-Eigen::VectorXd Ekf::f(Eigen::Vector3d u)
+Eigen::VectorXd Ekf::f(const Eigen::Vector3d u)
 {
 	Eigen::MatrixXd Xi(4,3);
 	Xi << 	-x_(1), -x_(2), -x_(3),
@@ -89,11 +90,11 @@ Eigen::VectorXd Ekf::f(Eigen::Vector3d u)
 	Eigen::MatrixXd G(7,3);
 	G << 	T_/2.0*Xi,
 		Eigen::MatrixXd::Zero(3,3);
-	
+
 	return x_+G*u;
 }
 
-Eigen::MatrixXd Ekf::F(Eigen::Vector3d u)
+Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
 {
 	Eigen::MatrixXd Xi(4,3);
 	Xi << 	-x_(1), -x_(2), -x_(3),
@@ -105,7 +106,7 @@ Eigen::MatrixXd Ekf::F(Eigen::Vector3d u)
 	Omega << 0.0, -u(0), -u(1), -u(2),
 		u(0), 0.0, u(2), -u(1),
 		u(1), -u(2), 0.0, u(0),
-		u(2), u(1), -u(0),0.0;
+		u(2), u(1), -u(0), 0.0;
 
 	Eigen::MatrixXd F(7,7);
 	F << 	T_/2.0*Omega, -T_/2.0*Xi,
@@ -131,10 +132,10 @@ Eigen::MatrixXd Ekf::H(void)
 	Eigen::MatrixXd H(6,7);
 	H << 	-2*Gz_*x_(2), 2*Gz_*x_(3), -2*Gz_*x_(0), 2*Gz_*x_(1), Eigen::MatrixXd::Zero(1,3),
 		2*Gz_*x_(1), 2*Gz_*x_(0), 2*Gz_*x_(3), 2*Gz_*x_(2), Eigen::MatrixXd::Zero(1,3),
-		2*Gz_*x_(0), -2*Gz_*x_(1), -2*Gz_*x_(2), 2*Gz_*x_(3),  Eigen::MatrixXd::Zero(1,3),
-		2*x_(0), -2*x_(1), -2*x_(2), -2*x_(3),  Eigen::MatrixXd::Zero(1,3),
-		-2*x_(3), 2*x_(2), 2*x_(1), -2*x_(0),  Eigen::MatrixXd::Zero(1,3),
-		2*x_(2), 2*x_(4), -2*x_(0), 2*x_(2),  Eigen::MatrixXd::Zero(1,3);
+		2*Gz_*x_(0), -2*Gz_*x_(1), -2*Gz_*x_(2), 2*Gz_*x_(3), Eigen::MatrixXd::Zero(1,3),
+		2*x_(0), -2*x_(1), -2*x_(2), -2*x_(3), Eigen::MatrixXd::Zero(1,3),
+		-2*x_(3), 2*x_(2), 2*x_(1), -2*x_(0), Eigen::MatrixXd::Zero(1,3),
+		2*x_(2), 2*x_(4), -2*x_(0), 2*x_(2), Eigen::MatrixXd::Zero(1,3);
 	return H;
 }
 
diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 41723cc..d89677b 100644
--- a/test/ekf_test.cpp
+++ b/test/ekf_test.cpp
@@ -73,16 +73,21 @@ int main(int argc,char *argv[])
 	
 	Eigen::Quaterniond q0;
 	q0.setIdentity();
+
 	Eigen::MatrixXd P0(7,7);
 	P0.setIdentity();
 	P0*=1e5;
+
 	Eigen::MatrixXd Pw(7,7);
 	Pw.setIdentity();
 	Pw*=1e-3*1e-3;
+
 	Eigen::MatrixXd Pv(6,6);
 	Pv.setIdentity();
 	Pv*=1e-3*1e-3;
-	const double T=0.02;
+
+	const double T=0.02;	// 0.01 is too fast for an UART at 115200 bps
+
 	imufusion::Ekf ekf(q0,P0,Pw,Pv,T);
 	
 	struct timeval tv0;