From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 16 Aug 2019 16:28:56 +0000 (-0300)
Subject: Fix Jacobians size.
X-Git-Tag: f1~8
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=08cef48d674877c5a53d0e9e744de94d8f2b838d;p=imufusion.git

Fix Jacobians size.
---

diff --git a/include/ekf.hpp b/include/ekf.hpp
index 799e08b..5f6bbc0 100644
--- a/include/ekf.hpp
+++ b/include/ekf.hpp
@@ -28,6 +28,7 @@
 #define __IMUFUSION_EKF_H__
 
 #include <Eigen/Dense>
+#include <unsupported/Eigen/EulerAngles>
 
 namespace imufusion
 {
@@ -93,7 +94,11 @@ class Ekf
 		@return Unitary quaternion with orientation
 	*/
 	Eigen::Quaterniond quaternion(void);
-	
+
+	/** @brief Get euler anles
+		@return Euler angles
+	*/
+	Eigen::EulerAnglesZYXd euler(void);
 };
 
 }
diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index 236eb8c..ab78532 100644
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -28,7 +28,11 @@ Ekf::Ekf(const Eigen::Quaterniond &q0,const Eigen::MatrixXd &P0,const Eigen::Mat
 {
 	Eigen::Vector3d gyroBias;
 	gyroBias.setZero();
-	x_ << q0.w(), q0.x(),q0.y(),q0.z(), gyroBias;
+	x_ << 	q0.w(),
+		q0.x(),
+		q0.y(),
+		q0.z(),
+		gyroBias;
 }
 
 Ekf::~Ekf(void)
@@ -38,9 +42,10 @@ Ekf::~Ekf(void)
 
 void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eigen::Vector3d &gyro)
 {
-	// Assemble observation
-	Eigen::VectorXd y;
-	y << acc, mag;
+	// Observation
+	Eigen::VectorXd y(acc.rows()+mag.rows());
+	y << 	acc,
+		mag;
 
 	// Gain
 	Eigen::MatrixXd K=P_*H().transpose()*(H()*P_*H().transpose()+Pv_).inverse();
@@ -65,6 +70,14 @@ Eigen::Quaterniond Ekf::quaternion(void)
 	return q;
 }
 
+Eigen::EulerAnglesZYXd Ekf::euler(void)
+{
+	Eigen::Quaterniond q=quaternion();
+	Eigen::Vector3d eulerVector=q.toRotationMatrix().eulerAngles(0,1,2);
+	Eigen::EulerAnglesZYXd euler(eulerVector(0),eulerVector(1),eulerVector(2));
+	return euler;
+}
+
 Eigen::VectorXd Ekf::f(Eigen::Vector3d u)
 {
 	Eigen::MatrixXd Xi(4,3);
@@ -74,7 +87,8 @@ Eigen::VectorXd Ekf::f(Eigen::Vector3d u)
 		-x_(2), x_(1), x_(0);
 		
 	Eigen::MatrixXd G(7,3);
-	G << T_/2.0*Xi, Eigen::MatrixXd::Zero(3,3);
+	G << 	T_/2.0*Xi,
+		Eigen::MatrixXd::Zero(3,3);
 	
 	return x_+G*u;
 }
@@ -87,14 +101,14 @@ Eigen::MatrixXd Ekf::F(Eigen::Vector3d u)
 		x_(3), x_(0), -x_(1),
 		-x_(2), x_(1), x_(0);
 	
-	Eigen::MatrixXd Omega(4,3);
+	Eigen::MatrixXd Omega(4,4);
 	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;
 
 	Eigen::MatrixXd F(7,7);
-	F << 	T_/2*Omega, -T_/2*Xi,
+	F << 	T_/2.0*Omega, -T_/2.0*Xi,
 		Eigen::MatrixXd::Zero(3,4), Eigen::MatrixXd::Zero(3,3);
 
 	return F;
@@ -114,13 +128,13 @@ Eigen::VectorXd Ekf::h(void)
 
 Eigen::MatrixXd Ekf::H(void)
 {
-	Eigen::MatrixXd H(6,4);
-	H << 	-2*Gz_*x_(2), 2*Gz_*x_(3), -2*Gz_*x_(0), 2*Gz_*x_(1),
-		2*Gz_*x_(1), 2*Gz_*x_(0), 2*Gz_*x_(3), 2*Gz_*x_(2),
-		2*Gz_*x_(0), -2*Gz_*x_(1), -2*Gz_*x_(2), 2*Gz_*x_(3),
-		2*x_(0), -2*x_(1), -2*x_(2), -2*x_(3),
-		-2*x_(3), 2*x_(2), 2*x_(1), -2*x_(0),
-		2*x_(2), 2*x_(4), -2*x_(0), 2*x_(2);
+	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);
 	return H;
 }
 
diff --git a/test/Makefile b/test/Makefile
index e53b627..45190cd 100644
--- a/test/Makefile
+++ b/test/Makefile
@@ -14,7 +14,7 @@ LDFLAGS=$(LIBDIR) $(LIBS)
 
 all: $(TARGET)
 
-$(TARGET): $(SRCS:.cpp=.o)
+$(TARGET): $(SRCS:.cpp=.o) ../lib/libimufusion.a
 	$(CXX) -o $@ $^ $(LDFLAGS)
 
 %.o: %.cpp
diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 0be6e05..41723cc 100644
--- a/test/ekf_test.cpp
+++ b/test/ekf_test.cpp
@@ -82,26 +82,27 @@ int main(int argc,char *argv[])
 	Eigen::MatrixXd Pv(6,6);
 	Pv.setIdentity();
 	Pv*=1e-3*1e-3;
-	const double T=0.01;
+	const double T=0.02;
 	imufusion::Ekf ekf(q0,P0,Pw,Pv,T);
 	
 	struct timeval tv0;
 	gettimeofday(&tv0,NULL);
-	double t1=0.0;
+	double tLast=0.0;
 	
 	for(;;)
 	{
 		struct timeval tv;
 		double t;
 		int s=0;
-		if(t1 > 0.0) do
+		if(tLast >= 0.0) do
 		{
 			if(s > 0) usleep(s);
 			gettimeofday(&tv,NULL);
 			t=(tv.tv_sec-tv0.tv_sec)*1e6+tv.tv_usec-tv0.tv_usec;
-			s=(int)(T*1e6-(t-t1)-120);
+			s=(int)(T*1e6-(t-tLast)-120);
 		}
-		while(t < t1+T*1e6);
+		while(t < tLast+T*1e6);
+		tLast=t;
 
 		try
 		{
@@ -114,7 +115,8 @@ int main(int argc,char *argv[])
 			
 			for(int i=0;i < 8;i++) std::cout << "----------";
 			std::cout << std::endl;
-			std::cout << "Acceleration=" << accel.transpose() << " m/s^2" << std::endl
+			std::cout << "t=" << t/1e6 << std::endl
+				<< "Acceleration=" << accel.transpose() << " m/s^2" << std::endl
 				<< "Magnetometer=" << mag.transpose() << " uT" << std::endl
 				<< "Gyroscope=" << gyro.transpose() << " rad/s" << std::endl;
 
@@ -122,8 +124,9 @@ int main(int argc,char *argv[])
 
 			Eigen::Quaterniond q=imu->quaternion();
 
-			std::cout << "EKF=" << qEkf.w() << " " << qEkf.vec().transpose() << std::endl
+			std::cout << "ekf.quaternion()=" << qEkf.w() << " " << qEkf.vec().transpose() << std::endl
 				<< "Quaternion=" << q.w() << " " << q.vec().transpose() << std::endl
+				<< "ekf.euler()=" << ekf.euler() << " rad" << std::endl
 				<< "Euler angles=" << imu->euler() << " rad" << std::endl
 				<< "Linear acceleration=" << imu->linearAccel().transpose() << " m/s^2" << std::endl
 				<< "Gravity=" << imu->gravity().transpose() << " m/s^2" << std::endl;