diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index f592dc941b..c994c1242b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -778,3 +778,19 @@ void Ekf::fixCovarianceErrors() makeSymmetrical(P,22,23); } } + +void Ekf::resetMagCovariance() +{ + // set the quaternion covariance terms to zero + zeroRows(P,0,3); + zeroCols(P,0,3); + + // set the magnetic field covariance terms to zero + zeroRows(P,16,21); + zeroCols(P,16,21); + + // set the field state variance to the observation variance + for (uint8_t rc_index=16; rc_index <= 21; rc_index ++) { + P[rc_index][rc_index] = sq(_params.mag_noise); + } +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 17aa1bf28a..fbaa4448c2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -399,4 +399,7 @@ private: // initialise the quaternion covariances using rotation vector variances void initialiseQuatCovariances(Vector3f &rot_vec_var); + // perform a limited reset of the magnetic field state covariances + void resetMagCovariance(); + }; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index b2d58703d7..d25a938a10 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -156,8 +156,8 @@ void Ekf::fuseMag() // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_x = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); + // we need to re-initialise covariances and abort this fusion step + resetMagCovariance(); ECL_ERR("EKF magX fusion numerical error - covariance reset"); return; } @@ -208,8 +208,8 @@ void Ekf::fuseMag() // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_y = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); + // we need to re-initialise covariances and abort this fusion step + resetMagCovariance(); ECL_ERR("EKF magY fusion numerical error - covariance reset"); return; } @@ -260,8 +260,8 @@ void Ekf::fuseMag() // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_z = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); + // we need to re-initialise covariances and abort this fusion step + resetMagCovariance(); ECL_ERR("EKF magZ fusion numerical error - covariance reset"); return; }