Fix bias in f() and F().

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