aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Limit heading innovations after the consistency check

Browse Source
This commit is contained in:
Paul Riseborough 2016-02-16 18:42:59 +11:00 committed by Randy Mackay
parent 6a34e4c384
commit d3c9a0aef1
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -684,13 +684,6 @@ void NavEKF2_core::fuseCompass()
// Copy raw value to output variable used for data logging // Copy raw value to output variable used for data logging
innovYaw = innovation; innovYaw = innovation;
// limit the innovation so that initial corrections are not too large
if (innovation > 0.5f) {
innovation = 0.5f;
} else if (innovation < -0.5f) {
innovation = -0.5f;
}
// calculate the innovation test ratio // calculate the innovation test ratio
yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov); yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov);
@ -706,6 +699,13 @@ void NavEKF2_core::fuseCompass()
magHealth = true; magHealth = true;
} }
// limit the innovation so that initial corrections are not too large
if (innovation > 0.5f) {
innovation = 0.5f;
} else if (innovation < -0.5f) {
innovation = -0.5f;
}
// correct the state vector // correct the state vector
stateStruct.angErr.zero(); stateStruct.angErr.zero();
for (uint8_t i=0; i<=stateIndexLim; i++) { for (uint8_t i=0; i<=stateIndexLim; i++) {