From: Walter Fetter Lages Date: Fri, 12 Sep 2025 06:31:14 +0000 (-0300) Subject: Add accelerometer and magnetometer biases. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;ds=sidebyside;p=imufusion.git Add accelerometer and magnetometer biases. --- diff --git a/lib/ekf.cpp b/lib/ekf.cpp index 4e157dd..01389d2 100644 --- 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 + Copyright (C) 2019..2025 Walter Fetter Lages 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; }