diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 552386843b..c0084ddbd4 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -701,8 +701,37 @@ void Ekf::fixCovarianceErrors() for (int i = 13; i <= 15; i++) { P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); } - // force symmetry - makeSymmetrical(P,13,15); + + // calculate accel bias term aligned with the gravity vector + float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; + float down_dvel_bias = 0.0f; + for (uint8_t axis_index=0; axis_index < 3; axis_index++) { + down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index); + } + + // check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation + bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim + && 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 + if (!bad_acc_bias) { + _time_acc_bias_check = _time_last_imu; + makeSymmetrical(P,13,15); + } + + // if we have failed for 7 seconds continuously, reset the accel bias covariance + if (_time_last_imu - _time_acc_bias_check > 7E6) { + float varX = P[13][13]; + float varY = P[14][14]; + float varZ = P[15][15]; + zeroRows(P,13,15); + zeroCols(P,13,15); + P[13][13] = varX; + P[14][14] = varY; + P[15][15] = varZ; + } + } // magnetic field states diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6d760a9bcf..62628a97ea 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -85,6 +85,7 @@ Ekf::Ekf(): _time_last_beta_fuse(0), _last_disarmed_posD(0.0f), _last_dt_overrun(0.0f), + _time_acc_bias_check(0.0f), _airspeed_innov(0.0f), _airspeed_innov_var(0.0f), _beta_innov(0.0f), diff --git a/EKF/ekf.h b/EKF/ekf.h index e2a70715a5..890ac87696 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -252,6 +252,8 @@ private: float _last_disarmed_posD; // vertical position recorded at arming (m) float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERIOD_MS (sec) + uint64_t _time_acc_bias_check; // last time the accel bias check passed (usec) + Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition