From 1e7ac873b945aa2d01c5ea4338ec1959d70b4797 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 23 Oct 2015 23:44:43 +1100 Subject: [PATCH] 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. --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index ba15c89ecc..5cb0df1fa3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 05bc2913c3..1530039859 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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;