From 9c374eb4a8272c95146c36eba5b6014fd5b42d61 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 22 Apr 2015 11:21:22 -0700 Subject: [PATCH] AP_NavEKF: apply constrained floor to barometer innovation while landing --- libraries/AP_NavEKF/AP_NavEKF.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index d6b4f4b782..31d3d77122 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2100,6 +2100,7 @@ void NavEKF::FuseVelPosNED() if (fuseHgtData) { // calculate height innovations innovVelPos[5] = statesAtHgtTime.position.z - observation[5]; + varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(_hgtInnovGate) * varInnovVelPos[5]); @@ -2159,6 +2160,22 @@ void NavEKF::FuseVelPosNED() R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else { 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