From 1a5ff0bae43627c167d69f36b29e10b59bcf8a3d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 31 Mar 2022 19:26:06 -0400 Subject: [PATCH] ekf2: save full covariance sub-matrix for mag I/B --- src/modules/ekf2/EKF/covariance.cpp | 25 ++++------------ src/modules/ekf2/EKF/ekf.h | 10 ++----- src/modules/ekf2/EKF/ekf_helper.cpp | 45 ++++++++++------------------- 3 files changed, 24 insertions(+), 56 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f1b7478bd9..72de2b6f7a 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -1021,7 +1021,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // magnetic field states if (!_control_status.flags.mag_3D) { - zeroMagCov(); + P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); + P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); } else { // constrain variances @@ -1101,29 +1102,13 @@ void Ekf::resetMagCov() { // reset the corresponding rows and columns in the covariance matrix and // set the variances on the magnetic field states to the measurement variance - clearMagCov(); + _mag_decl_cov_reset = false; P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise)); P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); - if (!_control_status.flags.mag_3D) { - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - // if already in 3-axis fusion mode, the covariances are automatically saved when switching out - // of this mode - saveMagCovData(); - } -} - -void Ekf::clearMagCov() -{ - zeroMagCov(); - _mag_decl_cov_reset = false; -} - -void Ekf::zeroMagCov() -{ - P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); + // save covariance data for re-use when auto-switching between heading and 3-axis fusion + saveMagCovData(); } void Ekf::resetZDeltaAngBiasCov() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c7773d02a5..e8f2b18228 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -255,7 +255,7 @@ public: return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; } - return _saved_mag_bf_variance; + return _saved_mag_bf_covmat.diag(); } bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } @@ -529,9 +529,8 @@ private: float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) - Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) - Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) - float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2) + Matrix3f _saved_mag_bf_covmat{}; ///< magnetometer bias state covariance sub-matrix saved for use at the next initialisation (Gauss**2) + Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) float _mag_heading_last_declination{NAN}; @@ -963,10 +962,7 @@ private: void increaseQuatYawErrVariance(float yaw_variance); // load and save mag field state covariance data for re-use - void loadMagCovData(); void saveMagCovData(); - void clearMagCov(); - void zeroMagCov(); void resetZDeltaAngBiasCov(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 96164bd91b..445bfea6a9 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -865,7 +865,7 @@ void Ekf::resetMagBias() P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); // reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion - _saved_mag_bf_variance.zero(); + _saved_mag_bf_covmat.zero(); _mag_counter = 0; } @@ -1198,13 +1198,18 @@ void Ekf::stopMagFusion() { stopMag3DFusion(); stopMagHdgFusion(); - clearMagCov(); + + P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); + P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); + + _mag_decl_cov_reset = false; } void Ekf::stopMag3DFusion() { // save covariance data for re-use if currently doing 3-axis fusion if (_control_status.flags.mag_3D) { + saveMagCovData(); _control_status.flags.mag_3D = false; @@ -1250,8 +1255,12 @@ void Ekf::startMag3DFusion() _yaw_test_ratio = 0.0f; - zeroMagCov(); - loadMagCovData(); + // re-instate the NE axis covariance sub-matrix + P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat; + + // re-instate variances for the XYZ body axis field + P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat; + _control_status.flags.mag_3D = true; } } @@ -1497,33 +1506,11 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance) // save covariance data for re-use when auto-switching between heading and 3-axis fusion void Ekf::saveMagCovData() { - // save variances for XYZ body axis field - _saved_mag_bf_variance(0) = P(19, 19); - _saved_mag_bf_variance(1) = P(20, 20); - _saved_mag_bf_variance(2) = P(21, 21); - // save the NE axis covariance sub-matrix - _saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16); + _saved_mag_ef_covmat = P.slice<3, 3>(16, 16); - // save variance for the D earth axis - _saved_mag_ef_d_variance = P(18, 18); -} - -void Ekf::loadMagCovData() -{ - P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); - - // re-instate variances for the XYZ body axis field - P(19, 19) = _saved_mag_bf_variance(0); - P(20, 20) = _saved_mag_bf_variance(1); - P(21, 21) = _saved_mag_bf_variance(2); - - // re-instate the NE axis covariance sub-matrix - P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat; - - // re-instate the D earth axis variance - P(18, 18) = _saved_mag_ef_d_variance; + // save variances for XYZ body axis field + _saved_mag_bf_covmat = P.slice<3, 3>(19, 19); } void Ekf::startAirspeedFusion()