diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index f17ce2e681..461199c068 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -87,9 +87,9 @@ void Ekf::initialiseCovariance() P[12][12] = P[10][10]; // accel bias - P[13][13] = sq(_params.switch_on_accel_bias * dt); - P[14][14] = P[13][13]; - P[15][15] = P[13][13]; + _prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt); + _prev_dvel_bias_var(1) = P[14][14] = P[13][13]; + _prev_dvel_bias_var(2) = P[15][15] = P[13][13]; // variances for optional states @@ -143,7 +143,7 @@ void Ekf::predictCovariance() float dvy_b = _state.accel_bias(1); float dvz_b = _state.accel_bias(2); - float dt = _imu_sample_delayed.delta_ang_dt; + float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS); // compute noise variance for stationary processes float process_noise[_k_num_states] = {}; @@ -159,8 +159,25 @@ void Ekf::predictCovariance() _ang_rate_mag_filt = fmax(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); _accel_mag_filt = fmax(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim) { + // store the bias state variances to be reinstated later + if (!_accel_bias_inhibit) { + _prev_dvel_bias_var(0) = P[13][13]; + _prev_dvel_bias_var(1) = P[14][14]; + _prev_dvel_bias_var(2) = P[15][15]; + } _accel_bias_inhibit = true; } else { + if (_accel_bias_inhibit) { + // reinstate the bias state variances + P[13][13] = _prev_dvel_bias_var(0); + P[14][14] = _prev_dvel_bias_var(1); + P[15][15] = _prev_dvel_bias_var(2); + } else { + // store the bias state variances to be reinstated later + _prev_dvel_bias_var(0) = P[13][13]; + _prev_dvel_bias_var(1) = P[14][14]; + _prev_dvel_bias_var(2) = P[15][15]; + } _accel_bias_inhibit = false; } @@ -436,15 +453,9 @@ void Ekf::predictCovariance() } } else { - // Inhibit delta velocity bias learning. Zero the covariance terms but preserve the variances from the - // previous prediction step which prevents these states being updated by any of the measurement fusion - // processes, but allows estimation to be resumed later. + // Inhibit delta velocity bias learning by zeroing the covariance terms zeroRows(nextP,13,15); zeroCols(nextP,13,15); - nextP[13][13] = P[13][13]; - nextP[14][14] = P[14][14]; - nextP[15][15] = P[15][15]; - } // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion @@ -714,7 +725,7 @@ void Ekf::fixCovarianceErrors() // by ensuring the corresponding covariance matrix values are kept at zero // accelerometer bias states - if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { + if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) { zeroRows(P,13,15); zeroCols(P,13,15); } else { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 35416f2943..c449c3f558 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -198,6 +198,7 @@ bool Ekf::init(uint64_t timestamp) _accel_mag_filt = 0.0f; _ang_rate_mag_filt = 0.0f; + _prev_dvel_bias_var.zero(); return ret; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 419c8e0cc9..ddf4bab24a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -325,6 +325,7 @@ private: bool _accel_bias_inhibit; // true when the accel bias learning is being inhibited float _accel_mag_filt; // acceleration magnitude after application of a decaying envelope filter (m/sec**2) float _ang_rate_mag_filt; // angular rate magnitude after application of a decaying envelope filter (rad/sec) + Vector3f _prev_dvel_bias_var; // saved delta velocity XYZ bias variances (m/sec)**2 // Terrain height state estimation float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)