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

AP_NavEKF: Improve robustness to long periods without GPS

This commit is contained in:
priseborough 2014-12-18 20:21:27 +11:00 committed by Andrew Tridgell
parent a44b4b5e87
commit f73816dbb5
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -77,7 +77,7 @@
#define ALT_NOISE_DEFAULT 0.5f #define ALT_NOISE_DEFAULT 0.5f
#define MAG_NOISE_DEFAULT 0.05f #define MAG_NOISE_DEFAULT 0.05f
#define GYRO_PNOISE_DEFAULT 0.015f #define GYRO_PNOISE_DEFAULT 0.015f
#define ACC_PNOISE_DEFAULT 0.25f #define ACC_PNOISE_DEFAULT 0.5f
#define GBIAS_PNOISE_DEFAULT 1E-06f #define GBIAS_PNOISE_DEFAULT 1E-06f
#define ABIAS_PNOISE_DEFAULT 0.0002f #define ABIAS_PNOISE_DEFAULT 0.0002f
#define MAGE_PNOISE_DEFAULT 0.0003f #define MAGE_PNOISE_DEFAULT 0.0003f
@ -396,7 +396,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_msecHgtDelay = 60; // Height measurement delay (msec) _msecHgtDelay = 60; // Height measurement delay (msec)
_msecMagDelay = 40; // Magnetometer measurement delay (msec) _msecMagDelay = 40; // Magnetometer measurement delay (msec)
_msecTasDelay = 240; // Airspeed measurement delay (msec) _msecTasDelay = 240; // Airspeed measurement delay (msec)
_gpsRetryTimeUseTAS = 20000; // GPS retry time with airspeed measurements (msec) _gpsRetryTimeUseTAS = 15000; // GPS retry time with airspeed measurements (msec)
_gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec) _gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec)
_hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec) _hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec)
_hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec) _hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec)
@ -787,6 +787,8 @@ void NavEKF::SelectVelPosFusion()
// check for and read new GPS data // check for and read new GPS data
readGpsData(); readGpsData();
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
// command fusion of GPS data and reset states as required // command fusion of GPS data and reset states as required
if (newDataGps) { if (newDataGps) {
// reset data arrived flag // reset data arrived flag
@ -800,7 +802,6 @@ void NavEKF::SelectVelPosFusion()
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
// If a long time since last GPS update, then reset position and velocity and reset stored state history // If a long time since last GPS update, then reset position and velocity and reset stored state history
uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) { if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) {
ResetPosition(); ResetPosition();
ResetVelocity(); ResetVelocity();
@ -813,6 +814,13 @@ void NavEKF::SelectVelPosFusion()
} else { } else {
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = false;
// If we haven't received GPS data for a while and are not using optical flow, then declare the EKF position and velocity output as unhealthy
if ((imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) && !(gpsInhibitMode == 2 && useOptFlow())) {
posHealth = false;
velHealth = false;
posTimeout = true;
velTimeout = true;
}
} }
} else if (staticMode ) { } else if (staticMode ) {
// in static mode use synthetic position measurements set to zero // in static mode use synthetic position measurements set to zero
@ -1768,11 +1776,11 @@ void NavEKF::CovariancePrediction()
nextP[i][i] = nextP[i][i] + processNoise[i]; nextP[i][i] = nextP[i][i] + processNoise[i];
} }
// if the total position variance exceeds 1E6 (1000m), then stop covariance // if the total position variance exceeds 1e4 (100m), then stop covariance
// growth by setting the predicted to the previous values // growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods // This prevent an ill conditioned matrix from occurring for long periods
// without GPS // without GPS
if ((P[7][7] + P[8][8]) > 1e6f) if ((P[7][7] + P[8][8]) > 1e4f)
{ {
for (uint8_t i=7; i<=8; i++) for (uint8_t i=7; i<=8; i++)
{ {