Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Reduce EKF health pre-arm check false positives

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-30 08:21:47 +11:00 committed by Randy Mackay
parent aa14de9d39
commit 59df23bc0f
2 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -466,7 +466,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;
}
@ -2169,17 +2169,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();
@ -4746,7 +4755,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;
@ -4862,6 +4871,7 @@ void NavEKF::InitialiseVariables()
delAngBiasAtArming.zero();
posResetNE.zero();
velResetNE.zero();
hgtInnovFiltState = 0.0f;
}
// return true if we should use the airspeed sensor

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -662,7 +662,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
@ -722,12 +722,12 @@ private:
uint32_t lastConstPosFuseTime_ms; // last time in msec the constant position constraint was applied
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate
Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming
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