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

AP_NavEKF2: Improve handling of GPS loss and recovery for planes 

Extended GPS loss can result in the earth field states becoming  rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
This commit is contained in:
Paul Riseborough 2015-10-23 23:44:43 +11:00
parent cd5ec3a3e0
commit 1e7ac873b9
2 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -151,7 +151,7 @@ void NavEKF2_core::SelectMagFusion()
} else {
// if we are not doing aiding with earth relative observations (eg GPS) then the declination is
// maintained by fusing declination as a synthesised observation
if (PV_AidingMode != AID_ABSOLUTE) {
if (PV_AidingMode != AID_ABSOLUTE || (imuSampleTime_ms - lastPosPassTime_ms) > 4000) {
FuseDeclination();
}
// fuse the three magnetometer componenents sequentially

7
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -305,7 +305,7 @@ void NavEKF2_core::FuseVelPosNED()
if (PV_AidingMode == AID_ABSOLUTE) {
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
if (posTimeout || ((varInnovVelPos[3] + varInnovVelPos[4]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
gpsPosGlitchOffsetNE.x += innovVelPos[3];
gpsPosGlitchOffsetNE.y += innovVelPos[4];
// limit the radius of the offset and decay the offset to zero radially
@ -316,6 +316,11 @@ void NavEKF2_core::FuseVelPosNED()
ResetVelocity();
// don't fuse data on this time step
fusePosData = false;
// Reset the position variances and corresponding covariances to a value that will pass the checks
zeroRows(P,6,7);
zeroCols(P,6,7);
P[6][6] = sq(float(0.5f*frontend._gpsGlitchRadiusMax));
P[7][7] = P[6][6];
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
velTestRatio = 0.0f;