fix(ekf2): don't reset yaw to an inaccurate GNSS heading (#28846)

Nothing checked the reported heading accuracy before GNSS yaw fusion started, and the reset always claimed the default heading noise, so a receiver still resolving its baseline could align yaw to a heading it reported with a 172 deg accuracy.

Assisted-by: Claude:claude-opus-5-5

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -88,9 +88,16 @@ void Ekf::controlGnssYawFusion(const imuSample &imu_delayed)
 		_time_last_gnss_yaw_fail_us = _time_delayed_us;
 	}
 
+	// A receiver still resolving its baseline can report a heading with an accuracy of tens of degrees. Starting may
+	// reset yaw to it, so hold it to the bar the yaw estimator must meet before a reset. Not every receiver reports an
+	// accuracy.
+	const bool is_gnss_yaw_accurate = !PX4_ISFINITE(gnss_yaw_sample.yaw_acc)
+					  || (gnss_yaw_sample.yaw_acc < _params.EKFGSF_yaw_err_max);
+
 	const bool starting_conditions_passing = continuing_conditions_passing
 			&& !is_gnss_yaw_data_intermittent
-			&& !is_heading_receiver_flagged;
+			&& !is_heading_receiver_flagged
+			&& is_gnss_yaw_accurate;
 
 	if (_control_status.flags.gnss_yaw) {
 		if (continuing_conditions_passing) {
@@ -247,7 +254,9 @@ bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset)
 	// the sensors module has already rotated the GNSS yaw measurement from the baseline into the body frame
 	const float measured_yaw = gnss_yaw;
 
-	const float yaw_variance = sq(fmaxf(_params.gnss_heading_noise, 1.e-2f));
+	// Take the variance updateGnssYaw() derived from this sample's reported accuracy, so the reset is no more confident
+	// than the measurement
+	const float yaw_variance = fmaxf(_aid_src_gnss_yaw.observation_variance, sq(1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance);
 
 	return true;

src/modules/ekf2/test/test_EKF_gnss_yaw.cpp

@@ -447,6 +447,34 @@ TEST_F(EkfGpsHeadingTest, unusableHeading)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 }
 
+TEST_F(EkfGpsHeadingTest, inaccurateHeadingDoesNotReset)
+{
+	// GIVEN: a receiver still resolving its baseline, reporting a heading 30 deg off with a 1 rad accuracy
+	const float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(30.f));
+	_sensor_simulator._gnss_yaw.setYaw(gps_heading);
+	_sensor_simulator._gnss_yaw.setYawAccuracy(1.f);
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: GNSS yaw fusion doesn't start, so yaw isn't reset to it
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+
+	// WHEN: the reported accuracy drops below 15 deg
+	const float yaw_acc = math::radians(10.f);
+	_sensor_simulator._gnss_yaw.setYawAccuracy(yaw_acc);
+
+	for (int i = 0; (i < 100) && (_ekf_wrapper.getQuaternionResetCounter() == initial_quat_reset_counter); i++) {
+		_sensor_simulator.runMicroseconds(10000);
+	}
+
+	// THEN: fusion starts with a reset to the heading, as uncertain as the reported accuracy
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+	checkConvergence(gps_heading, 0.5f);
+	EXPECT_NEAR(_ekf->getYawVar(), yaw_acc * yaw_acc, 0.1f * yaw_acc * yaw_acc);
+}
+
 TEST_F(EkfGpsHeadingTest, fusesAtOwnRate)
 {
 	// GIVEN: GNSS yaw fusion active with heading arriving faster than position