Concept: Separate GNSS position and heading topics

[oystub]

NB! Not for merge, just for discussing the approach, and to see what could be relevant for upstream master

We are currently on PX4 v1.13, though in the process of upgrading to v1.15 (and potentially v1.16). Since our long-term goal is to keep our firmware as close as possible to upstream, we’d like to get your feedback on the approach. If you agree with the direction, we’re happy to continue working on it and adapt it to the current master branch.

Terminology note: In this context I intentionally use GNSS and heading instead of GPS and yaw. “GPS” refers only to a single constellation, while “GNSS” is the broader and correct term. Similarly, the measurements in question provide a heading in the NED frame, which only coincides with yaw for certain Euler angle conventions (e.g. PX4’s roll–pitch–yaw sequence). That said, I sometimes slip into the older terminology when writing or talking, and you may still see “GPS” and “yaw” where interfacing with existing code requires it.

For reasons I elaborate below, we don’t want to use GPS blending in our setup, but we still need to make use of relative heading measurements from an MB/rover receiver pair. At the moment, this isn’t possible.

The current implementation in the GPS blending module feels both hacky and semantically incorrect for several reasons:

• GNSS heading is tied to blending, even though the two are conceptually unrelated.
• GNSS position and heading timestamps are forced to be identical, even though they may arrive at different rates.
• Because of the blending logic and timing, many sensor_gps messages end up without heading data—even when a heading was successfully computed. This complicates debugging.
• A heading measurement is fundamentally different from a position measurement: it comes from two body-frame receivers rather than a single one. (For self-contained dual-GPS units that act as a single receiver, this distinction is less relevant.)

To address these issues, we implemented a separate system where GNSS heading is decoupled from position. We added a new sensor bridge module that publishes vehicle_gnss_heading. This behaves analogously to vehicle_gps_position, but instead consumes sensor_gnss_relative (available since v1.13, link)

This selector validates the baseline length and fix type, then converts the body-frame relative receiver positions into a NED-frame heading. Only validated results are published to vehicle_gnss_heading. Right now, it only supports one input topic instance (more than 2 receivers are not supported anyway). If we want do do something more with relative GNSS in the future, this could be expanded.

Topics below are not part of this PR, just explains the background of why we propose these changes.

---

Background: Moving baseline navigation delay

As described in the u-blox integration manual and detailed in PX4-GPSDrivers#109, a u-blox receiver operating in moving baseline mode can reduce its navigation rate to as low as 1 Hz. This occurs not only when communication between the moving base and rover is lost, but also during periods of poor signal quality where the rover cannot compute a valid navigation solution.

This reduced rate is already undesirable, but the real issue is that the navigation data itself is delayed by about one second. Our testing shows that the data within both time_utc_usec and the navigation fields in the GNSS message are approximately 900 ms older than normal when received by PX4. Even if compensated, using the time field as reference, these measurements fall outside the EKF’s valid time horizon.

If the moving base stops sending corrections entirely, the rover remains in this degraded mode for the duration of CFG-NAVSPG-CONSTR_DGNSSTO. JonasPerolini suggested lowering this timeout, but this does not help in cases of intermittent connection or poor signal quality.

In practice, we have repeatedly observed the drone entering “toilet bowl” behavior and suffering position loss when the MB receiver dropped out, mainly because we had a unit which was vibration-sensitive and would occasionally go completely offline. Even though the rover continued reporting valid navigation data, the delay caused the EKF to accumulate large innovations, reject GNSS updates, and lose a reliable position estimate. (We can provide logs if anyone is interested.)

Because of these issues, we avoid blending for GNSS heading fusion, since rover-mode data cannot be trusted in these conditions.

Side note: Rover reconfiguration for fallback use

To mitigate these problems, we use the MB unit as the primary GNSS source for position, and only fall back to the rover unit when necessary. To make the rover a reliable fallback, we added functionality in the GPS driver and module to dynamically reconfigure it.

When the rover is promoted to position source, it is reconfigured from rover mode to standard mode by disabling RTCM input and issuing a soft position reset command. This brings the unit back online within 1–2 seconds and avoids the delayed navigation problem.

This feature is somewhat specialized and may not be broadly useful, but if others are interested, we would be happy to contribute it upstream as well. Our internal PR (with a slightly outdated description) is here: https://github.com/aviant-tech/PX4-Autopilot/pull/104

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-dev-call-sep-3-2025-team-sync-and-community-q-a/47287/2

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-dev-call-sep-3-2025-team-sync-and-community-q-a/47287/1

[bengrocholsky]

Just a heads-up that the math for the EKF2 relposned (heading) update might look simpler if the sensor_gnss_relative contained the NED vector same as the relposned ubx msg and the system kept track of every GNSS antenna lever arm by changing EKF2_GPS_POS_? to an array.

[oystub]

Just a heads-up that the math for the EKF2 relposned (heading) update might look simpler if the sensor_gnss_relative contained the NED vector same as the relposned ubx msg and the system kept track of every GNSS antenna lever arm by changing EKF2_GPS_POS_? to an array.

Good point. You're essentially saying that the GNSS driver should also give out the body-frame offsets between the two receivers used in a sensor_gnss_relative message? That might make sense. We should definitely get per-receiver-body frame offsets based on device id (like for magnetometers) into PX4. But it might make more sense for the driver itself to manage the offsets, since it would need to know which receivers is used to calculate the relative fix. (Assuming more than 3 receivers, or a separate MB that isn't connected to PX4).

If this body-frame info is added to sensor_gnss_relative, then we could skip the sensors bridge step.

[github-actions[bot]]

This PR was identified as stale and it will be closed in 30 days unless any activity is detected.

[dakejahl]

Bump. I think this would be a great improvement.

[bengrocholsky]

@oystub @dakejahl , thanks for implementation effort and bumping. I agree this is good as is. I can turn my comment into a more detailed design doc and start changes/MRs.

Yes, the motivation is from a few aspects and should be split up appropriately. Likely separate from this PR:

• Individual lever arms is needed to get the most out of fusion of more accurate position/velocity updates from each receiver.
• Modifying EFK2 interface to update via rel_pos vector is numerically simpler than converting to a heading angle.
• It can increase attitude and bias observability with dual antenna/rx. E.g. yaw/pitch if longitudinal, or yaw/roll if lateral.
• With three antenna/rx full roll/pitch/yaw is observable independent of motion (on supported interfaces e.g. ark dronecan rather than serial ports).

[oystub]

Thanks for the interest! I'm very busy with some other deadlines for the rest of the year, so I probably don't have the capacity to work more on upstreaming this in the near future.

Here is the 1.15 version of this that we ended up using, it's at least a little bit closer to main than this 1.13 one, if someone wants to do some more on this in the meantime. Feel free to use the code however you want. https://github.com/aviant-tech/PX4-Autopilot/commit/a93400c8a7158d19e14495e682c5aa2b0090fb00 (Note that it is currently missing some minor updates to ekf2 replay, where the vehicle_gnss_relative topic should also be included and replayed.)

@bengrocholsky

Individual lever arms is needed to get the most out of fusion of more accurate position/velocity updates from each receiver.

Yes! I think many users would appreciate a general per-device GNSS body-frame offset setting, like with mag orientations in the sensor hub. This is useful both in fallback and blending configurations.

I think we all agree that the current blending system isn't optimal. @dagar talked a bit about the possibility of fusing individual GNSS receivers with their own body-frame offsets in the EKF directly, instead of a blending step. (But given how bad and non-linear variance estimates and errors from GNSS receivers can behave, one probably has to think pretty hard about how to do this in a good way when combining multiple of them in order to get it right, especially with all the different types of hardware in use)

Modifying EFK2 interface to update via rel_pos vector is numerically simpler than converting to a heading angle.

I very much support doing this! The only reason we kept the heading fusion was to avoid unnecessary changes to ekf2 internals, keeping us closer to upstream until we get something like this.

With three antenna/rx full roll/pitch/yaw is observable independent of motion (on supported interfaces e.g. ark dronecan rather than serial ports).

This would also be a good addition. However since it's pretty niche, we shouldn't make the general GNSS handling more complex just to support this. If we can make it generalized in a good way that doesn't make the regular two-receiver case too complicated, it would be a good add-on!