dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

terrain_estimator: add vehicle_variance_scaler

This commit is contained in:
CarlOlsson 2018-06-28 14:50:42 +02:00 committed by Paul Riseborough
parent acde4ebcc3
commit 8a2e512c2d
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/terrain_estimator.cpp
View File

@ -128,7 +128,7 @@ void Ekf::fuseHagl()
_hagl_innov = pred_hagl - meas_hagl; _hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty // calculate the observation variance adding the variance of the vehicles own height uncertainty
float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng); float obs_variance = fmaxf(P[9][9] * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng);
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion // calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);