From 21054a4236765d735883bb79981a085f7227b111 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Jan 2020 16:58:21 +1100 Subject: [PATCH] EKF: Fix bug causing incorrect covariance initialisation The covariance initialisation should not be performed before the quaternion states are initialised. --- EKF/covariance.cpp | 2 ++ EKF/ekf.cpp | 6 +++--- EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 1 + 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 29db22d16a..f6ed75ec5b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -46,6 +46,8 @@ #include #include +// Sets initial values for the covariance matrix +// Do not call before quaternion states have been initialised void Ekf::initialiseCovariance() { P.zero(); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1c22c44882..8fc20e77e1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -67,8 +67,6 @@ void Ekf::reset() _output_new.pos.setZero(); _output_new.quat_nominal.setIdentity(); - initialiseCovariance(); - _delta_angle_corr.setZero(); _imu_updated = false; @@ -195,7 +193,6 @@ bool Ekf::initialiseFilter() // calculate the initial magnetic field and yaw alignment _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false); - // update the yaw angle variance using the variance of the measurement if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // using magnetic heading tuning parameter @@ -216,6 +213,9 @@ bool Ekf::initialiseFilter() // reset the output predictor state history to match the EKF initial values alignOutputFilter(); + // initialise the state covariance matrix now we have starting values for all lthe states + initialiseCovariance(); + return true; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index ea1609ec12..272f1cfbba 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -746,6 +746,7 @@ private: Vector3f calcRotVecVariances(); // initialise the quaternion covariances using rotation vector variances + // do not call before quaternion states are initialised void initialiseQuatCovariances(Vector3f &rot_vec_var); // perform a limited reset of the magnetic field state covariances diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 1dea56d18e..cd3a96a72a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1480,6 +1480,7 @@ Vector3f Ekf::calcRotVecVariances() } // initialise the quaternion covariances using rotation vector variances +// do not call before quaternion states are initialised void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) { // calculate an equivalent rotation vector from the quaternion