mirror of https://github.com/ArduPilot/ardupilot
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:
parent
cd5ec3a3e0
commit
1e7ac873b9
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue