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

AP_NavEKF: Rationalise health status reporting 

1) Un-used public methods to report height and position drifting have been removed
2) A time-out has been added to the airspeed innovation consistency check so that if we are relying on airspeed to constrain velocity drift, a filter divergence or other fault that causes the airspeed to be continually rejected will trigger a change in health status.
3) A timeout of velocity, position or height measurements does not cause a filter fault to be reported. Timeouts can be due to sensor errors and do not necessarily indicate that the filter has failed.
4) Time-outs of various measurements are used to present a consolidated bitmask which inidicates which parts of the solution can be used, eg attitude, height, velocity, relative position, absolute position, etc.
This commit is contained in:
priseborough 2014-12-19 20:58:10 +11:00 committed by Randy Mackay
parent 78b30e4ce5
commit 4eb19c2324
2 changed files with 48 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -400,6 +400,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec)
_hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec)
_hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec)
_tasRetryTime = 5000; // True airspeed timeout and retry interval (msec)
_magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
_magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate
_gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground
@ -442,31 +443,10 @@ bool NavEKF::healthy(void) const
return false;
}
// If measurements have failed innovation consistency checks for long enough to time-out
// and force fusion then the nav solution can be conidered to be unhealthy
// This will only be set as a transient
if (_fallback && (posTimeout || velTimeout || hgtTimeout)) {
return false;
}
// all OK
return true;
}
// return true if filter is dead-reckoning height
bool NavEKF::HeightDrifting(void) const
{
// Set to true if height measurements are failing the innovation consistency check
return !hgtHealth;
}
// return true if filter is dead-reckoning position
bool NavEKF::PositionDrifting(void) const
{
// Set to true if position measurements are failing the innovation consistency check
return !posHealth;
}
// resets position states to last GPS measurement or to zero if in static mode
void NavEKF::ResetPosition(void)
{
@ -789,6 +769,13 @@ void NavEKF::SelectVelPosFusion()
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
// If we haven't received GPS data for a while, then declare the position and velocity data as being timed out
if (imuSampleTime_ms - lastFixTime_ms > gpsRetryTimeout) {
posTimeout = true;
velTimeout = true;
}
// command fusion of GPS data and reset states as required
if (newDataGps) {
// reset data arrived flag
@ -814,13 +801,6 @@ void NavEKF::SelectVelPosFusion()
} else {
fuseVelData = 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 ) {
// in static mode use synthetic position measurements set to zero
@ -849,6 +829,13 @@ void NavEKF::SelectVelPosFusion()
// check for and read new height data
readHgtData();
// If we haven't received height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime = (_fusionModeGPS == 0 && !velTimeout) ? _hgtRetryTimeMode0 : _hgtRetryTimeMode12;
if (imuSampleTime_ms - lastHgtMeasTime > hgtRetryTime) {
hgtTimeout = true;
}
// command fusion of height data
if (newDataHgt)
{
@ -1007,6 +994,11 @@ void NavEKF::SelectTasFusion()
// get true airspeed measurement
readAirSpdData();
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
if (imuSampleTime_ms - lastAirspeedUpdate > _tasRetryTime) {
tasTimeout = true;
}
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then queue TAS fusion
tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas));
// if we have waited too long, set a timeout flag which will force fusion to occur
@ -1891,7 +1883,6 @@ void NavEKF::FuseVelPosNED()
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
bool badIMUdata = false;
if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * _msecHgtAvg)) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = statesAtHgtTime.position.z - observation[5];
@ -2003,10 +1994,6 @@ void NavEKF::FuseVelPosNED()
// test height measurements
if (fuseHgtData) {
// set the height data timeout depending on whether vertical velocity data is being used
uint32_t hgtRetryTime;
if (_fusionModeGPS == 0) hgtRetryTime = _hgtRetryTimeMode0;
else hgtRetryTime = _hgtRetryTimeMode12;
// calculate height innovations
hgtInnov = statesAtHgtTime.position.z - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS[5];
@ -3120,6 +3107,9 @@ void NavEKF::FuseAirspeed()
Vector22 H_TAS;
float VtasPred;
// health is set bad until test passed
tasHealth = false;
// copy required states to local variable names
vn = statesAtVtasMeasTime.velocity.x;
ve = statesAtVtasMeasTime.velocity.y;
@ -3196,9 +3186,17 @@ void NavEKF::FuseAirspeed()
// calculate the innovation consistency test ratio
tasTestRatio = sq(innovVtas) / (sq(_tasInnovGate) * varInnovVtas);
// test the ratio before fusing data
if (tasTestRatio < 1.0f)
// fail if the ratio is > 1, but don't fail if bad IMU data
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
tasTimeout = (imuSampleTime_ms - tasFailTime) > _tasRetryTime;
// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
if (tasHealth || (tasTimeout && posTimeout))
{
// restart the counter
tasFailTime = imuSampleTime_ms;
// correct the state vector
for (uint8_t j=0; j<=21; j++)
{
@ -4320,6 +4318,7 @@ void NavEKF::ZeroVariables()
velFailTime = imuSampleTime_ms;
posFailTime = imuSampleTime_ms;
hgtFailTime = imuSampleTime_ms;
tasFailTime = imuSampleTime_ms;
lastStateStoreTime_ms = imuSampleTime_ms;
lastFixTime_ms = imuSampleTime_ms;
secondLastFixTime_ms = imuSampleTime_ms;
@ -4334,7 +4333,9 @@ void NavEKF::ZeroVariables()
posTimeout = false;
hgtTimeout = false;
magTimeout = false;
tasTimeout = false;
badMag = false;
badIMUdata = false;
firstArmComplete = false;
finalMagYawInit = false;
storeIndex = 0;
@ -4486,7 +4487,7 @@ return filter timeout status as a bitmasked integer
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
5 = true airspeed measurement timeout
6 = unassigned
7 = unassigned
7 = unassigned
@ -4496,7 +4497,8 @@ void NavEKF::getFilterTimeouts(uint8_t &timeouts) const
timeouts = (posTimeout<<0 |
velTimeout<<1 |
hgtTimeout<<2 |
magTimeout<<3);
magTimeout<<3 |
tasTimeout<<4);
}
/*
@ -4513,10 +4515,10 @@ return filter function status as a bitmasked integer
void NavEKF::getFilterStatus(uint8_t &status) const
{
// add code to set bits using private filter data here
status = (!state.quat.is_nan()<<0 |
!(velTimeout && posTimeout)<<1 |
status = (!state.quat.is_nan()<<0 | // we can implement a better way to detect invalid attitude, but it is tricky to do reliably
!(velTimeout && tasTimeout)<<1 |
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
(gpsInhibitMode == 2 && useOptFlow())<<3 |
((gpsInhibitMode == 2 && useOptFlow()) || (!tasTimeout && assume_zero_sideslip()))<<3 |
!posTimeout<<4 |
!hgtTimeout<<5 |
!inhibitGndState<<6);

12
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -92,12 +92,6 @@ public:
// Check basic filter health metrics and return a consolidated health status
bool healthy(void) const;
// return true if filter is dead-reckoning height
bool HeightDrifting(void) const;
// return true if filter is dead-reckoning position
bool PositionDrifting(void) const;
// return the last calculated NED position relative to the reference point (m).
// return false if no position is available
bool getPosNED(Vector3f &pos) const;
@ -439,6 +433,7 @@ private:
AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec)
AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec)
AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec)
AP_Int16 _tasRetryTime; // true airspeed measurement retry time following innovation consistency fail (msec)
uint32_t _magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground
float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
@ -455,11 +450,14 @@ private:
bool posHealth; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth; // boolean true if height measurements have passed innovation consistency check
bool magHealth; // boolean true if magnetometer has passed innovation consistency check
bool tasHealth; // boolean true if true airspeed has passed innovation consistency check
bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badMag; // boolean true if the magnetometer is declared to be producing bad data
bool badIMUdata; // boolean true if the bad IMU data is detected
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector31 Kfusion; // Kalman gain vector
@ -538,9 +536,11 @@ private:
bool newDataHgt; // true when new height data has arrived
uint32_t lastHgtMeasTime; // time of last height measurement used to determine if new data has arrived
uint32_t lastHgtTime_ms; // time of last height update (msec) used to calculate timeout
uint16_t hgtRetryTime; // time allowed without use of height measurements before a height timeout is declared
uint32_t velFailTime; // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec)
uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec)
uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec)
uint32_t tasFailTime; // time stamp when airspeed measurement last failed covaraiance consistency check (msec)
uint8_t storeIndex; // State vector storage index
uint32_t lastStateStoreTime_ms; // time of last state vector storage
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived