AP_NavEKF2: Allow for terrain gradient when using range finder for height

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -485,6 +485,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Units: %
     AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF2, _useRngSwHgt, -1),
 
+    // @Param: TERR_GRAD
+    // @DisplayName: Maximum terrain gradient
+    // @Description: Specifies the maxium gradient of the terrain below the vehicle when it is using range finder as a height reference
+    // @Range: 0 0.2
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("TERR_GRAD", 43, NavEKF2, _terrGradMax, 0.1f),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -340,6 +340,7 @@ private:
     AP_Int16 _yawInnovGate;         // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
     AP_Int8 _tauVelPosOutput;       // Time constant of output complementary filter : csec (centi-seconds)
     AP_Int8 _useRngSwHgt;           // Maximum valid range of the range finder in metres
+    AP_Float _terrGradMax;          // Maximum terrain gradient below the vehicle
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -719,6 +719,8 @@ void NavEKF2_core::selectHeightForFusion()
             fuseHgtData = true;
             // set the observation noise
             posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
+            // add uncertainty created by terrain gradient and vehicle tilt
+            posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
         } else {
             // disable fusion if tilted too far
             fuseHgtData = false;