From 3dadc98b43179179a31ae6bb55c0312983878a4f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 29 Nov 2016 09:21:15 +1100 Subject: [PATCH] EKF: enable scaling of range observation variance with height --- EKF/common.h | 2 ++ EKF/terrain_estimator.cpp | 2 +- EKF/vel_pos_fusion.cpp | 3 +-- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index acfd491a64..ee0d7a05e7 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -235,6 +235,7 @@ struct parameters { float range_innov_gate; // range finder fusion innovation consistency gate size (STD) float rng_gnd_clearance; // minimum valid value for range when on ground (m) float rng_sens_pitch; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. + float range_noise_scaler; // scaling from range measurement to noise (m/m) // vision position fusion float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD) @@ -339,6 +340,7 @@ struct parameters { range_innov_gate = 5.0f; rng_gnd_clearance = 0.1f; rng_sens_pitch = 0.0f; + range_noise_scaler = 0.0f; // vision position fusion ev_innov_gate = 5.0f; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 286475f04b..e67dafa64c 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[9][9], 0.0f) + sq(_params.range_noise / _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)) * _R_rng_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); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 7c3459de86..7a20ff4caa 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -161,8 +161,7 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, _params.rng_gnd_clearance)); // observation variance - user parameter defined - R[5] = fmaxf(_params.range_noise, 0.01f); - R[5] = R[5] * R[5]; + R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); } else if (_control_status.flags.ev_hgt) {