From 643c6fec24e4e2dab344adf25d749e85a7c629a7 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 9 Mar 2026 16:22:18 +0100 Subject: [PATCH] feat(ekf2): clear heading correlation with other states when not observable --- msg/EstimatorStatusFlags.msg | 1 + .../ekf2/EKF/aid_sources/drag/drag_fusion.cpp | 4 ---- .../EKF/aid_sources/gravity/gravity_fusion.cpp | 2 -- src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF/control.cpp | 4 ++++ src/modules/ekf2/EKF/covariance.cpp | 17 +++++++++++++++++ src/modules/ekf2/EKF/ekf.h | 2 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++++ src/modules/ekf2/EKF2.cpp | 1 + 9 files changed, 30 insertions(+), 7 deletions(-) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 6de62f2c06..1351be504a 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -52,6 +52,7 @@ bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion bool cs_gnss_fault # 45 - true if GNSS true if GNSS measurements (lat, lon, vel) have been declared faulty bool cs_yaw_manual # 46 - true if yaw has been set manually bool cs_gnss_hgt_fault # 47 - true if GNSS true if GNSS measurements (alt) have been declared faulty +bool cs_heading_observable # 49 - true when heading is observable # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index 6b3f37c1fd..de496a9fab 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -164,10 +164,6 @@ void Ekf::fuseDrag(const dragSample &drag_sample) VectorState K = P * H / innovation_variance(axis_index); - // As drag fusion is often biased (due to approximate model or changing wind), using it to update the heading estimate - // can lead to unwanted heading corrections during acceleration and deceleration phases. - K(State::quat_nominal.idx + 2) = 0.f; - measurementUpdate(K, H, R_ACC, innovation(axis_index)); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index ffa1fe31fd..fb0b3bc932 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -110,8 +110,6 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - // Prevent heading corrections through gravity fusion due to correlations between tilt and heading in P - K(State::quat_nominal.idx + 2) = 0.f; fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b4730e77d2..01b1784c16 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -594,7 +594,7 @@ uint64_t gnss_fault : uint64_t gnss_hgt_fault : 1; ///< 47 - true if GNSS measurements (alt) have been declared faulty and are no longer used uint64_t in_transition : 1; ///< 48 - true if the vehicle is in vtol transition - + uint64_t heading_observable : 1; ///< 49 - true when heading is observable } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 4fbb5a7545..0b20db7768 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -168,4 +168,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // check if we are no longer fusing measurements that directly constrain velocity drift updateDeadReckoningStatus(); + + const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D + || _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw; + _control_status.flags.heading_observable = isNorthEastAidingActive() || yaw_aiding; } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 85ce9b9c17..dd2ce97cf6 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -139,6 +139,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var, dt); + if (!_control_status.flags.heading_observable) { + // Zero heading correlations to prevent unintended heading corrections when heading is not observable + uncorrelateAndLimitHeadingCovariance(); + } + // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances @@ -250,6 +255,11 @@ void Ekf::constrainStateVariances() // belong to the same group (e.g. vel_x, vel_y, vel_z) constrainStateVar(State::quat_nominal, 1e-9f, 1.f); + + if (!_control_status.flags.heading_observable) { + uncorrelateAndLimitHeadingCovariance(); + } + constrainStateVar(State::vel, 1e-6f, 1e6f); constrainStateVar(State::pos, 1e-6f, 1e6f); constrainStateVarLimitRatio(State::gyro_bias, kGyroBiasVarianceMin, 1.f); @@ -311,6 +321,13 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, constrainStateVar(state, limited_min, limited_max); } +void Ekf::uncorrelateAndLimitHeadingCovariance() +{ + const float heading_var = P(State::quat_nominal.idx + 2, State::quat_nominal.idx + 2); + const float heading_var_max = sq(_params.ekf2_head_noise); + P.uncorrelateCovarianceSetVariance<1>(State::quat_nominal.idx + 2, fminf(heading_var, heading_var_max)); +} + void Ekf::resetQuatCov(const float yaw_noise) { const float tilt_var = sq(math::max(_params.ekf2_angerr_init, 0.01f)); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ef70ff3613..0bd7c7b9a0 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -823,6 +823,8 @@ private: void constrainStateVar(const IdxDof &state, float min, float max); void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f); + void uncorrelateAndLimitHeadingCovariance(); + // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value void fuse(const VectorState &K, float innovation); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index f5b8703c0d..e497ad2a52 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1232,6 +1232,10 @@ void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const { + if (!_control_status.flags.heading_observable) { + K(State::quat_nominal.idx + 2) = 0.f; + } + for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { K(State::gyro_bias.idx + i) = 0.f; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3d770d6ca2..2bb9063867 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1950,6 +1950,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_gnss_fault = _ekf.control_status_flags().gnss_fault; status_flags.cs_yaw_manual = _ekf.control_status_flags().yaw_manual; status_flags.cs_gnss_hgt_fault = _ekf.control_status_flags().gnss_hgt_fault; + status_flags.cs_heading_observable = _ekf.control_status_flags().heading_observable; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;