Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Prevents aliasing triggering innovation consistency check failures

Browse Source
This commit is contained in:
Paul Riseborough 2014-02-24 19:56:20 +11:00 committed by Andrew Tridgell
parent 8a4289b56f
commit 72a91ccbca
1 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1467,6 +1467,23 @@ void NavEKF::FuseVelPosNED()
R_OBS[4] = R_OBS[3];
R_OBS[5] = hgtVarScaler*sq(constrain_float(_baroAltNoise, 0.1f, 10.0f));
// 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 && fuseHgtData) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = statesAtVelTime[9] - observation[5];
float velDErr = statesAtVelTime[6] - observation[2];
//check if they are the same sign and both more than 2-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 4.0f * (P[9][9] + R_OBS[5])) && (sq(velDErr) > 4.0f * (P[6][6] + R_OBS[2]))) {
badIMUdata = true;
} else {
badIMUdata = false;
}
}
// calculate innovations and check GPS data validity using an innovation consistency check
if (fuseVelData)
{
@ -1479,8 +1496,8 @@ void NavEKF::FuseVelPosNED()
velInnov[i] = statesAtVelTime[stateIndex] - observation[i];
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
}
// apply an innovation consistency threshold test
velHealth = ((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2])));
// apply an innovation consistency threshold test, but don't fail if bad IMU data
velHealth = !((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) > (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2])) && !badIMUdata);
velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
if (velHealth || velTimeout || staticMode)
{
@ -1506,8 +1523,8 @@ void NavEKF::FuseVelPosNED()
posInnov[1] = statesAtPosTime[8] - observation[4];
varInnovVelPos[3] = P[7][7] + R_OBS[3];
varInnovVelPos[4] = P[8][8] + R_OBS[4];
// apply an innovation consistency threshold test
posHealth = ((sq(posInnov[0]) + sq(posInnov[1])) < (sq(_gpsPosInnovGate) * (varInnovVelPos[3] + varInnovVelPos[4])));
// apply an innovation consistency threshold test, but don't fail if bad IMU data
posHealth = !((sq(posInnov[0]) + sq(posInnov[1])) > (sq(_gpsPosInnovGate) * (varInnovVelPos[3] + varInnovVelPos[4])) && !badIMUdata);
posTimeout = (hal.scheduler->millis() - posFailTime) > gpsRetryTime;
// Fuse position data if healthy or timed out or in static mode
if (posHealth || posTimeout || staticMode)
@ -1537,8 +1554,8 @@ void NavEKF::FuseVelPosNED()
// calculate height innovations
hgtInnov = statesAtHgtTime[9] - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS[5];
// apply an innovation consistency threshold test
hgtHealth = (sq(hgtInnov) < (sq(_hgtInnovGate) * varInnovVelPos[5]));
// apply an innovation consistency threshold test, but don't fail if bad IMU data
hgtHealth = !(sq(hgtInnov) > (sq(_hgtInnovGate) * varInnovVelPos[5]) && !badIMUdata);
hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime;
// Fuse height data if healthy or timed out or in static mode
if (hgtHealth || hgtTimeout || staticMode)