Change model to ENU convention.

diff --git a/include/ekf.hpp b/include/ekf.hpp
index ff353a1..3a2bd69 100644 (file)
--- 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 (file)
--- 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;
 }