From 78e983073a87c853d7e248f6f69a053af8a9e383 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 28 Feb 2018 11:48:53 +1100 Subject: [PATCH] EKF: Fix bug in use of gps velocity noise parameter (#401) This fixes a bug introduced by an earlier feature request PR. The parameter is supposed to define the lower limit on the observation noise. --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f84d719429..c706dfec9f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -525,7 +525,7 @@ void Ekf::controlGpsFusion() float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); _posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); - _velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); + _velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); // calculate innovations _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);