feat(ekf2): clear heading correlation with other states when not observable

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

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));
 		}
 	}

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]);

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;
 };

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;
 }

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));

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);

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;

src/modules/ekf2/EKF2.cpp

@@ -1950,6 +1950,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		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;