diff --git a/EKF/common.h b/EKF/common.h index fc871116ea..c297fa6f36 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -412,6 +412,7 @@ union fault_status_u { bool bad_pos_N: 1; // 12 - true if fusion of the North position has encountered a numerical error bool bad_pos_E: 1; // 13 - true if fusion of the East position has encountered a numerical error bool bad_pos_D: 1; // 14 - true if fusion of the Down position has encountered a numerical error + bool bad_acc_bias: 1; // 15 - true if bad delta velocity bias estimates have been detected } flags; uint16_t value; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 3191fb63df..e07341fe50 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -715,10 +715,12 @@ void Ekf::fixCovarianceErrors() && down_dvel_bias * _vel_pos_innov[2] < 0.0f && down_dvel_bias * _vel_pos_innov[5] < 0.0f); - // record the pass and force symmetry on the covariance matrix + // record the pass/fail if (!bad_acc_bias) { + _fault_status.flags.bad_acc_bias = true; _time_acc_bias_check = _time_last_imu; - makeSymmetrical(P,13,15); + } else { + _fault_status.flags.bad_acc_bias = true; } // if we have failed for 7 seconds continuously, reset the accel bias covariance @@ -732,7 +734,11 @@ void Ekf::fixCovarianceErrors() P[14][14] = varY; P[15][15] = varZ; _time_acc_bias_check = _time_last_imu; + _fault_status.flags.bad_acc_bias = false; ECL_WARN("EKF invalid accel bias - resetting covariance"); + } else { + // ensure the covariance values are symmetrical + makeSymmetrical(P,13,15); } }