Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
James Goppert
2016-07-11 16:02:39 -04:00
committed by GitHub
parent a740d80a20
commit 817f695297
3 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+4 -4
View File
@@ -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
+1 -1
View File
@@ -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
+4 -4
View File
@@ -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