From 6f2dec726adeedcb53aad9a77a8fcdfc8f7b3054 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 4 Mar 2021 11:35:12 +0100 Subject: [PATCH] gps sacc: apply same minimum for EKF2 and yaw estimator --- EKF/EKFGSF_yaw.cpp | 2 +- EKF/control.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 99c31339de..f382e9d246 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -293,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) bool EKFGSF_yaw::updateEKF(const uint8_t model_index) { // set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum - const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f)); + const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f)); // calculate velocity observation innovations _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0); diff --git a/EKF/control.cpp b/EKF/control.cpp index 2a354f56a5..1472d90e98 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -702,7 +702,9 @@ void Ekf::controlGpsFusion() gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit)); } - _last_vel_obs_var.setAll(sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise))); + _gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise); + + _last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc)); _last_vel_obs_var(2) *= sq(1.5f); // calculate innovations