Add accelerometer and magnetometer biases.

diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index 4e157dd..01389d2 100644 (file)
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -1,7 +1,7 @@
 /******************************************************************************
                                IMU Fusion Library
                              Extended Kalman Filter
-          Copyright (C) 2019, 2020 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2019..2025 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -24,7 +24,7 @@
 namespace imufusion
 {
 
-Ekf::Ekf(const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T,int initSamples): T_(T), initSamples_(initSamples), nSamples_(1), x_(7), P_(7,7), Pw_(Pw),Pv_(Pv)
+Ekf::Ekf(const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T,int initSamples): T_(T), initSamples_(initSamples), nSamples_(1), x_(13), P_(13,13), Pw_(Pw),Pv_(Pv)
 {
        P_.setIdentity();
        P_*=1e6;
@@ -32,9 +32,9 @@ Ekf::Ekf(const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T,int initSa
        x_.setZero();
        x_[0]=1.0;
 
-       mag0_ << 0,
-               1,
-               0;
+       mag0_ << 0.0,
+               1.0,
+               0.0;
 
        accel_.setZero();
        gyro_.setZero();
@@ -64,13 +64,15 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig
 
                Eigen::Quaterniond q=Eigen::Quaterniond(R);
 
-               Eigen::VectorXd xi(7);
+               Eigen::VectorXd xi(13);
 
                xi <<   q.w(),
                        q.x(),
                        q.y(),
                        q.z(),
-                       gyro;
+                       gyro,
+                       Eigen::Vector3d::Zero(),        // accelerometer bias
+                       Eigen::Vector3d::Zero();        // magnetometer bias
 
                if(nSamples_ >=2) P_=(nSamples_-2.0)/(nSamples_-1.0)*P_+1.0/nSamples_*(xi-x_)*(xi-x_).transpose();
 
@@ -102,7 +104,9 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig
                                q.x(),
                                q.y(),
                                q.z(),
-                               gyro_;
+                               gyro_,
+                               Eigen::Vector3d::Zero(),        // accelerometer bias
+                               Eigen::Vector3d::Zero();        // magnetometer bias
 
                        mag0_=R*mag_;
                        accel0_=R*accel_;
@@ -119,21 +123,21 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig
 
        // Gain
        Eigen::MatrixXd K=P_*H().transpose()*(H()*P_*H().transpose()+Pv_).inverse();
-       
+
        // State estimation
        Eigen::VectorXd x=x_+K*(y-h());
-       x.head<4>().normalize();
+       x.segment<4>(0).normalize();
 
        // Covariance a posteriori
-       Eigen::MatrixXd P=(Eigen::MatrixXd::Identity(7,7)-K*H())*P_;
+       Eigen::MatrixXd P=(Eigen::MatrixXd::Identity(13,13)-K*H())*P_;
 
        // Covariance prediction
-       Eigen::Vector3d u=gyro-x_.tail<3>();
+       Eigen::Vector3d u=gyro-x_.segment<3>(4);
        P_=F(u)*P*F(u).transpose()+Pw_;
 
        // State prediction
        x_=f(u);        // Should be the last operation because it changes x_
-       x_.head<4>().normalize();
+       x_.segment<4>(0).normalize();
 }
 
 Eigen::Quaterniond Ekf::quaternion(void)
@@ -169,9 +173,9 @@ Eigen::MatrixXd Ekf::Xi(void)
 
 Eigen::VectorXd Ekf::f(const Eigen::Vector3d u)
 {
-       Eigen::MatrixXd G(7,3);
+       Eigen::MatrixXd G(13,3);
        G <<    T_/2.0*Xi(),
-               Eigen::MatrixXd::Zero(3,3);
+               Eigen::MatrixXd::Zero(9,3);
 
        return x_+G*u;
 }
@@ -184,10 +188,10 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
                u(1), -u(2), 0.0, u(0),
                u(2), u(1), -u(0), 0.0;
 
-       Eigen::MatrixXd F(7,7);
-       F <<    T_/2.0*Omega, -T_/2.0*Xi(),
-               Eigen::MatrixXd::Zero(3,4), Eigen::MatrixXd::Zero(3,3);
-       F+=Eigen::MatrixXd::Identity(7,7);
+       Eigen::MatrixXd F(13,13);
+       F.setIdentity();
+       F.block<4,4>(0,0)+=T_/2.0*Omega;
+       F.block<4,3>(0,4)+=-T_/2.0*Xi();
 
        return F;
 }
@@ -195,8 +199,8 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u)
 Eigen::VectorXd Ekf::h(void)
 {
        Eigen::Matrix3d R=Eigen::Quaterniond(x_[0],x_[1],x_[2],x_[3]).matrix(); // Do not use x_.head<>(). Internal order of elements is different.
-       Eigen::Vector3d ya=R.transpose()*accel0_;
-       Eigen::Vector3d ym=R.transpose()*mag0_;
+       Eigen::Vector3d ya=R.transpose()*accel0_+x_.segment<3>(7);
+       Eigen::Vector3d ym=R.transpose()*mag0_+x_.segment<3>(10);
 
        Eigen::VectorXd h(6);
        h <<    ya,
@@ -207,44 +211,55 @@ Eigen::VectorXd Ekf::h(void)
 
 Eigen::MatrixXd Ekf::H(void)
 {
-       Eigen::MatrixXd H(6,7);
+       Eigen::MatrixXd H(6,13);
 
        H <<    2*x_(0)*accel0_(0)+2*x_(3)*accel0_(1)-2*x_(2)*accel0_(2),
                 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2),
                 -2*x_(2)*accel0_(0)+2*x_(1)*accel0_(1)-2*x_(0)*accel0_(2),
                 -2*x_(3)*accel0_(0)+2*x_(0)*accel0_(1)+2*x_(1)*accel0_(2),
-                Eigen::MatrixXd::Zero(1,3),
+                Eigen::RowVector3d::Zero(),
+                1.0, 0.0, 0.0,
+                Eigen::RowVector3d::Zero(),
 
                 -2*x_(3)*accel0_(0)+2*x_(0)*accel0_(1)+2*x_(1)*accel0_(2),
                 2*x_(2)*accel0_(0)-2*x_(1)*accel0_(1)+2*x_(0)*accel0_(2),
                 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2),
                 -2*x_(0)*accel0_(0)-2*x_(3)*accel0_(1)+2*x_(2)*accel0_(2),
-                Eigen::MatrixXd::Zero(1,3),
+                Eigen::RowVector3d::Zero(),
+                0.0, 1.0, 0.0,
+                Eigen::RowVector3d::Zero(),
 
                 2*x_(2)*accel0_(0)-2*x_(1)*accel0_(1)+2*x_(0)*accel0_(2),
                 2*x_(3)*accel0_(0)-2*x_(0)*accel0_(1)-2*x_(1)*accel0_(2),
                 2*x_(0)*accel0_(0)+2*x_(3)*accel0_(1)-2*x_(2)*accel0_(2),
                 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2),
-                Eigen::MatrixXd::Zero(1,3),
-
+                Eigen::RowVector3d::Zero(),
+                0.0, 0.0, 1.0,
+                Eigen::RowVector3d::Zero(),
 
                 2*x_(0)*mag0_(0)+2*x_(3)*mag0_(1)-2*x_(2)*mag0_(2),
                 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2),
                 -2*x_(2)*mag0_(0)+2*x_(1)*mag0_(1)-2*x_(0)*mag0_(2),
                 -2*x_(3)*mag0_(0)+2*x_(0)*mag0_(1)+2*x_(1)*mag0_(2),
-                Eigen::MatrixXd::Zero(1,3),
+                Eigen::RowVector3d::Zero(),
+                Eigen::RowVector3d::Zero(),
+                1.0, 0.0, 0.0,
 
                 -2*x_(3)*mag0_(0)+2*x_(0)*mag0_(1)+2*x_(1)*mag0_(2),
                 2*x_(2)*mag0_(0)-2*x_(1)*mag0_(1)+2*x_(0)*mag0_(2),
                 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2),
                 -2*x_(0)*mag0_(0)-2*x_(3)*mag0_(1)+2*x_(2)*mag0_(2),
-                Eigen::MatrixXd::Zero(1,3),
+                Eigen::RowVector3d::Zero(),
+                Eigen::RowVector3d::Zero(),
+                0.0, 1.0, 0.0,
 
                 2*x_(2)*mag0_(0)-2*x_(1)*mag0_(1)+2*x_(0)*mag0_(2),
                 2*x_(3)*mag0_(0)-2*x_(0)*mag0_(1)-2*x_(1)*mag0_(2),
                 2*x_(0)*mag0_(0)+2*x_(3)*mag0_(1)-2*x_(2)*mag0_(2),
                 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2),
-                Eigen::MatrixXd::Zero(1,3);
+                Eigen::RowVector3d::Zero(),
+                Eigen::RowVector3d::Zero(),
+                0.0, 0.0, 1.0;
 
        return H;
 }