From 8a2e512c2d697f7b96b9694b3573215a9654dda1 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Thu, 28 Jun 2018 14:50:42 +0200 Subject: [PATCH] terrain_estimator: add vehicle_variance_scaler --- 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 0891492526..d54b9639ce 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -128,7 +128,7 @@ void Ekf::fuseHagl() _hagl_innov = pred_hagl - meas_hagl; // 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 _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);