From f32fce28a31af293d08372e7e89c35bab01b3e4b Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 3 Dec 2019 12:57:25 +0100 Subject: [PATCH] Fix gps vertical observation variance --- EKF/control.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index dad5b03d32..3781d5eae0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -718,6 +718,7 @@ void Ekf::controlGpsFusion() } 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)); + gps_vel_obs_var(2) = sq(1.5f) * gps_vel_obs_var(2); // calculate innovations _gps_vel_innov(0) = _state.vel(0) - _gps_sample_delayed.vel(0);