From f20fc08b7d9fbeec92ce0b2a3f88d7b9be6e5ec9 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Tue, 17 Mar 2020 10:07:20 +0100 Subject: [PATCH] ekf2: centralize mag covariance reset (#693) * ekf2: centralize mag covariance reset. A complete and clean reset of the mag states covariances is now performed through the "resetMagCov" function only. This avoid having slight differences of implementations across the code. * Ekf: reset quat cov with initial uncertainty instead of zero --- EKF/covariance.cpp | 48 ++++++++++++++++++++++++++++++---------------- EKF/ekf.h | 6 +++++- EKF/ekf_helper.cpp | 36 +++------------------------------- 3 files changed, 39 insertions(+), 51 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index f6ed75ec5b..483ad4ef7d 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -57,11 +57,7 @@ void Ekf::initialiseCovariance() const float dt = FILTER_UPDATE_PERIOD_S; - // define the initial angle uncertainty as variances for a rotation vector - Vector3f rot_vec_var; - rot_vec_var.setAll(sq(_params.initial_tilt_err)); - - initialiseQuatCovariances(rot_vec_var); + resetQuatCov(); // velocity P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); @@ -97,14 +93,7 @@ void Ekf::initialiseCovariance() // record IMU bias state covariance reset time - used to prevent resets being performed too often _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; - // earth frame and body frame magnetic field - // set to observation variance - for (uint8_t index = 16; index <= 21; index ++) { - P(index,index) = sq(_params.mag_noise); - } - - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); + resetMagCov(); // wind P(22,22) = sq(_params.initial_wind_uncertainty); @@ -869,16 +858,41 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) void Ekf::resetMagRelatedCovariances() { - // set the quaternion covariance terms to zero + resetQuatCov(); + resetMagCov(); +} + +void Ekf::resetQuatCov(){ + zeroQuatCov(); + + // define the initial angle uncertainty as variances for a rotation vector + Vector3f rot_vec_var; + rot_vec_var.setAll(sq(_params.initial_tilt_err)); + + initialiseQuatCovariances(rot_vec_var); +} + +void Ekf::zeroQuatCov() +{ P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); +} + +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(); - // reset the field state variance to the observation variance P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise)); P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); + 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() diff --git a/EKF/ekf.h b/EKF/ekf.h index 1a75f08aa9..f8eddd4be4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -760,9 +760,13 @@ private: // do not call before quaternion states are initialised void initialiseQuatCovariances(Vector3f &rot_vec_var); - // perform a limited reset of the magnetic field state covariances + // perform a limited reset of the magnetic field related state covariances void resetMagRelatedCovariances(); + void resetQuatCov(); + void zeroQuatCov(); + void resetMagCov(); + // perform a limited reset of the wind state covariances void resetWindCovariance(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7a108e0911..feac851d91 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -459,17 +459,7 @@ bool Ekf::realignYawGPS() _R_to_earth = Dcmf(_state.quat_nominal); _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; - // reset the corresponding rows and columns in the covariance matrix and set the - // variances on the magnetic field states to the measurement variance - clearMagCov(); - if (_control_status.flags.mag_3D) { - for (uint8_t index = 16; index <= 21; index ++) { - P(index,index) = sq(_params.mag_noise); - } - - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); - } + resetMagCov(); // record the start time for the magnetic field alignment _flt_mag_align_start_time = _imu_sample_delayed.time_us; @@ -485,17 +475,7 @@ bool Ekf::realignYawGPS() // calculate initial earth magnetic field states _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; - // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance - clearMagCov(); - - if (_control_status.flags.mag_3D) { - for (uint8_t index = 16; index <= 21; index ++) { - P(index,index) = sq(_params.mag_noise); - } - - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); - } + resetMagCov(); // record the start time for the magnetic field alignment _flt_mag_align_start_time = _imu_sample_delayed.time_us; @@ -579,17 +559,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // set the earth magnetic field states using the updated rotation _state.mag_I = _R_to_earth * mag_init; - // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance - clearMagCov(); - - if (_control_status.flags.mag_3D) { - for (uint8_t index = 16; index <= 21; index ++) { - P(index,index) = sq(_params.mag_noise); - } - - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); - } + resetMagCov(); // record the time for the magnetic field alignment event _flt_mag_align_start_time = _imu_sample_delayed.time_us;