diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cb..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -280,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise)