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

AP_NavEKF : Fix bug in GPS innovation variance growth calculation 

The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error.
The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres.
This error was found during flight testing with 8g pullups
This commit is contained in:
priseborough 2014-09-07 09:14:13 +10:00 committed by Andrew Tridgell
parent f0ee11e951
commit f99f5759f5
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

@ -1717,7 +1717,7 @@ void NavEKF::FuseVelPosNED()
// apply an innovation consistency threshold test, but don't fail if bad IMU data // apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter // 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 // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
float accelScale = (1.0f + 0.1f * accNavMagHoriz); float accelScale = (1.0f + 0.1f * accNavMag);
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime)));
posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata); posHealth = ((posTestRatio < 1.0f) || badIMUdata);