From 4434da03133bd22ce3902e4ca6990be5d6d11c3a Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 15 Jul 2025 09:20:33 +0200 Subject: [PATCH] ekf2-gnss: add reset mode This allows the user to choose whether the position should immediately be reset to GNSS on fusion timeout or if the EKF can continue with velocity dead-reckoning. --- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 181 ++++++++++++------ src/modules/ekf2/EKF/common.h | 8 + src/modules/ekf2/EKF/ekf.h | 3 +- src/modules/ekf2/EKF/estimator_interface.cpp | 11 ++ src/modules/ekf2/EKF/estimator_interface.h | 2 + src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/params_gnss.yaml | 10 + 8 files changed, 153 insertions(+), 64 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 442998ec49..65b49064ba 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -104,10 +104,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) bool do_vel_pos_reset = false; - if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { - // Reset checks need to run together, before each control function runs because the result would change - // after the first one runs - do_vel_pos_reset = shouldResetGpsFusion(); + if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) { if (_control_status.flags.in_air && isYawFailure() @@ -131,16 +128,17 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for if (_control_status.flags.gnss_vel) { if (continuing_conditions_passing) { - if (force_reset) { - ECL_WARN("GNSS fusion timeout, resetting"); - resetVelocityToGnss(aid_src); + fuseVelocity(aid_src); - } else { - fuseVelocity(aid_src); + const bool fusion_timeout = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); - if (isHeightResetRequired()) { - // reset vertical velocity if height is failing - resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]); + if (fusion_timeout || force_reset) { + if (isGnssVelResetAllowed() || force_reset) { + ECL_WARN("GNSS fusion timeout, resetting"); + resetVelocityToGnss(aid_src); + + } else { + stopGnssVelFusion(); } } @@ -150,19 +148,27 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for } else { if (starting_conditions_passing) { - ECL_INFO("starting GNSS velocity fusion"); - _information_events.flags.starting_gps_fusion = true; + bool fused = false; - // when already using another velocity source velocity reset is not necessary - if (!isHorizontalAidingActive() - || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - || !_control_status_prev.flags.yaw_align - ) { - // reset velocity - resetVelocityToGnss(aid_src); + const bool do_reset = force_reset || !_control_status_prev.flags.yaw_align; + + // Start fusing the data without reset if possible to avoid disturbing the filter + if (!do_reset && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) { + fused = fuseVelocity(aid_src); } - _control_status.flags.gnss_vel = true; + bool reset = false; + + if (!fused && (isGnssVelResetAllowed() || force_reset)) { + resetVelocityToGnss(aid_src); + reset = true; + } + + if (fused || reset) { + ECL_INFO("starting GNSS velocity fusion"); + _information_events.flags.starting_gps_fusion = true; + _control_status.flags.gnss_vel = true; + } } } } @@ -179,12 +185,19 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for if (_control_status.flags.gnss_pos) { if (continuing_conditions_passing) { - if (force_reset) { - ECL_WARN("GNSS fusion timeout, resetting"); - resetHorizontalPositionToGnss(aid_src); + fuseHorizontalPosition(aid_src); - } else { - fuseHorizontalPosition(aid_src); + const bool fusion_timeout = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); + + if (fusion_timeout || force_reset) { + if (isGnssPosResetAllowed()) { + ECL_WARN("GNSS fusion timeout, resetting"); + resetHorizontalPositionToGnss(aid_src); + + } else { + stopGnssPosFusion(); + _control_status.flags.gnss_fault = true; + } } } else { @@ -193,11 +206,30 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for } else { if (starting_conditions_passing) { - ECL_INFO("starting GNSS position fusion"); - _information_events.flags.starting_gps_fusion = true; + bool fused = false; - resetHorizontalPositionToGnss(aid_src); - _control_status.flags.gnss_pos = true; + const bool do_reset = force_reset || !_control_status_prev.flags.yaw_align; + + // Start fusing the data without reset if possible to avoid disturbing the filter + if (_local_origin_lat_lon.isInitialized() + && !do_reset + && ((aid_src.test_ratio[0] + aid_src.test_ratio[1]) < sq(0.5f))) { + fused = fuseHorizontalPosition(aid_src); + } + + bool reset = false; + + if (!fused && isGnssPosResetAllowed()) { + resetHorizontalPositionToGnss(aid_src); + reset = true; + } + + if (fused || reset) { + ECL_INFO("starting GNSS position fusion"); + _information_events.flags.starting_gps_fusion = true; + _control_status.flags.gnss_pos = true; + _control_status.flags.gnss_fault = false; + } } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { resetHorizontalPositionToGnss(aid_src); @@ -205,6 +237,61 @@ void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool for } } +bool Ekf::isGnssVelResetAllowed() const +{ + if (_control_status.flags.gnss_fault) { + return false; + } + + bool allowed = true; + + switch (static_cast(_params.ekf2_gps_mode)) { + case GnssMode::AUTO: + if (isOtherSourceOfHorizontalVelocityAidingThan(_control_status.flags.gnss_vel) + && !_control_status.flags.wind_dead_reckoning) { + allowed = false; + } + + break; + + case GnssMode::DEAD_RECKONING: + if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_vel)) { + allowed = false; + } + + break; + } + + return allowed; +} + +bool Ekf::isGnssPosResetAllowed() const +{ + if (_control_status.flags.gnss_fault) { + return false; + } + + bool allowed = true; + + switch (static_cast(_params.ekf2_gps_mode)) { + case GnssMode::AUTO: + if (isOtherSourceOfHorizontalPositionAidingThan(_control_status.flags.gnss_pos)) { + allowed = false; + } + + break; + + case GnssMode::DEAD_RECKONING: + if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) { + allowed = false; + } + + break; + } + + return allowed; +} + void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) { // correct velocity for offset relative to IMU @@ -358,38 +445,6 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) resetAidSourceStatusZeroInnovation(aid_src); } -bool Ekf::shouldResetGpsFusion() const -{ - /* We are relying on aiding to constrain drift so after a specified time - * with no aiding we need to do something - */ - bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - - if (has_horizontal_aiding_timed_out) { - // horizontal aiding hasn't timed out if optical flow still active - if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) { - has_horizontal_aiding_timed_out = false; - } - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - - const bool is_reset_required = has_horizontal_aiding_timed_out - || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max) - && (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::HPOS))); - - const bool is_inflight_nav_failure = _control_status.flags.in_air - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && (_time_last_hor_vel_fuse > _time_last_on_ground_us) - && (_time_last_hor_pos_fuse > _time_last_on_ground_us); - - return (is_reset_required || is_inflight_nav_failure); -} - void Ekf::stopGnssFusion() { if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 04f7409bfd..eae82ff78b 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -148,6 +148,11 @@ enum class GnssCtrl : uint8_t { YAW = (1 << 3) }; +enum class GnssMode : uint8_t { + AUTO = 0, ///< Reset on fusion timeout if no other source of position is available + DEAD_RECKONING = 1 ///< Reset on fusion timeout if no source of velocity is availabl +}; + enum class RngCtrl : uint8_t { DISABLED = 0, CONDITIONAL = 1, @@ -322,6 +327,7 @@ struct parameters { #if defined(CONFIG_EKF2_GNSS) int32_t ekf2_gps_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; + int32_t ekf2_gps_mode {static_cast(GnssMode::AUTO)}; float ekf2_gps_delay{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) @@ -599,6 +605,8 @@ uint64_t mag_heading_consistent : uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended +uint64_t gnss_fault : + 1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f6f88fceb1..c4aaed1cbf 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -861,11 +861,12 @@ private: void stopGnssPosFusion(); void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src); + bool isGnssVelResetAllowed() const; + bool isGnssPosResetAllowed() const; void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel); bool tryYawEmergencyReset(); void resetVelocityToGnss(estimator_aid_source3d_s &aid_src); void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src); - bool shouldResetGpsFusion() const; void controlGnssHeightFusion(const gnssSample &gps_sample); void stopGpsHgtFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 7acdc5505b..5c30c72afc 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -627,6 +627,17 @@ bool EstimatorInterface::isHorizontalPositionAidingActive() const return getNumberOfActiveHorizontalPositionAidingSources() > 0; } +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalVelocityAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalVelocityAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalVelocityAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalVelocityAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const { return int(_control_status.flags.gnss_vel) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 43e6dca48c..35597d3803 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -214,6 +214,7 @@ public: bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalVelocityAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -227,6 +228,7 @@ public: */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalVelocityAidingThan(bool aiding_flag) const; // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f70eca2b39..7a220f583c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -80,6 +80,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_noaid_noise(_params->ekf2_noaid_noise), #if defined(CONFIG_EKF2_GNSS) _param_ekf2_gps_ctrl(_params->ekf2_gps_ctrl), + _param_ekf2_gps_mode(_params->ekf2_gps_mode), _param_ekf2_gps_delay(_params->ekf2_gps_delay), _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index dcae0b0cf3..05a36e3036 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -513,6 +513,7 @@ private: #if defined(CONFIG_EKF2_GNSS) (ParamExtInt) _param_ekf2_gps_ctrl, + (ParamExtInt) _param_ekf2_gps_mode, (ParamExtFloat) _param_ekf2_gps_delay, (ParamExtFloat) _param_ekf2_gps_pos_x, diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index e21a8423e2..3fc87250c4 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -17,6 +17,16 @@ parameters: default: 7 min: 0 max: 15 + EKF2_GPS_MODE: + description: + short: Fusion reset mode + long: 'Automatic: reset on fusion timeout if no other source of position is available + Dead-reckoning: reset on fusion timeout if no source of velocity is available' + type: enum + values: + 0: Automatic + 1: Dead-reckoning + default: 0 EKF2_GPS_DELAY: description: short: GPS measurement delay relative to IMU measurements