aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Add tuning of visual odometry observation error

Browse Source 
Also adjust default values based on replay analysis.
This commit is contained in:
priseborough 2017-07-27 14:51:17 +10:00 committed by Randy Mackay
parent eaf8aad5ad
commit 593437ca95
3 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -548,6 +548,25 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @RebootRequired: True
AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 0),
// @Param: VIS_VERR_MIN
// @DisplayName: Visual odometry minimum velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when maximum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
// @Range: 0.05 0.5
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VIS_VERR_MIN", 51, NavEKF3, _visOdmVelErrMin, 0.1f),
// @Param: VIS_VERR_MAX
// @DisplayName: Visual odometry maximum velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when minimum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VIS_VERR_MAX", 52, NavEKF3, _visOdmVelErrMax, 0.9f),
// @Param: WENC_VERR
AP_GROUPEND
};

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -395,6 +395,8 @@ private:
AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s)
AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
AP_Float _visOdmVelErrMax; // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
AP_Float _visOdmVelErrMin; // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -122,9 +122,7 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
// simple model of accuracy
// TODO move this calculation outside of EKF into the sensor driver
const float minVelErr = 0.5f;
const float maxVelErr = 10.0f;
bodyOdmDataNew.velErr = minVelErr + (maxVelErr - minVelErr) * (1.0f - 0.01f * quality);
bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);
storedBodyOdm.push(bodyOdmDataNew);