From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 29 Sep 2019 21:57:56 +0000 (-0300)
Subject: Change model to ENU convention.
X-Git-Tag: f1~1
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=95ce062312ac44d72d21fd5f5cd3431217374a9a;p=imufusion.git

Change model to ENU convention.
---

diff --git a/include/ekf.hpp b/include/ekf.hpp
index ff353a1..3a2bd69 100644
--- a/include/ekf.hpp
+++ b/include/ekf.hpp
@@ -44,8 +44,14 @@ class Ekf
 	Eigen::MatrixXd P_;		///< Covariance a priori
 	Eigen::MatrixXd Pw_;		///< Covariance of process noise
 	Eigen::MatrixXd Pv_;		///< Covariance of observation noise
+
+	/** @brief Quaternion transition function
+		\f${}^0\dot{Q}_g(t)=\frac{1}{2} \Xi\left({}^0Q_g(t)\right) {}^g\omega\f$
+		@return Quaternion transition matrix
+	*/	
+	Eigen::MatrixXd Xi(void);
 	
-	/** @brief Process fuction
+	/** @brief Process function
 		@param u Process input (gyro)
 		@return Predicted state
 	*/
diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index f3695f6..b5764d4 100644
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -84,17 +84,21 @@ Eigen::MatrixXd Ekf::covariance(void)
 	return P_;
 }
 
-
-Eigen::VectorXd Ekf::f(const Eigen::Vector3d u)
+Eigen::MatrixXd Ekf::Xi(void)
 {
 	Eigen::MatrixXd Xi(4,3);
 	Xi << 	-x_(1), -x_(2), -x_(3),
-		x_(0), -x_(3), -x_(2),
+		x_(0), -x_(3), x_(2),
 		x_(3), x_(0), -x_(1),
 		-x_(2), x_(1), x_(0);
-		
+	return Xi;
+}
+
+
+Eigen::VectorXd Ekf::f(const Eigen::Vector3d u)
+{
 	Eigen::MatrixXd G(7,3);
-	G << 	T_/2.0*Xi,
+	G << 	T_/2.0*Xi(),
 		Eigen::MatrixXd::Zero(3,3);
 
 	return x_+G*u;
@@ -102,12 +106,6 @@ Eigen::VectorXd Ekf::f(const Eigen::Vector3d u)
 
 Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
 {
-	Eigen::MatrixXd Xi(4,3);
-	Xi << 	-x_(1), -x_(2), -x_(3),
-		x_(0), -x_(3), -x_(2),
-		x_(3), x_(0), -x_(1),
-		-x_(2), x_(1), x_(0);
-	
 	Eigen::MatrixXd Omega(4,4);
 	Omega << 0.0, -u(0), -u(1), -u(2),
 		u(0), 0.0, u(2), -u(1),
@@ -115,7 +113,7 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
 		u(2), u(1), -u(0), 0.0;
 
 	Eigen::MatrixXd F(7,7);
-	F << 	T_/2.0*Omega, -T_/2.0*Xi,
+	F << 	T_/2.0*Omega, -T_/2.0*Xi(),
 		Eigen::MatrixXd::Zero(3,4), Eigen::MatrixXd::Zero(3,3);
 
 	return F;
@@ -124,24 +122,24 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
 Eigen::VectorXd Ekf::h(void)
 {
 	Eigen::VectorXd h(6);
-	h << 	2*Gz_*(x_(1)*x_(3)-x_(2)*x_(0)),
-		2*Gz_*(x_(2)*x_(3)+x_(1)*x_(0)),
-		Gz_*(x_(0)*x_(0)-x_(1)*x_(1)-x_(2)*x_(2)+x_(3)*x_(3)),
-		x_(0)*x_(0)+x_(1)*x_(1)-x_(2)*x_(2)-x_(3)*x_(3),
-		2*(x_(1)*x_(2)-x_(3)*x_(0)),
-		2*(x_(1)*x_(3)+x_(2)*x_(0));
+	h << 	-2*Gz_*(x_(1)*x_(3)-x_(2)*x_(0)),
+		-2*Gz_*(x_(2)*x_(3)+x_(1)*x_(0)),
+		-Gz_*(x_(0)*x_(0)-x_(1)*x_(1)-x_(2)*x_(2)+x_(3)*x_(3)),
+		2*(x_(1)*x_(2)+x_(3)*x_(0)),
+		x_(0)*x_(0)-x_(1)*x_(1)+x_(2)*x_(2)-x_(3)*x_(3),
+		2*(x_(2)*x_(3)-x_(1)*x_(0));
 	return h;
 }
 
 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);
+	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_(3), 2*x_(2), 2*x_(1), 2*x_(0), Eigen::MatrixXd::Zero(1,3),
+		2*x_(0), -2*x_(1), 2*x_(2), -2*x_(3), Eigen::MatrixXd::Zero(1,3),
+		-2*x_(1), -2*x_(0), 2*x_(3), 2*x_(2), Eigen::MatrixXd::Zero(1,3);
 	return H;
 }