From a989e5338ce3902dd63e5da41d99931d8185fdf8 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Thu, 2 Nov 2023 14:24:35 +0100 Subject: [PATCH] ekf2: reset globlal position uncertainty when GNSS is fused There is no reason to keep an uncertainty on the origin as it is then already contained in the local position estimate when GNSS data is fused in the filter. --- src/modules/ekf2/EKF/gnss_height_control.cpp | 1 + src/modules/ekf2/EKF/gps_control.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 100d4150f1..430fe175c7 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -142,6 +142,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) _information_events.flags.reset_hgt_to_gps = true; resetVerticalPositionTo(-measurement, measurement_var); + _gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty bias_est.reset(); } else { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index a5671b111e..ef8a0e8d04 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -248,6 +248,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) // reset position _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(position, pos_obs_var); + _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; }