Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(ekf2): update GSF composite yaw when predicting

Browse Source 
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 <brescianimathieu@gmail.com>
This commit is contained in:
bresch
2026-09-23 10:01:51 +02:00
committed by Mathieu Bresciani
parent e6d55568ae
commit d38eb8f5b3
3 changed files with 129 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+38 -19
View File
@@ -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();
}
}
src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h
+6
View File
@@ -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
src/modules/ekf2/test/test_EKF_yaw_estimator.cpp
+85
View File
@@ -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)));
}