From 9dc7719d4a8c989459ff57a7843fe9d86cb25eab Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 26 Apr 2024 09:06:31 +0200 Subject: [PATCH] ekf2: Only reset to GNSS heading if necessary When North-East (e.g.: GNSS pos/vel) aiding is active, the heading estimate is constrained and consistent with the vel/pos aiding. Reset to GNSS heading should only occur if no N-E aiding is active or if the filter is not yes aligned. Otherwise, just wait for the consistency check to pass again (will pass at some point if the heading uncertainty of the filter is getting too high). --- src/modules/ekf2/EKF/ekf.h | 1 - src/modules/ekf2/EKF/gps_control.cpp | 46 +++++++++------------- src/modules/ekf2/test/test_EKF_gps_yaw.cpp | 22 +++++------ 3 files changed, 30 insertions(+), 39 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b0ca385d90..fd3cdbd18c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -672,7 +672,6 @@ private: # if defined(CONFIG_EKF2_GNSS_YAW) estimator_aid_source1d_s _aid_src_gnss_yaw{}; - uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index d8a6b26522..a88bb5739c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -358,7 +358,6 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) && !_gps_intermittent; if (_control_status.flags.gps_yaw) { - if (continuing_conditions_passing) { fuseGpsYaw(gps_sample.yaw_offset); @@ -366,27 +365,7 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); if (is_fusion_failing) { - if (_nb_gps_yaw_reset_available > 0) { - // Data seems good, attempt a reset - resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset); - - if (_control_status.flags.in_air) { - _nb_gps_yaw_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - _control_status.flags.gps_yaw_fault = true; - stopGpsYawFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - stopGpsYawFusion(); - } - - // TODO: should we give a new reset credit when the fusion does not fail for some time? + stopGpsYawFusion(); } } else { @@ -397,14 +376,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) } else { if (starting_conditions_passing) { // Try to activate GPS yaw fusion - if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { - ECL_INFO("starting GPS yaw fusion"); + const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + if (!_control_status.flags.in_air + || !_control_status.flags.yaw_align + || not_using_ne_aiding) { + + // Reset before starting the fusion + if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { + _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + _control_status.flags.gps_yaw = true; + _control_status.flags.yaw_align = true; + } + + } else if (!_aid_src_gnss_yaw.innovation_rejected) { + // Do not force a reset but wait for the consistency check to pass _control_status.flags.gps_yaw = true; - _control_status.flags.yaw_align = true; + fuseGpsYaw(gps_sample.yaw_offset); + } - _nb_gps_yaw_reset_available = 1; + if (_control_status.flags.gps_yaw) { + ECL_INFO("starting GPS yaw fusion"); } } } diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index 2c3afc655a..9fd8c4b179 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -275,9 +275,15 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(8); - // THEN: the fusion should reset - EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + // THEN: the fusion should stop, reset to mag + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + + // AND THEN: restart GNSS yaw fusion + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f)); } @@ -285,7 +291,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) { // GIVEN: the GPS yaw fusion activated float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f)); _sensor_simulator.runSeconds(5); _ekf->set_in_air_status(true); @@ -295,20 +301,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(7.5); - // THEN: the fusion should reset - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); - - // BUT WHEN: the measurement jumps a 2nd time - gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f)); - _sensor_simulator._gps.setYaw(gps_heading); - _sensor_simulator.runSeconds(7.5); + // THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); // THEN: after a few seconds, the fusion should stop and // the estimator doesn't fall back to mag fusion because it has // been declared inconsistent with the filter states EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent()); - //TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop? EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); }