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

AP_NavEKF: apply constrained floor to barometer innovation while landing

This commit is contained in:
Jonathan Challinger 2015-04-22 11:21:22 -07:00 committed by Randy Mackay
parent 3c6446fadd
commit 9c374eb4a8
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2100,6 +2100,7 @@ void NavEKF::FuseVelPosNED()
if (fuseHgtData) { if (fuseHgtData) {
// calculate height innovations // calculate height innovations
innovVelPos[5] = statesAtHgtTime.position.z - observation[5]; innovVelPos[5] = statesAtHgtTime.position.z - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio // calculate the innovation consistency test ratio
hgtTestRatio = sq(innovVelPos[5]) / (sq(_hgtInnovGate) * varInnovVelPos[5]); hgtTestRatio = sq(innovVelPos[5]) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
@ -2159,6 +2160,22 @@ void NavEKF::FuseVelPosNED()
R_OBS[obsIndex] *= sq(gpsNoiseScaler); R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else { } else {
innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex]; innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
if (obsIndex == 5) {
static const float gndMaxBaroErr = 4.0f;
static const float gndBaroInnovFloor = -0.5f;
if(getTouchdownExpected()) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
// this function looks like this:
// |/
//---------|---------
// ____/|
// / |
// / |
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
}
}
} }
// calculate the Kalman gain and calculate innovation variances // calculate the Kalman gain and calculate innovation variances