diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index ffc7a04b6d..bafdff4819 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -200,24 +200,19 @@ void Ekf::fuseAirspeed() 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; - // update individual measurement health status _fault_status.flags.bad_airspeed = true; } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 4283420610..ac77da246c 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -274,28 +274,18 @@ void Ekf::fuseDrag() // the covariance matrix is unhealthy and must be corrected bool healthy = true; - //_fault_status.flags.bad_sideslip = false; 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; - - // update individual measurement health status - //_fault_status.flags.bad_sideslip = true; - } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 6a9a4f183e..934be677a3 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -245,25 +245,20 @@ void Ekf::fuseGpsAntYaw() 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; - // update individual measurement health status _fault_status.flags.bad_hdg = true; } } - // only apply covariance and state corrections if healthy if (healthy) { _time_last_gps_yaw_fuse = _time_last_imu; // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1325bd3f55..5534100e71 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -413,12 +413,10 @@ void Ekf::fuseMag() } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections @@ -733,24 +731,19 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f 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; - // update individual measurement health status _fault_status.flags.bad_hdg = true; } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections @@ -1022,24 +1015,19 @@ void Ekf::fuseDeclination(float decl_sigma) _fault_status.flags.bad_mag_decl = false; 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; - // update individual measurement health status _fault_status.flags.bad_mag_decl = true; } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index caa9916ac9..95ef19d369 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -475,13 +475,10 @@ void Ekf::fuseOptFlow() 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; - // update individual measurement health status if (obs_index == 0) { _fault_status.flags.bad_optflow_X = true; @@ -491,12 +488,10 @@ void Ekf::fuseOptFlow() } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index ef5b5c9486..5526e59d2d 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -249,23 +249,18 @@ void Ekf::fuseSideslip() 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; - // update individual measurement health status _fault_status.flags.bad_sideslip = true; } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8042e57dff..9f68e6a01c 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -187,12 +187,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } } - // only apply covariance and state corrections if healthy if (healthy) { // apply the covariance corrections P -= KHP; - // correct the covariance matrix for gross errors fixCovarianceErrors(true); // apply the state corrections