AP_NavEKF3: Add tuning of visual odometry observation error

Also adjust default values based on replay analysis.

libraries/AP_NavEKF3/AP_NavEKF3.cpp

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

libraries/AP_NavEKF3/AP_NavEKF3.h

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

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

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