fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Changes arising from code review

Browse Source
This commit is contained in:
Paul Riseborough 2017-07-04 08:12:15 +10:00
parent 7b5f55303a
commit 929f205a00
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/covariance.cpp
View File

@ -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];
}
}