From fa075363149958ca74190a8003b2a1b888b02fad Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 6 Apr 2017 10:27:59 +1000 Subject: [PATCH 1/2] EKF: Prevent rounding errors causing bad conditioned covariance matrix The previous practice of relying on the off-diagonals being zero caused problems with conditioning of the magnetometer fusion on one flight. By storing the variances when the learning inhibit becomes active and ensuring that the rows and columns in the covariance matrix for the inhibited states are always zero, the observed numerical conditioning error has been eliminated for replay of the problem flight log . --- EKF/covariance.cpp | 33 ++++++++++++++++++++++----------- EKF/ekf.cpp | 1 + EKF/ekf.h | 1 + 3 files changed, 24 insertions(+), 11 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index f17ce2e681..0e2591ff54 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 @@ -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) From bef7325884223580a1a8a893ed8002876cdac3ef Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 6 Apr 2017 13:00:07 +1000 Subject: [PATCH 2/2] EKF: protect covariance prediction from delta time errors Allow time step to vary by a factor of 2 to allow for jitter. --- EKF/covariance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 0e2591ff54..461199c068 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -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] = {};