fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 03:37:35 +08:00
Code Issues 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()