From 172f4be594a0b33997a07f5b46899c141a6cee86 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 21:17:11 +1000 Subject: [PATCH] EKF: Fix bug in calculation of terrain observation variance --- EKF/terrain_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index f812ad2110..cca1d2b45f 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -99,7 +99,7 @@ void Ekf::fuseHagl() _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[8][8], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2)); + float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2)); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);