From d94068b88a0cd9ec74b44ef681a47d5a2eecd88b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 30 Nov 2016 23:22:31 +1100 Subject: [PATCH] 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. --- EKF/terrain_estimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index e67dafa64c..b5748adc46 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -98,8 +98,8 @@ void Ekf::fuseHagl() // calculate the innovation _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 - 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; + // 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); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);