Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Improve terrain height estimation

Browse Source 
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.
This commit is contained in:
Paul Riseborough 2015-11-12 19:11:56 +11:00 committed by Andrew Tridgell
parent d820a538d5
commit c9eea98142
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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);

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -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;