ekf2: GNSS yaw, use reported yaw accuracy when available

src/modules/ekf2/EKF/common.h

@@ -155,6 +155,7 @@ struct gpsMessage {
 	int32_t     alt{};              ///< Altitude in 1E-3 meters (millimeters) above MSL
 	float       yaw{};              ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
 	float       yaw_offset{};       ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
+	float       yaw_accuracy{};	///< yaw measurement accuracy (rad, [0, 2PI])
 	uint8_t     fix_type{};         ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
 	float       eph{};              ///< GPS horizontal position accuracy in m
 	float       epv{};              ///< GPS vertical position accuracy in m
@@ -198,6 +199,7 @@ struct gpsSample {
 	float       hacc{};     ///< 1-std horizontal position error (m)
 	float       vacc{};     ///< 1-std vertical position error (m)
 	float       sacc{};     ///< 1-std speed error (m/sec)
+	float       yaw_acc{};  ///< 1-std yaw error (rad)
 };
 
 struct magSample {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -173,6 +173,13 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 			_gps_yaw_offset = 0.0f;
 		}
 
+		if (PX4_ISFINITE(gps.yaw_accuracy)) {
+			gps_sample_new.yaw_acc = gps.yaw_accuracy;
+
+		} else {
+			gps_sample_new.yaw_acc = 0.f;
+		}
+
 		// Only calculate the relative position if the WGS-84 location of the origin is set
 		if (collect_gps(gps)) {
 			gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -57,9 +57,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 		// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 		const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
 
-		// using magnetic heading process noise
+		const float R_YAW = sq(fmaxf(gps_sample.yaw_acc, _params.gps_heading_noise));
-		// TODO extend interface to use yaw uncertainty provided by GPS if available
-		const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
 
 		float heading_innov;
 		float heading_innov_var;

src/modules/ekf2/EKF2.cpp

@@ -1931,6 +1931,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.alt = vehicle_gps_position.alt,
 			.yaw = vehicle_gps_position.heading,
 			.yaw_offset = vehicle_gps_position.heading_offset,
+			.yaw_accuracy = vehicle_gps_position.heading_accuracy,
 			.fix_type = vehicle_gps_position.fix_type,
 			.eph = vehicle_gps_position.eph,
 			.epv = vehicle_gps_position.epv,