Fix Jacobians size.

diff --git a/include/ekf.hpp b/include/ekf.hpp
index 799e08b..5f6bbc0 100644 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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;