fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Parameterize maximum angle for rng fusion

Browse Source
This commit is contained in:
CarlOlsson
2017-09-26 20:53:48 +02:00
parent 160e4d69c1
commit 0a7c3ecbc6
4 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+2 -2
View File
@@ -47,7 +47,7 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > 0.7071f) {
if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
// initialise state variance to variance of measurement
@@ -113,7 +113,7 @@ void Ekf::runTerrainEstimator()
void Ekf::fuseHagl()
{
// If the vehicle is excessively tilted, do not try to fuse range finder observations
if (_R_rng_to_earth_2_2 > 0.7071f) {
if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// get a height above ground measurement from the range finder assuming a flat earth
float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;