Change to LPE terrain model to account for velocity scaling. (#5027)

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -85,7 +85,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pn_p_noise_density(this, "PN_P"),
 	_pn_v_noise_density(this, "PN_V"),
 	_pn_b_noise_density(this, "PN_B"),
-	_pn_t_noise_density(this, "PN_T"),
+	_t_max_grade(this, "T_MAX_GRADE"),
 
 	// init home
 	_init_home_lat(this, "LAT"),
@@ -831,9 +831,9 @@ void BlockLocalPositionEstimator::updateSSParams()
 	_Q(X_by, X_by) = pn_b_sq;
 	_Q(X_bz, X_bz) = pn_b_sq;
 
-	// terrain random walk noise
-	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
-	_Q(X_tz, X_tz) = pn_t_sq;
+	// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
+	float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+	_Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev;
 }
 
 void BlockLocalPositionEstimator::predict()

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -290,7 +290,7 @@ private:
 	BlockParamFloat  _pn_p_noise_density;
 	BlockParamFloat  _pn_v_noise_density;
 	BlockParamFloat  _pn_b_noise_density;
-	BlockParamFloat  _pn_t_noise_density;
+	BlockParamFloat  _t_max_grade;
 
 	// init home
 	BlockParamFloat  _init_home_lat;

src/modules/local_position_estimator/params.c

@@ -286,15 +286,15 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
 
 /**
- * Terrain random walk noise density, hilly/outdoor (1e-1), flat/Indoor (1e-3)
+ * Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg)
  *
  * @group Local Position Estimator
- * @unit m/s/sqrt(Hz)
+ * @unit %
  * @min 0
- * @max 1
+ * @max 100
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-1f);
+PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
 
 /**
  * Flow gyro high pass filter cut off frequency