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

AP_NavEKF: Reduce EKF health pre-arm check false positives

This commit is contained in:
Paul Riseborough 2015-10-30 08:21:47 +11:00 committed by Randy Mackay
parent 9e5ebd70b7
commit 58e81b7d99
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -455,7 +455,7 @@ bool NavEKF::healthy(void) const
}
// barometer and position innovations must be within limits when on-ground
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (!vehicleArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
if (!vehicleArmed && (!hgtHealth || fabsf(hgtInnovFiltState) > 1.0f || horizErrSq > 2.0f)) {
return false;
}
@ -2121,17 +2121,26 @@ void NavEKF::FuseVelPosNED()
if (fuseHgtData) {
// calculate height innovations
innovVelPos[5] = statesAtHgtTime.position.z - observation[5];
// calculate the innovation variance
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio
hgtTestRatio = sq(innovVelPos[5]) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime) > hgtRetryTime;
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime;
// Fuse height data if healthy or timed out or in constant position mode
if (hgtHealth || hgtTimeout || constPosMode) {
// Calculate a filtered value to be used by pre-flight health checks
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
if (!vehicleArmed) {
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
const float hgtInnovFiltTC = 2.0f;
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
}
// declare height healthy and able to be fused
lastHgtPassTime_ms = imuSampleTime_ms;
hgtHealth = true;
lastHgtPassTime = imuSampleTime_ms;
// if timed out, reset the height, but do not fuse data on this time step
if (hgtTimeout) {
ResetHeight();
@ -4674,7 +4683,7 @@ void NavEKF::InitialiseVariables()
lastVelPassTime = imuSampleTime_ms;
lastPosPassTime = imuSampleTime_ms;
lastPosFailTime = 0;
lastHgtPassTime = imuSampleTime_ms;
lastHgtPassTime_ms = imuSampleTime_ms;
lastTasPassTime = imuSampleTime_ms;
lastStateStoreTime_ms = imuSampleTime_ms;
lastFixTime_ms = 0;
@ -4778,6 +4787,7 @@ void NavEKF::InitialiseVariables()
imuNoiseFiltState1 = 0.0f;
imuNoiseFiltState2 = 0.0f;
lastImuSwitchState = IMUSWITCH_MIXED;
hgtInnovFiltState = 0.0f;
}
// return true if we should use the airspeed sensor

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -634,7 +634,7 @@ private:
uint32_t lastVelPassTime; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastPosFailTime; // time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_t lastHgtPassTime; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint8_t storeIndex; // State vector storage index
uint32_t lastStateStoreTime_ms; // time of last state vector storage
@ -687,6 +687,7 @@ private:
bool yawResetAngleWaiting; // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
bool consistentMagData; // true when the magnetometers are passing consistency checks
float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement