dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Improve protection against badly conditioned dVel bias covariances 

Reduces likelihood of Z delta velocity bias learning to wrong value.
This commit is contained in:
Paul Riseborough 2018-05-18 10:51:16 +10:00 committed by Lorenz Meier
parent 225057aaf5
commit 93c456f2b8
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/covariance.cpp
View File

@ -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