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

EKF: Simplify calculation of height above terrain observation variance 

The tilt compensation being applied previously was based on a flat earth geometric model assuming perfect tilt knowledge which reduces the effect of range errors on height error as the vehicle tilts. however in the real world, variations in terrain gradient and uncertainty in vehicle tilt and sensor alignment tend to increase height error with tilt, so the adjustment of observation variance with tilt has been removed given we do not have a valid mathematical model on which to base it.
This commit is contained in:
Paul Riseborough 2016-11-30 23:22:31 +11:00 committed by Roman
parent 3dadc98b43
commit d94068b88a
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/terrain_estimator.cpp
View File

@ -98,8 +98,8 @@ void Ekf::fuseHagl()
// calculate the innovation // calculate the innovation
_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 and factoring in the effect of tilt on measurement error // 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)) * _R_rng_to_earth_2_2; float obs_variance = fmaxf(P[9][9], 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);