From 7b5f55303acffea9d69b904cd0ba45adae1ecb0a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 29 Jun 2017 21:14:15 +1000 Subject: [PATCH 1/2] EKF: Prevent covariance instability in delta velocity bias state estimation --- EKF/common.h | 2 +- EKF/covariance.cpp | 37 ++++++++++++++++++++++++++++++++++--- 2 files changed, 35 insertions(+), 4 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index e4e2dc3092..84c92be36d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -212,7 +212,7 @@ struct parameters { // process noise float gyro_bias_p_noise{1.0e-3f}; // process noise for IMU rate gyro bias prediction (rad/sec**2) - float accel_bias_p_noise{3.0e-3f}; // process noise for IMU accelerometer bias prediction (m/sec**3) + float accel_bias_p_noise{6.0e-3f}; // process noise for IMU accelerometer bias prediction (m/sec**3) float mage_p_noise{1.0e-3f}; // process noise for earth magnetic field prediction (Guass/sec) float magb_p_noise{1.0e-4}; // process noise for body magnetic field prediction (Guass/sec) float wind_vel_p_noise{1.0e-1f}; // process noise for wind velocity prediction (m/sec/sec) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index e7f72a553c..2bc5dcd890 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -729,11 +729,42 @@ void Ekf::fixCovarianceErrors() zeroRows(P,13,15); zeroCols(P,13,15); } else { - // constrain variances - for (int i = 13; i <= 15; i++) { - P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); + // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum + const float minSafeStateVar = 1e-9f; + float maxStateVar = minSafeStateVar; + bool resetRequired = false; + for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { + if (P[stateIndex][stateIndex] > maxStateVar) { + maxStateVar = P[stateIndex][stateIndex]; + } else if (P[stateIndex][stateIndex] < minSafeStateVar) { + resetRequired = true; + } } + // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must + // not exceed 100 and the minimum variance must not fall below the target minimum + const float minStateVarTarget = 1E-8f; + float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); + for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { + P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * _dt_ekf_avg)); + } + + // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero + if (resetRequired) { + float delVelBiasVar[3]; + // store all delta velocity bias variances + for (uint8_t i=0; i<=2; i++) { + delVelBiasVar[i] = P[i+13][i+13]; + } + // reset all delta velocity bias covariances + zeroCols(P,13,15); + // restore all delta velocity bias variances + for (uint8_t i=0; i<=2; i++) { + P[i+13][i+13] = delVelBiasVar[i]; + } + } + + // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong // 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; From 929f205a00683f85af6ca352cfdc539619eb1d30 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 4 Jul 2017 08:12:15 +1000 Subject: [PATCH 2/2] EKF: Changes arising from code review --- EKF/covariance.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 2bc5dcd890..f85eb54361 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -743,24 +743,25 @@ void Ekf::fixCovarianceErrors() // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must // not exceed 100 and the minimum variance must not fall below the target minimum + // Also limit variance to a maximum equivalent to a 1g uncertainty const float minStateVarTarget = 1E-8f; float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * _dt_ekf_avg)); + P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(_gravity_mss * _dt_ekf_avg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { float delVelBiasVar[3]; // store all delta velocity bias variances - for (uint8_t i=0; i<=2; i++) { - delVelBiasVar[i] = P[i+13][i+13]; + for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { + delVelBiasVar[stateIndex-13] = P[stateIndex][stateIndex]; } // reset all delta velocity bias covariances zeroCols(P,13,15); // restore all delta velocity bias variances - for (uint8_t i=0; i<=2; i++) { - P[i+13][i+13] = delVelBiasVar[i]; + for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { + P[stateIndex][stateIndex] = delVelBiasVar[stateIndex-13]; } }