From d38eb8f5b349f033ddefe0ad5cf99c7fe6b5867a Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 17 Sep 2026 15:30:08 +0200 Subject: [PATCH] fix(ekf2): update GSF composite yaw when predicting The composite yaw and variance were only recomputed when GNSS velocity was fused, freezing them when the speed accuracy was too poor while the models kept tracking the gyro, which risks an emergency yaw reset to a stale heading. Also reset the estimator after 60 s without fusion. Assisted-by: Claude:claude-opus-5 Signed-off-by: bresch --- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 57 ++++++++----- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.h | 6 ++ .../ekf2/test/test_EKF_yaw_estimator.cpp | 85 +++++++++++++++++++ 3 files changed, 129 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 52fe8c04b55..428fa4583fa 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -59,6 +59,7 @@ EKFGSF_yaw::EKFGSF_yaw() void EKFGSF_yaw::reset() { _ekf_gsf_vel_fuse_started = false; + _time_since_last_vel_fusion = 0.f; _gsf_yaw_variance = INFINITY; } @@ -102,10 +103,23 @@ void EKFGSF_yaw::predict(const matrix::Vector3f &delta_ang, const float delta_an for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { predictEKF(model_index, delta_ang, delta_ang_dt, delta_vel, delta_vel_dt, in_air); } + + if (_ekf_gsf_vel_fuse_started) { + _time_since_last_vel_fusion += delta_vel_dt; + + if (_time_since_last_vel_fusion > kVelFusionTimeout) { + reset(); + + } else { + updateComposite(); + } + } } void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const bool in_air) { + _time_since_last_vel_fusion = 0.f; + // we don't start running the EKF part of the algorithm until there are regular velocity observations if (!_ekf_gsf_vel_fuse_started) { @@ -147,30 +161,35 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, _model_weights /= total_weight; } - // Calculate a composite yaw vector as a weighted average of the states for each model. - // To avoid issues with angle wrapping, the yaw state is converted to a vector with length - // equal to the weighting value before it is summed. - Vector2f yaw_vector; + updateComposite(); + } +} - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2)); - yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2)); - } +void EKFGSF_yaw::updateComposite() +{ + // Calculate a composite yaw vector as a weighted average of the states for each model. + // To avoid issues with angle wrapping, the yaw state is converted to a vector with length + // equal to the weighting value before it is summed. + Vector2f yaw_vector; - _gsf_yaw = atan2f(yaw_vector(1), yaw_vector(0)); + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { + yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2)); + yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2)); + } - // calculate a composite variance for the yaw state from a weighted average of the variance for each model - // models with larger innovations are weighted less - _gsf_yaw_variance = 0.0f; + _gsf_yaw = atan2f(yaw_vector(1), yaw_vector(0)); - for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); - _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); - } + // calculate a composite variance for the yaw state from a weighted average of the variance for each model + // models with larger innovations are weighted less + _gsf_yaw_variance = 0.0f; - if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { - reset(); - } + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { + const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); + _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); + } + + if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { + reset(); } } diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index db1e74d5fd7..5af54e7ff0e 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h @@ -125,6 +125,10 @@ private: bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active + // the yaw estimate is only observable through velocity fusion; without it the models dead-reckon on gyro only + static constexpr float kVelFusionTimeout{60.f}; // (sec) + float _time_since_last_vel_fusion{}; // (sec) + // initialise states and covariance data for the GSF and EKF filters void initialiseEKFGSF(const matrix::Vector2f &vel_NE, const float vel_accuracy); @@ -136,6 +140,8 @@ private: // return false if update failed bool updateEKF(const uint8_t model_index, const matrix::Vector2f &vel_NE, const float vel_accuracy); + void updateComposite(); + inline float sq(float x) const { return x * x; }; // Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 1d1fdfb6ce8..570bc1dc323 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -113,3 +113,88 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); } + +TEST_F(EKFYawEstimatorTest, freeRunningYawAfterVelocityFusionStops) +{ + const float yaw = math::radians(-130.f); + _sensor_simulator.setOrientation(Dcmf{Eulerf(0.f, 0.f, yaw)}); + _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f)); + _ekf->set_in_air_status(true); + _sensor_simulator.runTrajectorySeconds(3.f); + + float yaw_est{}; + float yaw_est_var{}; + float model_yaw[5]; + float innov_vn[5]; + float innov_ve[5]; + float weight[5]; + EXPECT_TRUE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, model_yaw, innov_vn, innov_ve, weight)); + EXPECT_NEAR(yaw_est, yaw, math::radians(5.f)); + + const float yaw_est_var_before = yaw_est_var; + const float innov_vn_before = innov_vn[0]; + + // GIVEN: a GNSS speed accuracy too poor for the yaw estimator to fuse velocity + gnssSample gnss = _sensor_simulator._gps.getData(); + gnss.sacc = 5.f; + _sensor_simulator._gps.setData(gnss); + + // WHEN: the vehicle yaws + const float yaw_rate = math::radians(30.f); + const float duration = 3.f; + _sensor_simulator.setImuBias(Vector3f{}, Vector3f(0.f, 0.f, yaw_rate)); + _sensor_simulator.runSeconds(duration); + + EXPECT_TRUE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, model_yaw, innov_vn, innov_ve, weight)); + + EXPECT_FLOAT_EQ(innov_vn[0], innov_vn_before); + + // THEN: the composite yaw follows the rotation and its variance grows + EXPECT_NEAR(yaw_est, wrap_pi(yaw + yaw_rate * duration), math::radians(5.f)); + EXPECT_GT(yaw_est_var, yaw_est_var_before); +} + +TEST_F(EKFYawEstimatorTest, resetAfterVelocityFusionTimeout) +{ + const float yaw = math::radians(-130.f); + _sensor_simulator.setOrientation(Dcmf{Eulerf(0.f, 0.f, yaw)}); + _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, 0.f)); + _ekf->set_in_air_status(true); + _sensor_simulator.runTrajectorySeconds(3.f); + + float yaw_est{}; + float yaw_est_var{}; + float dummy[5]; + EXPECT_TRUE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, dummy, dummy, dummy, dummy)); + + // GIVEN: a GNSS speed accuracy too poor for the yaw estimator to fuse velocity + gnssSample gnss = _sensor_simulator._gps.getData(); + gnss.sacc = 5.f; + _sensor_simulator._gps.setData(gnss); + + // WHEN: the estimator dead-reckons for just under the timeout + _sensor_simulator.runTrajectorySeconds(59.f); + + // THEN: it is still running + EXPECT_TRUE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, dummy, dummy, dummy, dummy)); + + _sensor_simulator.runTrajectorySeconds(2.f); + + // AND WHEN: the timeout expires, the estimate is invalidated + EXPECT_FALSE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, dummy, dummy, dummy, dummy)); + + // while GNSS is still being used, so the reset came from the timeout and not from stopping GNSS fusion + EXPECT_TRUE(_ekf->control_status_flags().gnss_pos); + + // AND WHEN: the speed accuracy recovers and the vehicle accelerates again + gnss = _sensor_simulator._gps.getData(); + gnss.sacc = 0.2f; + _sensor_simulator._gps.setData(gnss); + _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(-2.f, 2.f, 0.f)); + _sensor_simulator.runTrajectorySeconds(5.f); + + // THEN: the estimator restarts and re-converges to the true heading + EXPECT_TRUE(_ekf->getDataEKFGSF(&yaw_est, &yaw_est_var, dummy, dummy, dummy, dummy)); + EXPECT_NEAR(yaw_est, yaw, math::radians(5.f)); + EXPECT_LT(yaw_est_var, sq(math::radians(15.f))); +}