From c9eea98142ee112c8e91a6132b8956cf93ea27fd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Nov 2015 19:11:56 +1100 Subject: [PATCH] AP_NavEKF2: Improve terrain height estimation The ad-hoc scaling of error growth has been replaced with a consistent method that uses the main nav filters published vertical velocity uncertainty and the terrain gradient assumption. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 7a783a3019..c491131994 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -450,7 +450,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : flowIntervalMax_ms(100), // maximum allowable time between flow fusion events gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect - gndGradientSigma(2), // RMS terrain gradient percentage assumed by the terrain height estimation + gndGradientSigma(50), // RMS terrain gradient percentage assumed by the terrain height estimation fusionTimeStep_ms(10) // The minimum number of msec between covariance prediction and fusion operations { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index e796bc681e..f78717e94a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -121,9 +121,9 @@ void NavEKF2_core::EstimateTerrainOffset() prevPosN = stateStruct.position[0]; prevPosE = stateStruct.position[1]; - // in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter + // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); - float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(float(frontend->gndGradientSigma) * timeLapsed); + float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[5][5]; Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms;