From 5465a2184e41e71ded83c1e4ce9e4b8a5bb8ceda Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Tue, 3 Oct 2023 16:05:35 +0200 Subject: [PATCH] ekf2: set uncertainty of global origin to 0 when fusing GNSS data When global measurements are fused in the filter, the local position naturally contains the global position uncertainty. --- 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 4c620de17f..02e534eaaf 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; }