From 817f69529784a5ad48aa366d1c8794c781e4f2a8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 11 Jul 2016 16:02:39 -0400 Subject: [PATCH] Change to LPE terrain model to account for velocity scaling. (#5027) --- .../BlockLocalPositionEstimator.cpp | 8 ++++---- .../BlockLocalPositionEstimator.hpp | 2 +- src/modules/local_position_estimator/params.c | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 50f47a9621..89dae8cd99 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/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() diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index c541f53e03..abf858aa6a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/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; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 43e6988b97..361147235e 100644 --- a/src/modules/local_position_estimator/params.c +++ b/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