diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 423dc6130b57..f7c537135519 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -186,6 +186,7 @@ set(msg_files
 	vehicle_control_mode.msg
 	vehicle_global_position.msg
 	vehicle_gps_position.msg
+	vehicle_gnss_heading.msg
 	vehicle_imu.msg
 	vehicle_imu_status.msg
 	vehicle_land_detected.msg
diff --git a/msg/vehicle_gnss_heading.msg b/msg/vehicle_gnss_heading.msg
new file mode 100644
index 000000000000..e1dd4c47cbcb
--- /dev/null
+++ b/msg/vehicle_gnss_heading.msg
@@ -0,0 +1,5 @@
+uint64 timestamp                  # time since system start(microseconds)
+uint64 timestamp_sample           # time since system start(microseconds)
+
+float32 heading                   # Heading angle of XYZ body frame rel to NED.(radians)
+float32 heading_accuracy          # Accuracy of heading of the relative position vector(radians)
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index b107c43d1e05..322f728fc0bf 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -79,6 +79,12 @@ struct gps_message {
 	float pdop;		///< position dilution of precision
 };
 
+struct gnss_heading_message {
+	uint64_t time_usec{0};	///< timestamp of the measurement (uSec)
+	float heading;		///< heading angle in radians
+	float heading_accuracy;	///< heading accuracy in radians
+};
+
 struct outputSample {
 	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	Quatf  quat_nominal;	///< nominal quaternion describing vehicle attitude
@@ -112,6 +118,11 @@ struct gpsSample {
 	float	    vacc;	///< 1-std vertical position error (m)
 	float       sacc;	///< 1-std speed error (m/sec)
 };
+struct gnssHeadingSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	float heading;		///< heading angle in radians
+	float heading_accuracy;	///< heading accuracy in radians
+};
 
 struct magSample {
 	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
@@ -202,10 +213,11 @@ enum TerrainFusionMask : int32_t {
 #define MAG_FUSE_TYPE_NONE	5	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 
 // Maximum sensor intervals in usec
-#define GPS_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
-#define BARO_MAX_INTERVAL (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define RNG_MAX_INTERVAL  (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
+#define GPS_MAX_INTERVAL           (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
+#define GNSS_HEADING_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GNSS heading measurements (uSec)
+#define BARO_MAX_INTERVAL          (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define RNG_MAX_INTERVAL           (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
+#define EV_MAX_INTERVAL            (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
 
 // bad accelerometer detection and mitigation
 #define BADACC_PROBATION  (uint64_t)10e6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index 0682e1214ac7..29407b629c03 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -123,6 +123,12 @@ void Ekf::controlFusionModes()
 		}
 	}
 
+	if (_gnss_heading_buffer) {
+		// check for arrival of new sensor data at the fusion time horizon
+		_time_prev_gnss_heading_us = _gnss_heading_sample_delayed.time_us;
+		_gnss_heading_data_ready = _gnss_heading_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gnss_heading_sample_delayed);
+	}
+
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
 		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
@@ -179,6 +185,7 @@ void Ekf::controlFusionModes()
 	controlMagFusion();
 	controlOpticalFlowFusion();
 	controlGpsFusion();
+	controlGNSSHeadingFusion();
 	controlAirDataFusion();
 	controlBetaFusion();
 	controlDragFusion();
@@ -554,7 +561,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
 	_time_good_motion_us = _imu_sample_delayed.time_us;
 }
 
-void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
+void Ekf::controlGNSSHeadingFusion()
 {
 	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
 	    || _control_status.flags.gps_yaw_fault) {
@@ -563,29 +570,20 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 		return;
 	}
 
-	const bool is_new_data_available = PX4_ISFINITE(_gps_sample_delayed.yaw);
-
-	if (is_new_data_available) {
-
-		const bool continuing_conditions_passing = !gps_checks_failing;
+	if (_gnss_heading_data_ready) {
 
-		const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL);
+		const bool is_gnss_heading_data_intermittent = !isRecent(_time_last_gnss_heading_data, 2 * GNSS_HEADING_MAX_INTERVAL);
 
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _control_status.flags.tilt_align
-				&& gps_checks_passing
-				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
+		const bool starting_conditions_passing = _control_status.flags.tilt_align
+				&& !is_gnss_heading_data_intermittent;
 
-		_time_last_gps_yaw_data = _time_last_imu;
+		_time_last_gnss_heading_data = _time_last_imu;
 
 		if (_control_status.flags.gps_yaw) {
 
-			if (continuing_conditions_passing) {
-
 				fuseGpsYaw();
 
-				const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max);
+				const bool is_fusion_failing = isTimedOut(_time_last_gnss_heading_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {
@@ -611,10 +609,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 					// TODO: should we give a new reset credit when the fusion does not fail for some time?
 				}
 
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
 
 		} else {
 			if (starting_conditions_passing) {
@@ -627,7 +621,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 			}
 		}
 
-	} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) {
+	} else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gnss_heading_data, _params.reset_timeout_max)) {
 		// No yaw data in the message anymore. Stop until it comes back.
 		stopGpsYawFusion();
 	}
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 6eeb52fb1af8..3d25ff3b56ce 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -376,6 +376,7 @@ class Ekf final : public EstimatorInterface
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
+	bool _gnss_heading_data_ready{false};///< true when new GNSS heading data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
@@ -383,6 +384,7 @@ class Ekf final : public EstimatorInterface
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
+	uint64_t _time_prev_gnss_heading_us{0}; ///< time stamp of previous GNSS heading data retrieved from the buffer (uSec)
 	uint64_t _time_last_aiding{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
 	bool _using_synthetic_position{false};	///< true if we are using a synthetic position to constrain drift
 
@@ -396,8 +398,8 @@ class Ekf final : public EstimatorInterface
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_fake_pos_fuse{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
-	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint64_t _time_last_gnss_heading_fuse{0};	///< time the last fusion of GNSS heading measurements were performed (uSec)
+	uint64_t _time_last_gnss_heading_data{0};	///< time the last GNSS heading measurement was available (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
@@ -833,7 +835,7 @@ class Ekf final : public EstimatorInterface
 	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
 
-	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
+	void controlGNSSHeadingFusion();
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index e16f66551dac..4ab6a178e08d 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1559,10 +1559,6 @@ void Ekf::stopGpsFusion()
 		stopGpsVelFusion();
 	}
 
-	if (_control_status.flags.gps_yaw) {
-		stopGpsYawFusion();
-	}
-
 	// We do not need to know the true North anymore
 	// EV yaw can start again
 	_inhibit_ev_yaw_use = false;
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
index d94b2a076a61..8614f2b9f2b3 100644
--- a/src/modules/ekf2/EKF/estimator_interface.cpp
+++ b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -47,6 +47,7 @@
 EstimatorInterface::~EstimatorInterface()
 {
 	delete _gps_buffer;
+	delete _gnss_heading_buffer;
 	delete _mag_buffer;
 	delete _baro_buffer;
 	delete _range_buffer;
@@ -193,6 +194,41 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 	}
 }
 
+void EstimatorInterface::setGnssHeadingData(const gnss_heading_message &gnss_heading)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	if (_gnss_heading_buffer == nullptr) {
+		_gnss_heading_buffer = new RingBuffer<gnssHeadingSample>(_obs_buffer_length);
+
+		if (_gnss_heading_buffer == nullptr || !_gnss_heading_buffer->valid()) {
+			delete _gnss_heading_buffer;
+			_gnss_heading_buffer = nullptr;
+			printBufferAllocationFailed("GNSS heading");
+			return;
+		}
+	}
+
+	if ((gnss_heading.time_usec - _time_last_gnss_heading) > _min_obs_interval_us) {
+		_time_last_gnss_heading = gnss_heading.time_usec;
+
+		gnssHeadingSample gnss_heading_sample_new;
+
+		gnss_heading_sample_new.time_us = gnss_heading.time_usec - static_cast<uint64_t>(_params.gps_delay_ms * 1000);
+		gnss_heading_sample_new.time_us -= static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
+
+		gnss_heading_sample_new.heading = gnss_heading.heading;
+		gnss_heading_sample_new.heading_accuracy = gnss_heading.heading_accuracy;
+
+		_gnss_heading_buffer->push(gnss_heading_sample_new);
+	} else {
+		ECL_ERR("GNSS heading data too fast %" PRIu64, gnss_heading.time_usec - _time_last_gnss_heading);
+	}
+}
+
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
 	if (!_initialised) {
@@ -564,6 +600,10 @@ void EstimatorInterface::print_status()
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
+	if (_gnss_heading_buffer) {
+		printf("gnss heading buffer: %d/%d (%d Bytes)\n", _gnss_heading_buffer->entries(), _gnss_heading_buffer->get_length(), _gnss_heading_buffer->get_total_size());
+	}
+
 	if (_mag_buffer) {
 		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index f98d3394d90e..aef8525e746f 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -86,6 +86,8 @@ class EstimatorInterface
 
 	void setGpsData(const gps_message &gps);
 
+	void setGnssHeadingData(const gnss_heading_message &gnss_heading_sample);
+
 	void setBaroData(const baroSample &baro_sample);
 
 	void setAirspeedData(const airspeedSample &airspeed_sample);
@@ -294,6 +296,7 @@ class EstimatorInterface
 	// measurement samples capturing measurements on the delayed time horizon
 	baroSample _baro_sample_delayed{};
 	gpsSample _gps_sample_delayed{};
+	gnssHeadingSample _gnss_heading_sample_delayed{};
 	sensor::SensorRangeFinder _range_sensor{};
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
@@ -363,6 +366,7 @@ class EstimatorInterface
 	RingBuffer<outputVert> _output_vert_buffer{12};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+	RingBuffer<gnssHeadingSample> *_gnss_heading_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 	RingBuffer<rangeSample> *_range_buffer{nullptr};
@@ -375,6 +379,7 @@ class EstimatorInterface
 	// timestamps of latest in buffer saved measurement in microseconds
 	uint64_t _time_last_imu{0};
 	uint64_t _time_last_gps{0};
+	uint64_t _time_last_gnss_heading{0};
 	uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
 	uint64_t _time_last_baro{0};
 	uint64_t _time_last_range{0};
diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index c78eea087167..aa254238240c 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -51,8 +51,6 @@ void Ekf::controlGpsFusion()
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 
-		controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
-
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		const bool mandatory_conditions_passing = _control_status.flags.tilt_align
diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp
index 317744da17e8..9b3cf5cbc690 100644
--- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp
@@ -53,16 +53,12 @@ void Ekf::fuseGpsYaw()
 	const float &q2 = _state.quat_nominal(2);
 	const float &q3 = _state.quat_nominal(3);
 
-	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
-
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+	// First column of rotation matrix = body x-axis in earth frame
+	const Vector3f ant_vec_ef = _R_to_earth.col(0);
 
 	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-		return;
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
+	return;
 	}
 
 	// calculate predicted antenna yaw angle
@@ -138,7 +134,7 @@ void Ekf::fuseGpsYaw()
 
 	// calculate the innovation and define the innovation gate
 	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-	_heading_innov = predicted_hdg - measured_hdg;
+	_heading_innov = predicted_hdg - _gnss_heading_sample_delayed.heading;
 
 	// wrap the innovation to the interval between +-pi
 	_heading_innov = wrap_pi(_heading_innov);
@@ -191,28 +187,26 @@ void Ekf::fuseGpsYaw()
 	_fault_status.flags.bad_hdg = !is_fused;
 
 	if (is_fused) {
-		_time_last_gps_yaw_fuse = _time_last_imu;
+		_time_last_gnss_heading_fuse = _time_last_imu;
 	}
 }
 
 bool Ekf::resetYawToGps()
 {
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+	// First column of rotation matrix = body x-axis in earth frame
+	const Vector3f ant_vec_ef = _R_to_earth.col(0);
 
 	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
 		return false;
 	}
 
-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
-	const float measured_yaw = _gps_sample_delayed.yaw;
+	// TODO: Consider using the reported variance
 
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+	resetQuatStateYaw(_gnss_heading_sample_delayed.heading, yaw_variance, true);
 
-	_time_last_gps_yaw_fuse = _time_last_imu;
+	_time_last_gnss_heading_fuse = _time_last_imu;
 	_yaw_signed_test_ratio_lpf.reset(0.f);
 
 	return true;
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 166a08f1afe6..326085d026c9 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -188,6 +188,7 @@ EKF2::~EKF2()
 	perf_free(_msg_missed_airspeed_perf);
 	perf_free(_msg_missed_distance_sensor_perf);
 	perf_free(_msg_missed_gps_perf);
+	perf_free(_msg_missed_gnss_heading_perf);
 	perf_free(_msg_missed_landing_target_pose_perf);
 	perf_free(_msg_missed_magnetometer_perf);
 	perf_free(_msg_missed_odometry_perf);
@@ -252,6 +253,7 @@ int EKF2::print_status()
 	perf_print_counter(_msg_missed_airspeed_perf);
 	perf_print_counter(_msg_missed_distance_sensor_perf);
 	perf_print_counter(_msg_missed_gps_perf);
+	perf_print_counter(_msg_missed_gnss_heading_perf);
 	perf_print_counter(_msg_missed_landing_target_pose_perf);
 	perf_print_counter(_msg_missed_magnetometer_perf);
 	perf_print_counter(_msg_missed_odometry_perf);
@@ -579,6 +581,7 @@ void EKF2::Run()
 		UpdateBaroSample(ekf2_timestamps);
 		UpdateFlowSample(ekf2_timestamps);
 		UpdateGpsSample(ekf2_timestamps);
+		UpdateGNSSHeadingSample(ekf2_timestamps);
 		UpdateMagSample(ekf2_timestamps);
 		UpdateRangeSample(ekf2_timestamps);
 
@@ -1764,6 +1767,29 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+void EKF2::UpdateGNSSHeadingSample(ekf2_timestamps_s &ekf2_timestamps)
+{
+	const unsigned last_generation = _vehicle_gnss_heading_sub.get_last_generation();
+	vehicle_gnss_heading_s vehicle_gnss_heading;
+
+	if (_vehicle_gnss_heading_sub.update(&vehicle_gnss_heading)) {
+		if (_msg_missed_gnss_heading_perf == nullptr) {
+			_msg_missed_gnss_heading_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gnss_heading messages missed");
+
+		} else if (_vehicle_gnss_heading_sub.get_last_generation() != last_generation + 1) {
+			perf_count(_msg_missed_gnss_heading_perf);
+		}
+
+		gnss_heading_message gps_yaw_msg{
+			.time_usec = vehicle_gnss_heading.timestamp,
+			.heading = vehicle_gnss_heading.heading,
+			.heading_accuracy = vehicle_gnss_heading.heading_accuracy,
+		};
+		_ekf.setGnssHeadingData(gps_yaw_msg);
+	}
+
+}
+
 void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	const unsigned last_generation = _magnetometer_sub.get_last_generation();
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index fe7fbdca636c..abadafd1314e 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -86,6 +86,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_gnss_heading.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -160,6 +161,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateGNSSHeadingSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 
@@ -194,6 +196,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	perf_counter_t _msg_missed_airspeed_validated_perf{nullptr};
 	perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
 	perf_counter_t _msg_missed_gps_perf{nullptr};
+	perf_counter_t _msg_missed_gnss_heading_perf{nullptr};
 	perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
 	perf_counter_t _msg_missed_magnetometer_perf{nullptr};
 	perf_counter_t _msg_missed_odometry_perf{nullptr};
@@ -257,6 +260,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_gnss_heading_sub{ORB_ID(vehicle_gnss_heading)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt
index 6a2d9dc2296b..67b760cf8772 100644
--- a/src/modules/ekf2/test/CMakeLists.txt
+++ b/src/modules/ekf2/test/CMakeLists.txt
@@ -42,7 +42,7 @@ px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_s
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_gnss_heading.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
diff --git a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt
index 58bdcee73388..6916484f42c3 100644
--- a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt
+++ b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt
@@ -41,6 +41,7 @@ set(SRCS
 	mag.cpp
 	baro.cpp
 	gps.cpp
+	gnss_heading.cpp
 	flow.cpp
 	range_finder.cpp
 	vio.cpp
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
index e90e01282fbb..38dd0ab0e751 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
@@ -65,7 +65,7 @@ bool EkfWrapper::isIntendingGpsFusion() const
 	return _ekf->control_status_flags().gps;
 }
 
-void EkfWrapper::enableGpsHeadingFusion()
+void EkfWrapper::enableGnssHeadingFusion()
 {
 	_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
 }
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
index 80b4cc064436..00347940f250 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
@@ -65,7 +65,7 @@ class EkfWrapper
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;
 
-	void enableGpsHeadingFusion();
+	void enableGnssHeadingFusion();
 	void disableGpsHeadingFusion();
 	bool isIntendingGpsHeadingFusion() const;
 
diff --git a/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp b/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp
new file mode 100644
index 000000000000..20f04618b4e9
--- /dev/null
+++ b/src/modules/ekf2/test/sensor_simulator/gnss_heading.cpp
@@ -0,0 +1,58 @@
+#include "gnss_heading.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+GnssHeading::GnssHeading(std::shared_ptr<Ekf> ekf) : Sensor(ekf)
+{
+}
+
+GnssHeading::~GnssHeading()
+{
+}
+
+void GnssHeading::send(const uint64_t time)
+{
+	if (!PX4_ISFINITE(_gnss_heading_data.heading)) {
+		// if heading is not set, don't send, as in reality, this data wouldn't be published to vehicle_gnss_heading
+		return;
+	}
+
+	// keep dt in case you want to evolve heading with a rate later
+	const float dt = static_cast<float>(time - _gnss_heading_data.time_usec) * 1e-6f;
+	(void)dt;
+
+	_gnss_heading_data.time_usec = time;
+
+	// hand the sample to the EKF
+	_ekf->setGnssHeadingData(_gnss_heading_data);
+}
+
+void GnssHeading::setData(const gnss_heading_message &gps_yaw)
+{
+	_gnss_heading_data = gps_yaw;
+}
+
+void GnssHeading::setHeading(const float heading)
+{
+	_gnss_heading_data.heading = heading;
+}
+
+void GnssHeading::setHeadingAccuracy(const float heading_accuracy)
+{
+	_gnss_heading_data.heading_accuracy = heading_accuracy;
+}
+
+gnss_heading_message GnssHeading::getDefaultGnssHeadingData()
+{
+	gnss_heading_message gps_yaw{};
+	gps_yaw.time_usec = 0;
+	gps_yaw.heading = NAN;
+	gps_yaw.heading_accuracy = 0.1f;
+	return gps_yaw;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator
diff --git a/src/modules/ekf2/test/sensor_simulator/gnss_heading.h b/src/modules/ekf2/test/sensor_simulator/gnss_heading.h
new file mode 100644
index 000000000000..d6a3253b8ef9
--- /dev/null
+++ b/src/modules/ekf2/test/sensor_simulator/gnss_heading.h
@@ -0,0 +1,31 @@
+#ifndef EKF_GNSS_HEADING_H
+#define EKF_GNSS_HEADING_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class GnssHeading: public Sensor
+{
+public:
+	GnssHeading(std::shared_ptr<Ekf> ekf);
+	~GnssHeading();
+
+	void setData(const gnss_heading_message &gps_yaw);
+	void setHeading(const float heading);
+	void setHeadingAccuracy(const float heading_accuracy);
+
+	gnss_heading_message getDefaultGnssHeadingData();
+
+private:
+	void send(uint64_t time) override;
+
+	gnss_heading_message _gnss_heading_data{};
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // EKF_GNSS_HEADING_H
diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
index a489a79831dc..cb1fd4a14fe7 100644
--- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
@@ -1,11 +1,12 @@
 #include "sensor_simulator.h"
-
+#include <cmath>
 
 SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
 	_airspeed(ekf),
 	_baro(ekf),
 	_flow(ekf),
 	_gps(ekf),
+	_gps_yaw(ekf),
 	_imu(ekf),
 	_mag(ekf),
 	_rng(ekf),
@@ -59,6 +60,9 @@ void SensorSimulator::loadSensorDataFromFile(std::string file_name)
 		} else if (!sensor_type.compare("gps")) {
 			sensor_sample.sensor_type = sensor_info::GPS;
 
+		} else if (!sensor_type.compare("gps_yaw")) {
+			sensor_sample.sensor_type = sensor_info::GPS_YAW;
+
 		} else if (!sensor_type.compare("airspeed")) {
 			sensor_sample.sensor_type = sensor_info::AIRSPEED;
 
@@ -113,6 +117,7 @@ void SensorSimulator::setSensorRateToDefault()
 	_mag.setRateHz(80);
 	_baro.setRateHz(80);
 	_gps.setRateHz(5);
+	_gps_yaw.setRateHz(5);
 	_flow.setRateHz(50);
 	_rng.setRateHz(30);
 	_vio.setRateHz(30);
@@ -125,6 +130,7 @@ void SensorSimulator::setSensorDataToDefault()
 	_baro.setData(122.2f);
 	_flow.setData(_flow.dataAtRest());
 	_gps.setData(_gps.getDefaultGpsData());
+	_gps_yaw.setData(_gps_yaw.getDefaultGnssHeadingData());
 	_imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f});
 	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
 	_rng.setData(0.2f, 100);
@@ -169,6 +175,7 @@ void SensorSimulator::updateSensors()
 	_mag.update(_time);
 	_baro.update(_time);
 	_gps.update(_time);
+	_gps_yaw.update(_time);
 	_flow.update(_time);
 	_rng.update(_time);
 	_vio.update(_time);
@@ -258,6 +265,9 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample)
 					  (float) sample.sensor_data[4],
 					  (float) sample.sensor_data[5]));
 
+	} else if (sample.sensor_type == sensor_info::GPS_YAW) {
+		_gps_yaw.setHeading((float) sample.sensor_data[0]);
+
 	} else if (sample.sensor_type == sensor_info::AIRSPEED) {
 		_airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
 
@@ -385,6 +395,12 @@ void SensorSimulator::setSensorDataFromTrajectory()
 		/* _gps.setLongitude(); */
 		_gps.setVelocity(vel_world);
 	}
+
+	if (_gps_yaw.isRunning()) {
+		// Heading derived from body X-axis projected in world (NED) frame
+		const float heading = std::atan2(_R_body_to_world(1, 0), _R_body_to_world(0, 0));
+		_gps_yaw.setHeading(heading);
+	}
 }
 
 void SensorSimulator::setGpsLatitude(const double latitude)
diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h
index b80901ff9f3a..c628be6b6a99 100644
--- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h
+++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h
@@ -55,6 +55,7 @@
 #include "mag.h"
 #include "baro.h"
 #include "gps.h"
+#include "gnss_heading.h"
 #include "flow.h"
 #include "range_finder.h"
 #include "vio.h"
@@ -65,7 +66,7 @@ using namespace sensor_simulator::sensor;
 
 struct sensor_info {
 	uint64_t timestamp{};
-	enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
+	enum measurement_t {IMU, MAG, BARO, GPS, GPS_YAW, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
 	std::array<double, 10> sensor_data{};
 };
 
@@ -94,6 +95,9 @@ class SensorSimulator
 	void startGps() { _gps.start(); }
 	void stopGps() { _gps.stop(); }
 
+	void startGnssHeading() { _gps_yaw.start(); }
+	void stopGnssHeading() { _gps_yaw.stop(); }
+
 	void startFlow() { _flow.start(); }
 	void stopFlow() { _flow.stop(); }
 
@@ -122,6 +126,7 @@ class SensorSimulator
 	Baro        _baro;
 	Flow        _flow;
 	Gps         _gps;
+	GnssHeading      _gps_yaw;
 	Imu         _imu;
 	Mag         _mag;
 	RangeFinder _rng;
diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_heading.cpp
similarity index 88%
rename from src/modules/ekf2/test/test_EKF_gps_yaw.cpp
rename to src/modules/ekf2/test/test_EKF_gnss_heading.cpp
index de74025d162e..6d79e1103c6e 100644
--- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp
+++ b/src/modules/ekf2/test/test_EKF_gnss_heading.cpp
@@ -55,7 +55,7 @@ class EkfGpsHeadingTest : public ::testing::Test
 	SensorSimulator _sensor_simulator;
 	EkfWrapper _ekf_wrapper;
 
-	void runConvergenceScenario(float yaw_offset_rad = 0.f, float antenna_offset_rad = 0.f);
+	void runConvergenceScenario(float yaw_offset_rad = 0.f);
 	void checkConvergence(float truth, float tolerance = FLT_EPSILON);
 
 	// Setup the Ekf with synthetic measurements
@@ -69,26 +69,27 @@ class EkfGpsHeadingTest : public ::testing::Test
 
 		_sensor_simulator.runSeconds(_init_duration_s);
 		_sensor_simulator._gps.setYaw(NAN);
+		_sensor_simulator._gps_yaw.setHeading(NAN);
 		_sensor_simulator.runSeconds(2);
 		_ekf_wrapper.enableGpsFusion();
-		_ekf_wrapper.enableGpsHeadingFusion();
+		_ekf_wrapper.enableGnssHeadingFusion();
 		_sensor_simulator.startGps();
+		_sensor_simulator.startGnssHeading();
 		_sensor_simulator.runSeconds(11);
 	}
 
 	const uint32_t _init_duration_s{4};
 };
 
-void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad)
+void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one
 	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad);
 
-	_sensor_simulator._gps.setYaw(gps_heading);
-	_sensor_simulator._gps.setYawOffset(antenna_offset_rad);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
-	_ekf_wrapper.enableGpsHeadingFusion();
+	_ekf_wrapper.enableGnssHeadingFusion();
 	_sensor_simulator.runSeconds(5);
 
 	// THEN: the estimate is reset and stays close to the measurement
@@ -108,8 +109,8 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
 
 	// WHEN: enabling GPS heading fusion and heading difference is bigger than 15 degrees
 	const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(20.f);
-	_sensor_simulator._gps.setYaw(gps_heading);
-	_ekf_wrapper.enableGpsHeadingFusion();
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
+	_ekf_wrapper.enableGnssHeadingFusion();
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	_sensor_simulator.runSeconds(0.4);
 
@@ -121,7 +122,7 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
 	EXPECT_NEAR(_ekf_wrapper.getYawAngle(), gps_heading, 0.001);
 
 	// WHEN: GPS heading is disabled
-	_sensor_simulator._gps.stop();
+	_sensor_simulator._gps_yaw.stop();
 	_sensor_simulator.runSeconds(11);
 
 	// THEN: after a while the fusion should be stopped
@@ -134,10 +135,10 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 	const float initial_yaw = math::radians(10.f);
 	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + initial_yaw);
 
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
-	_ekf_wrapper.enableGpsHeadingFusion();
+	_ekf_wrapper.enableGnssHeadingFusion();
 	_sensor_simulator.runSeconds(5);
 
 	// THEN: the estimate is reset and stays close to the measurement
@@ -145,7 +146,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 
 	// AND WHEN: the the measurement changes
 	gps_heading += math::radians(2.f);
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(20);
 
 	// THEN: the estimate slowly converges to the new measurement
@@ -161,29 +162,25 @@ TEST_F(EkfGpsHeadingTest, yaw0)
 TEST_F(EkfGpsHeadingTest, yaw60)
 {
 	const float yaw_offset_rad = math::radians(60.f);
-	const float antenna_offset_rad = math::radians(80.f);
-	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+	runConvergenceScenario(yaw_offset_rad);
 }
 
 TEST_F(EkfGpsHeadingTest, yaw180)
 {
 	const float yaw_offset_rad = math::radians(180.f);
-	const float antenna_offset_rad = math::radians(-20.f);
-	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+	runConvergenceScenario(yaw_offset_rad);
 }
 
 TEST_F(EkfGpsHeadingTest, yawMinus120)
 {
 	const float yaw_offset_rad = math::radians(120.f);
-	const float antenna_offset_rad = math::radians(-42.f);
-	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+	runConvergenceScenario(yaw_offset_rad);
 }
 
 TEST_F(EkfGpsHeadingTest, yawMinus30)
 {
 	const float yaw_offset_rad = math::radians(-30.f);
-	const float antenna_offset_rad = math::radians(10.f);
-	runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
+	runConvergenceScenario(yaw_offset_rad);
 }
 
 TEST_F(EkfGpsHeadingTest, fallBackToMag)
@@ -193,7 +190,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
 	// to the filter
 	_sensor_simulator.runSeconds(6);
 	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(10.f));
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
 	_sensor_simulator.runSeconds(1);
@@ -208,7 +205,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
 
 	// BUT WHEN: the GPS yaw is suddenly invalid
 	gps_heading = NAN;
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(7.5);
 
 	// THEN: after a few seconds, the fusion should stop and
@@ -227,7 +224,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
 	float gps_heading = math::radians(90.f);
 	const float true_heading = math::radians(-20.f);
 
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(10);
 
 	const Vector3f accel_frd{-1.0, -1.5f, 0.f};
@@ -265,14 +262,14 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
 {
 	// GIVEN: the GPS yaw fusion activated
 	float gps_heading = _ekf_wrapper.getYawAngle();
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(1);
 	_ekf->set_in_air_status(false);
 
 	// WHEN: the measurement suddenly changes
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(8);
 
 	// THEN: the fusion should reset
@@ -285,14 +282,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 {
 	// GIVEN: the GPS yaw fusion activated
 	float gps_heading = _ekf_wrapper.getYawAngle();
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(5);
 	_ekf->set_in_air_status(true);
 
 	// WHEN: the measurement suddenly changes
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(7.5);
 
 	// THEN: the fusion should reset
@@ -300,7 +297,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 
 	// BUT WHEN: the measurement jumps a 2nd time
 	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(7.5);
 
 	// THEN: after a few seconds, the fusion should stop and
@@ -314,12 +311,12 @@ TEST_F(EkfGpsHeadingTest, stopOnGround)
 	// GIVEN: the GPS yaw fusion activated and there is no mag data
 	_sensor_simulator._mag.stop();
 	float gps_heading = _ekf_wrapper.getYawAngle();
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(5);
 
 	// WHEN: the measurement stops
 	gps_heading = NAN;
-	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps_yaw.setHeading(gps_heading);
 	_sensor_simulator.runSeconds(7.5);
 
 	// THEN: the fusion should stop and the GPS pos/vel aiding
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index dc635653e049..f0b123e200d1 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -107,6 +107,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_control_mode");
 	add_topic("vehicle_global_position", 200);
 	add_topic("vehicle_gps_position", 500);
+	add_topic("vehicle_gnss_heading", 500);
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
@@ -280,6 +281,7 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("sensor_selection");
 	add_topic("vehicle_air_data");
 	add_topic("vehicle_gps_position");
+	add_topic("vehicle_gnss_heading");
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_magnetometer");
 	add_topic("vehicle_status");
diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index 0eb8a7e0a968..fbbcc245d5c0 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 add_subdirectory(vehicle_air_data)
 add_subdirectory(vehicle_gps_position)
+add_subdirectory(vehicle_gnss_heading)
 add_subdirectory(vehicle_imu)
 add_subdirectory(vehicle_magnetometer)
 
@@ -60,6 +61,7 @@ px4_add_module(
 		vehicle_angular_velocity
 		vehicle_air_data
 		vehicle_gps_position
+		vehicle_gnss_heading
 		vehicle_imu
 		vehicle_magnetometer
 	)
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index fd93e18dff7a..58010423e3ed 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -233,3 +233,39 @@ PARAM_DEFINE_INT32(SENS_IMU_MODE, 1);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1);
+
+/**
+ * GPS Relative X offset
+ *
+ * The body-frame offset between the GPS moving baseline and the rover.
+ * A positive value means the rover antenna is in front of the moving baseline.
+ *
+ * @group Sensors
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GNSSREL_PX, 0);
+
+/**
+ * GPS Relative Y offset
+ *
+ * The body-frame offset between the GPS moving baseline and the rover.
+ * A positive value means the rover antenna is to the right of the moving baseline.
+ *
+ * @group Sensors
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GNSSREL_PY, 0);
+
+/**
+ * GPS Relative Z offset
+ *
+ * The body-frame offset between the GPS moving baseline and the rover.
+ * A positive value means the rover antenna is below the moving baseline.
+ *
+ * @group Sensors
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GNSSREL_PZ, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 30ba6aeaac1e..7abc8c0cdb9d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -76,6 +76,7 @@
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
 #include "vehicle_air_data/VehicleAirData.hpp"
 #include "vehicle_gps_position/VehicleGPSPosition.hpp"
+#include "vehicle_gnss_heading/VehicleGNSSHeading.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
 #include "vehicle_magnetometer/VehicleMagnetometer.hpp"
 
@@ -181,6 +182,7 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 	VehicleAirData          *_vehicle_air_data{nullptr};
 	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
 	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
+	VehicleGNSSHeading	   	*_vehicle_gnss_heading{nullptr};
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
@@ -218,6 +220,7 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleGPSPosition();
+	void		InitializeVehicleGNSSHeading();
 	void		InitializeVehicleIMU();
 	void		InitializeVehicleMagnetometer();
 
@@ -267,6 +270,7 @@ Sensors::Sensors(bool hil_enabled) :
 
 	InitializeVehicleAirData();
 	InitializeVehicleGPSPosition();
+	InitializeVehicleGNSSHeading();
 	InitializeVehicleIMU();
 	InitializeVehicleMagnetometer();
 }
@@ -291,6 +295,11 @@ Sensors::~Sensors()
 		delete _vehicle_gps_position;
 	}
 
+	if (_vehicle_gnss_heading) {
+		_vehicle_gnss_heading->Stop();
+		delete _vehicle_gnss_heading;
+	}
+
 	if (_vehicle_magnetometer) {
 		_vehicle_magnetometer->Stop();
 		delete _vehicle_magnetometer;
@@ -385,6 +394,7 @@ int Sensors::parameters_update()
 
 	InitializeVehicleAirData();
 	InitializeVehicleGPSPosition();
+	InitializeVehicleGNSSHeading();
 	InitializeVehicleMagnetometer();
 
 	return PX4_OK;
@@ -589,6 +599,19 @@ void Sensors::InitializeVehicleGPSPosition()
 	}
 }
 
+void Sensors::InitializeVehicleGNSSHeading()
+{
+	if (_param_sys_has_gps.get()) {
+		if (_vehicle_gnss_heading == nullptr) {
+			_vehicle_gnss_heading = new VehicleGNSSHeading();
+
+			if (_vehicle_gnss_heading) {
+				_vehicle_gnss_heading->Start();
+			}
+		}
+	}
+}
+
 void Sensors::InitializeVehicleIMU()
 {
 	// create a VehicleIMU instance for each accel/gyro pair
@@ -791,6 +814,11 @@ int Sensors::print_status()
 		_vehicle_gps_position->PrintStatus();
 	}
 
+	if (_vehicle_gnss_heading) {
+		PX4_INFO_RAW("\n");
+		_vehicle_gnss_heading->PrintStatus();
+	}
+
 	PX4_INFO_RAW("\n");
 
 	for (auto &i : _vehicle_imu_list) {
diff --git a/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt b/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt
new file mode 100644
index 000000000000..e216d6e2d5d1
--- /dev/null
+++ b/src/modules/sensors/vehicle_gnss_heading/CMakeLists.txt
@@ -0,0 +1,8 @@
+px4_add_library(vehicle_gnss_heading
+	VehicleGNSSHeading.cpp
+	VehicleGNSSHeading.hpp
+)
+target_link_libraries(vehicle_gnss_heading
+	PRIVATE
+		px4_work_queue
+)
diff --git a/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp
new file mode 100644
index 000000000000..3152b37cfe9f
--- /dev/null
+++ b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.cpp
@@ -0,0 +1,169 @@
+#include "VehicleGNSSHeading.hpp"
+
+#include <px4_platform_common/log.h>
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+namespace sensors
+{
+VehicleGNSSHeading::VehicleGNSSHeading() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	_vehicle_gnss_heading_pub.advertise();
+}
+
+VehicleGNSSHeading::~VehicleGNSSHeading()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleGNSSHeading::Start()
+{
+	// force initial updates
+	ParametersUpdate(true);
+
+	_sensor_gnss_relative_sub.registerCallback();
+
+	ScheduleNow();
+
+	return true;
+}
+
+void VehicleGNSSHeading::Stop()
+{
+	Deinit();
+	_sensor_gnss_relative_sub.unregisterCallback();
+}
+
+void VehicleGNSSHeading::ParametersUpdate(bool force)
+{
+	if (_parameter_update_sub.updated() || force) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+	}
+
+	const float bx = _params_gpsrel_px.get();
+	const float by = _params_gpsrel_py.get();
+	const float bz = _params_gpsrel_pz.get();
+
+	const float baseline_length = sqrtf(bx * bx + by * by + bz * bz);
+
+	if (baseline_length < 0.3f) {
+		PX4_ERR("[vehicle_gnss_heading]Expected baseline length is too small: %.2f m", (double)baseline_length);
+		_expected_baseline_length_m = NAN;
+		_baseline_body_angle = NAN;
+		return;
+	}
+
+	_expected_baseline_length_m = baseline_length;
+	_baseline_body_angle = atan2f(by, bx);
+}
+
+void VehicleGNSSHeading::Run()
+{
+	perf_begin(_cycle_perf);
+	ParametersUpdate();
+
+	if (PX4_ISFINITE(_expected_baseline_length_m) && PX4_ISFINITE(_baseline_body_angle)
+	    && _sensor_gnss_relative_sub.updated()) {
+		sensor_gnss_relative_s d{};
+		_sensor_gnss_relative_sub.copy(&d);
+
+		// Basic validity
+		if (!PX4_ISFINITE(d.heading) || !PX4_ISFINITE(d.heading_accuracy) ||
+		    !PX4_ISFINITE(d.position_length) || !d.heading_valid || !d.relative_position_valid) {
+			perf_end(_cycle_perf);
+			return;
+		}
+
+		if (_params_gpsyaw_crq.get() == 1 && (!d.carrier_solution_floating || !d.carrier_solution_fixed)) {
+			perf_end(_cycle_perf);
+			return;
+		}
+
+		if (_params_gpsyaw_crq.get() == 2 && !d.carrier_solution_fixed) {
+			perf_end(_cycle_perf);
+			return;
+		}
+
+		// The reported baseline length compared to expected is a good indication of accuracy.
+		// Additionally, it prevents potentially large errors if the wrong RTCM stream is sent to the receiver.
+		const float gate_len = _params_gpsyaw_lrq.get();
+
+		if (PX4_ISFINITE(gate_len) && gate_len >= 0.0f) {
+			const float len_diff = fabsf(d.position_length - _expected_baseline_length_m);
+
+			if (len_diff > gate_len) {
+				perf_end(_cycle_perf);
+				return;
+			}
+		}
+
+		const hrt_abstime prev_valid = _last_valid_timestamp;
+		_last_valid_timestamp = d.timestamp;
+
+		// Reject if the delta time to previous valid update is too large, ublox receivers can output stale data
+		// when in rover mode. F9P rovers publish every 125 ms, we accept up to 200 ms additional delay.
+		if (prev_valid == 0 || d.timestamp - prev_valid > 325_ms) {
+			perf_end(_cycle_perf);
+			return;
+		}
+
+		// Rotate into vehicle frame using configured antenna position
+		const float vehicle_yaw = matrix::wrap_pi(d.heading - _baseline_body_angle);
+
+		// publish
+		vehicle_gnss_heading_s out{
+			.timestamp = d.timestamp,
+			.timestamp_sample = d.timestamp_sample,
+			.heading = vehicle_yaw,
+			.heading_accuracy = d.heading_accuracy
+		};
+
+		_vehicle_gnss_heading_pub.publish(out);
+	}
+
+	perf_end(_cycle_perf);
+}
+
+void VehicleGNSSHeading::PrintStatus()
+{
+	PX4_INFO("VehicleGNSSHeading status:");
+
+	const float bx = _params_gpsrel_px.get();
+	const float by = _params_gpsrel_py.get();
+	const float bz = _params_gpsrel_pz.get();
+	const float gate_len = _params_gpsyaw_lrq.get();
+	const int32_t crq = _params_gpsyaw_crq.get();
+
+	PX4_INFO("  Antenna baseline (body) [m]: bx=%.2f by=%.2f bz=%.2f", (double)bx, (double)by, (double)bz);
+	PX4_INFO("  Expected baseline length [m]: %s%.2f",
+		 isfinite(_expected_baseline_length_m) ? "" : "(invalid) ",
+		 (double)_expected_baseline_length_m);
+
+	if (PX4_ISFINITE(gate_len) && gate_len >= 0.f) {
+		PX4_INFO("  Baseline length gate [m]: %.2f m", (double)gate_len);
+
+	} else {
+		PX4_INFO("  Baseline length gate: disabled");
+	}
+
+	PX4_INFO("  Required carrier solution: %d (0=None, 1=Floating or Fixed, 2=Fixed)", (int)crq);
+
+	if (_last_valid_timestamp != 0) {
+		const float since_last_valid_s = (hrt_absolute_time() - _last_valid_timestamp) * 1e-6f;
+		PX4_INFO("  Last valid GNSS-relative message: %.3f s ago", (double)since_last_valid_s);
+
+	} else {
+		PX4_INFO("  Last valid GNSS-relative message: none yet");
+	}
+
+	perf_print_counter(_cycle_perf);
+}
+
+}; // namespace sensors
diff --git a/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp
new file mode 100644
index 000000000000..d800ea760d84
--- /dev/null
+++ b/src/modules/sensors/vehicle_gnss_heading/VehicleGNSSHeading.hpp
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gnss_relative.h>
+#include <uORB/topics/vehicle_gnss_heading.h>
+
+using namespace time_literals;
+
+namespace sensors
+{
+class VehicleGNSSHeading : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	VehicleGNSSHeading();
+	~VehicleGNSSHeading() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate(bool force = false);
+
+	uORB::Publication<vehicle_gnss_heading_s> _vehicle_gnss_heading_pub{ORB_ID(vehicle_gnss_heading)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	// Only one subscription callback for GNSS relative, since PX4 currently only supports two receivers,
+	// i.e. one base/rover pair.
+	uORB::SubscriptionCallbackWorkItem _sensor_gnss_relative_sub{this, ORB_ID(sensor_gnss_relative), 0};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SENS_GNSSREL_PX>) _params_gpsrel_px,
+		(ParamFloat<px4::params::SENS_GNSSREL_PY>) _params_gpsrel_py,
+		(ParamFloat<px4::params::SENS_GNSSREL_PZ>) _params_gpsrel_pz,
+		(ParamFloat<px4::params::SENS_GNSSHDG_LRQ>) _params_gpsyaw_lrq,
+		(ParamInt<px4::params::SENS_GNSSHDG_CRQ>) _params_gpsyaw_crq
+	)
+
+	float _expected_baseline_length_m{NAN};
+	float _baseline_body_angle{NAN};
+	hrt_abstime _last_valid_timestamp{0};
+};
+}; // namespace sensors
diff --git a/src/modules/sensors/vehicle_gnss_heading/params.c b/src/modules/sensors/vehicle_gnss_heading/params.c
new file mode 100644
index 000000000000..9b90f41bd9f6
--- /dev/null
+++ b/src/modules/sensors/vehicle_gnss_heading/params.c
@@ -0,0 +1,24 @@
+/**
+ * GPS Yaw Baseline Length Accuracy Requirement
+ *
+ * Discard any GPS yaw data if the baseline length deviates from the expected length by more than this value.
+ * Expected length is calculated from the SENS_GNSSREL_P{X,Y,Z} parameters.
+ * A value of 0 disables the gate. (This is not recommended.)
+ *
+ * @group Sensors
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GNSSHDG_LRQ, 0.1f);
+
+/**
+ * Required carrier phase range solution for GPS Yaw
+ *
+ * @value 0 None
+ * @value 1 Floating or Fixed
+ * @value 2 Fixed
+ * @group Sensors
+ * @min 0
+ * @max 2
+ */
+PARAM_DEFINE_INT32(SENS_GNSSHDG_CRQ, 2);