Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix minor bug in calculation of innovation variance 

the innovation variance for GPS should be the sum of squares of the state and measurement uncertainty.
This commit is contained in:
Paul Riseborough 2015-04-15 17:18:31 +10:00 committed by Randy Mackay
parent 1008c6390c
commit 5d70854c08
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1988,7 +1988,7 @@ void NavEKF::FuseVelPosNED()
// calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter
// max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
float accelScale = (1.0f + 0.1f * accNavMag);
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - lastPosPassTime)));
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise) + sq(0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - lastPosPassTime)));
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data or not aiding