From 2df2dede2f3ffa5bca4ffc87042d858c8bb61e50 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Wed, 13 Nov 2019 09:53:31 +0100 Subject: [PATCH] Fix variance vs stdDev bug (cherry picked from commit 0faea89da2a3326d070a6ecca93ec52ec3e29591) --- EKF/control.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 61ba2450ff..afc699fe02 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -305,7 +305,7 @@ void Ekf::controlExternalVisionFusion() _ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); // observation 1-STD error, incremental pos observation is expected to have more uncertainty - ev_pos_obs_var(0) = ev_pos_obs_var(1) = fmaxf(_ev_sample_delayed.posErr, 0.5f); + ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.5f)); } // record observation and estimate for use next time @@ -322,7 +322,7 @@ void Ekf::controlExternalVisionFusion() _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); // observation 1-STD error - ev_pos_obs_var(0) = ev_pos_obs_var(1) = fmaxf(_ev_sample_delayed.posErr, 0.01f); + ev_pos_obs_var(0) = ev_pos_obs_var(1) = sq(fmaxf(_ev_sample_delayed.posErr, 0.01f)); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) { @@ -374,7 +374,7 @@ void Ekf::controlExternalVisionFusion() } // observation 1-STD error - ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = fmaxf(_ev_sample_delayed.velErr, 0.01f); + ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = sq(fmaxf(_ev_sample_delayed.velErr, 0.01f)); // innovation gate size ev_vel_innov_gates(0) = ev_vel_innov_gates(1) = fmaxf(_params.ev_vel_innov_gate, 1.0f); @@ -708,13 +708,13 @@ void Ekf::controlGpsFusion() if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { // if we are using other sources of aiding, then relax the upper observation // noise limit which prevents bad GPS perturbing the position estimate - gps_pos_obs_var(0) = gps_pos_obs_var(1) = fmaxf(_gps_sample_delayed.hacc, lower_limit); + gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit)); } else { // if we are not using another source of aiding, then we are reliant on the GPS // observations to constrain attitude errors and must limit the observation noise value. float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - gps_pos_obs_var(0) = gps_pos_obs_var(1) = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); + gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit)); } gps_vel_obs_var(0) = gps_vel_obs_var(1) = gps_vel_obs_var(2) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); @@ -1401,10 +1401,10 @@ void Ekf::controlFakePosFusion() _time_last_fake_pos = _time_last_imu; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - fake_pos_obs_var(0) = fake_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); } else { - fake_pos_obs_var(0) = fake_pos_obs_var(1) = 0.5f; + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f); } _gps_pos_innov(0) = _state.pos(0) - _last_known_posNE(0);