From 89bfcc2167ed738df3538b4bbd1f9a303fefe5f0 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 11 Jul 2020 09:13:39 +0200 Subject: [PATCH] Introduce checkAndFixCovarianceUpdate(KHP) function --- EKF/airspeed_fusion.cpp | 16 ++-------- EKF/covariance.cpp | 14 +++++++++ EKF/drag_fusion.cpp | 12 +------- EKF/ekf.h | 4 +++ EKF/gps_yaw_fusion.cpp | 16 ++-------- EKF/mag_fusion.cpp | 67 +++++++++-------------------------------- EKF/optflow_fusion.cpp | 23 +++----------- EKF/sideslip_fusion.cpp | 15 ++------- 8 files changed, 44 insertions(+), 123 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 35b285f332..a542c4705a 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -177,21 +177,9 @@ void Ekf::fuseAirspeed() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_airspeed = false; + const bool healthy = checkAndFixCovarianceUpdate(KHP); - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; - - _fault_status.flags.bad_airspeed = true; - - } - } + _fault_status.flags.bad_airspeed = !healthy; if (healthy) { // apply the covariance corrections diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 21e0264c84..d226b3cecc 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -848,6 +848,20 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } } +// if the covariance correction will result in a negative variance, then +// the covariance matrix is unhealthy and must be corrected +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) { + bool healthy = true; + for (int i = 0; i < _k_num_states; i++) { + if (P(i, i) < KHP(i, i)) { + P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); + healthy = false; + } + } + return healthy; +} + + void Ekf::resetMagRelatedCovariances() { resetQuatCov(); diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index dca6b9524d..2ff3f8ad2b 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -239,17 +239,7 @@ void Ekf::fuseDrag() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; - } - } + bool healthy = checkAndFixCovarianceUpdate(KHP); if (healthy) { // apply the covariance corrections diff --git a/EKF/ekf.h b/EKF/ekf.h index 2d31eb3099..ab38e52afe 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -662,6 +662,10 @@ private: Vector3f getVisionVelocityVarianceInEkfFrame() const; + // if the covariance correction will result in a negative variance, then + // the covariance matrix is unhealthy and must be corrected + bool checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP); + // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 42abd0d40c..df97925085 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -232,21 +232,9 @@ void Ekf::fuseGpsYaw() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_hdg = false; + const bool healthy = checkAndFixCovarianceUpdate(KHP); - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; - - _fault_status.flags.bad_hdg = true; - - } - } + _fault_status.flags.bad_hdg = !healthy; if (healthy) { // apply the covariance corrections diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 5d08e531ac..eac37b344d 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -143,13 +143,13 @@ void Ekf::fuseMag() } // Perform an innovation consistency check and report the result - bool healthy = true; + bool all_innovation_checks_passed = true; for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index)); if (_mag_test_ratio(index) > 1.0f) { - healthy = false; + all_innovation_checks_passed = false; _innov_check_fail_status.value |= (1 << (index + 3)); } else { @@ -161,7 +161,7 @@ void Ekf::fuseMag() _yaw_test_ratio = 0.0f; // if any axis fails, abort the mag fusion - if (!healthy) { + if (!all_innovation_checks_passed) { return; } @@ -169,10 +169,6 @@ void Ekf::fuseMag() // before they are used to constrain heading drift const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); - _fault_status.flags.bad_mag_x = false; - _fault_status.flags.bad_mag_y = false; - _fault_status.flags.bad_mag_z = false; - // Observation jacobian and Kalman gain vectors Vector24f H_MAG; Vector24f Kfusion; @@ -367,28 +363,16 @@ void Ekf::fuseMag() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - // zero rows and columns - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - //flag as unhealthy - healthy = false; + const bool healthy = checkAndFixCovarianceUpdate(KHP); - // update individual measurement health status - if (index == 0) { - _fault_status.flags.bad_mag_x = true; + if (index == 0) { + _fault_status.flags.bad_mag_x = !healthy; - } else if (index == 1) { - _fault_status.flags.bad_mag_y = true; + } else if (index == 1) { + _fault_status.flags.bad_mag_y = !healthy; - } else if (index == 2) { - _fault_status.flags.bad_mag_z = true; - } - } + } else if (index == 2) { + _fault_status.flags.bad_mag_z = !healthy; } if (healthy) { @@ -698,21 +682,9 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_hdg = false; - - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); + const bool healthy = checkAndFixCovarianceUpdate(KHP); - healthy = false; - - _fault_status.flags.bad_hdg = true; - - } - } + _fault_status.flags.bad_hdg = !healthy; if (healthy) { // apply the covariance corrections @@ -983,20 +955,9 @@ void Ekf::fuseDeclination(float decl_sigma) } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_mag_decl = false; - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); + const bool healthy = checkAndFixCovarianceUpdate(KHP); - healthy = false; - - _fault_status.flags.bad_mag_decl = true; - - } - } + _fault_status.flags.bad_mag_decl = !healthy; if (healthy) { // apply the covariance corrections diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 07098f1d22..9c53d0a7c2 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -425,9 +425,6 @@ void Ekf::fuseOptFlow() } - _fault_status.flags.bad_optflow_X = false; - _fault_status.flags.bad_optflow_Y = false; - for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { // copy the Kalman gain vector for the axis we are fusing @@ -465,23 +462,13 @@ void Ekf::fuseOptFlow() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; + const bool healthy = checkAndFixCovarianceUpdate(KHP); - if (obs_index == 0) { - _fault_status.flags.bad_optflow_X = true; + if (obs_index == 0) { + _fault_status.flags.bad_optflow_X = !healthy; - } else if (obs_index == 1) { - _fault_status.flags.bad_optflow_Y = true; - } - } + } else if (obs_index == 1) { + _fault_status.flags.bad_optflow_Y = !healthy; } if (healthy) { diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 2fdd4afb22..2be758173b 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -224,20 +224,9 @@ void Ekf::fuseSideslip() } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_sideslip = false; + const bool healthy = checkAndFixCovarianceUpdate(KHP); - for (int i = 0; i < _k_num_states; i++) { - if (P(i,i) < KHP(i,i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; - - _fault_status.flags.bad_sideslip = true; - } - } + _fault_status.flags.bad_sideslip = !healthy; if (healthy) { // apply the covariance corrections