From 7ef38112d27982f3c252b937654dc89d59857063 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 11 Mar 2022 13:36:32 -0500 Subject: [PATCH] ekf2: return saved mag bias variance when not in 3d magnetometer fusion - the estimated mag bias was requiring > 30 seconds of continuous 3d mag fusion to be reported stable (and saved back to mag cal), this restores the original intent requiring 30 seconds of accumulated valid 3d mag fusion --- src/modules/ekf2/EKF/ekf.h | 15 +++++++++++--- src/modules/ekf2/EKF/ekf_helper.cpp | 31 ++++++++++++++++------------- 2 files changed, 29 insertions(+), 17 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f8dcca758b..03e807f661 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -246,7 +246,15 @@ public: Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2 - Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; } + + Vector3f getMagBiasVariance() const + { + if (_control_status.flags.mag_3D) { + return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; + } + + return _saved_mag_bf_variance; + } bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } @@ -518,8 +526,9 @@ 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) - float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) - Matrix2f _saved_mag_ef_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) + 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) gps_check_fail_status_u _gps_check_fail_status{}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a4253adc24..8830804d5f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -879,10 +879,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[0] is the the D earth axis - _saved_mag_bf_variance[1] = 0; - _saved_mag_bf_variance[2] = 0; - _saved_mag_bf_variance[3] = 0; + _saved_mag_bf_variance.zero(); } // get EKF innovation consistency check status information comprising of: @@ -1487,24 +1484,30 @@ 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 the D earth axis and XYZ body axis field - for (uint8_t index = 0; index <= 3; index ++) { - _saved_mag_bf_variance[index] = P(index + 18, index + 18); - } + // 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_covmat = P.slice<2, 2>(16, 16); + _saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16); + + // save variance for the D earth axis + _saved_mag_ef_d_variance = P(18, 18); } void Ekf::loadMagCovData() { - // re-instate variances for the D earth axis and XYZ body axis field - for (uint8_t index = 0; index <= 3; index ++) { - P(index + 18, index + 18) = _saved_mag_bf_variance[index]; - } + // 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_covmat; + 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; } void Ekf::startAirspeedFusion()