From 644e9035527e91a6f97889865e0acb91431c126e Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 22 Aug 2020 10:24:03 +0200 Subject: [PATCH] Apply measurementUpdate function --- EKF/airspeed_fusion.cpp | 20 +++--------------- EKF/drag_fusion.cpp | 18 +---------------- EKF/gps_yaw_fusion.cpp | 20 +++--------------- EKF/mag_fusion.cpp | 45 ++++++++--------------------------------- EKF/optflow_fusion.cpp | 21 ++++--------------- EKF/sideslip_fusion.cpp | 19 +++-------------- 6 files changed, 22 insertions(+), 121 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index d9a86a59fe..2f678c93ad 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -152,26 +152,12 @@ void Ekf::fuseAirspeed() _innov_check_fail_status.flags.reject_airspeed = false; } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, _airspeed_innov); - const bool healthy = checkAndFixCovarianceUpdate(KHP); - - _fault_status.flags.bad_airspeed = !healthy; - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _airspeed_innov); + _fault_status.flags.bad_airspeed = !is_fused; + if (is_fused) { _time_last_arsp_fuse = _time_last_imu; - } } diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 1f95728fcb..a2d84d46f1 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -264,23 +264,7 @@ void Ekf::fuseDrag() // if the innovation consistency check fails then don't fuse the sample if (_drag_test_ratio(axis_index) <= 1.0f) { - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); - - const bool healthy = checkAndFixCovarianceUpdate(KHP); - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _drag_innov(axis_index)); - - } + measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index)); } } } diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index ed43709d29..237bd52652 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -186,24 +186,10 @@ void Ekf::fuseGpsYaw() Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row)); } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); - - const bool healthy = checkAndFixCovarianceUpdate(KHP); - - _fault_status.flags.bad_hdg = !healthy; - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _heading_innov); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov); + _fault_status.flags.bad_hdg = !is_fused; + if (is_fused) { _time_last_gps_yaw_fuse = _time_last_imu; } } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index dca25e6cc6..bd61c3b57a 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -399,33 +399,19 @@ void Ekf::fuseMag() Kfusion(21) = HKZ23*HKZ24; } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); - - const bool healthy = checkAndFixCovarianceUpdate(KHP); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index)); if (index == 0) { - _fault_status.flags.bad_mag_x = !healthy; + _fault_status.flags.bad_mag_x = !is_fused; } else if (index == 1) { - _fault_status.flags.bad_mag_y = !healthy; + _fault_status.flags.bad_mag_y = !is_fused; } else if (index == 2) { - _fault_status.flags.bad_mag_z = !healthy; + _fault_status.flags.bad_mag_z = !is_fused; } - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _mag_innov(index)); - - // constrain the declination of the earth field states + if (is_fused) { limitDeclination(); } } @@ -926,26 +912,11 @@ void Ekf::fuseDeclination(float decl_sigma) const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f); - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - // take advantage of the empty columns in KH to reduce the number of operations - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation); - const bool healthy = checkAndFixCovarianceUpdate(KHP); + _fault_status.flags.bad_mag_decl = !is_fused; - _fault_status.flags.bad_mag_decl = !healthy; - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, innovation); - - // constrain the declination of the earth field states + if (is_fused) { limitDeclination(); } } diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 0834deb6cd..dcbe90a759 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -312,29 +312,16 @@ void Ekf::fuseOptFlow() } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); - - const bool healthy = checkAndFixCovarianceUpdate(KHP); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, _flow_innov(obs_index)); if (obs_index == 0) { - _fault_status.flags.bad_optflow_X = !healthy; + _fault_status.flags.bad_optflow_X = !is_fused; } else if (obs_index == 1) { - _fault_status.flags.bad_optflow_Y = !healthy; + _fault_status.flags.bad_optflow_Y = !is_fused; } - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _flow_innov(obs_index)); - + if (is_fused) { _time_last_of_fuse = _time_last_imu; } } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 254c304a96..2b966a1b72 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -203,24 +203,11 @@ void Ekf::fuseSideslip() Kfusion(22) = HK45*HK52; Kfusion(23) = HK42*HK52; - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, _beta_innov); - const bool healthy = checkAndFixCovarianceUpdate(KHP); - - _fault_status.flags.bad_sideslip = !healthy; - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, _beta_innov); + _fault_status.flags.bad_sideslip = !is_fused; + if (is_fused) { _time_last_beta_fuse = _time_last_imu; } }