From d936b85d714de4cc651da83228b3db4e984a4152 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Oct 2020 13:15:04 -0400 Subject: [PATCH] EKF: add fault status bit for clipping --- EKF/common.h | 1 + EKF/covariance.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 6c0bf4a02e..5c84fca8a1 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -401,6 +401,7 @@ union fault_status_u { 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 + bool bad_acc_clipping: 1; ///< 16 - true if delta velocity data contains clipping (asymmetric railing) } flags; uint16_t value; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index b25e348863..3d5e9bdf37 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -255,17 +255,22 @@ void Ekf::predictCovariance() dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); // Accelerometer Clipping + _fault_status.flags.bad_acc_clipping = false; // reset flag + // delta velocity X: increase process noise if sample contained any X axis clipping if (_imu_sample_delayed.delta_vel_clipping[0]) { dvxVar = sq(dt * BADACC_BIAS_PNOISE); + _fault_status.flags.bad_acc_clipping = true; } // delta velocity Y: increase process noise if sample contained any Y axis clipping if (_imu_sample_delayed.delta_vel_clipping[1]) { dvyVar = sq(dt * BADACC_BIAS_PNOISE); + _fault_status.flags.bad_acc_clipping = true; } // delta velocity Z: increase process noise if sample contained any Z axis clipping if (_imu_sample_delayed.delta_vel_clipping[2]) { dvzVar = sq(dt * BADACC_BIAS_PNOISE); + _fault_status.flags.bad_acc_clipping = true; } // predict the covariance