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

AP_NavEKF: Fix minor bug in calculation of innovation variance

Browse Source 
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