diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 37d4474f11..215440e934 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -769,12 +769,12 @@ 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; + // Also limit variance to a maximum equivalent to a 0.1g uncertainty + const float minStateVarTarget = 5E-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(CONSTANTS_ONE_G * _dt_ekf_avg)); + P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero