FusionCore, which is a ROS 2 Jazzy sensor fusion package (robot_localization replacement)

ROS ROS General
1

Hey everyone,
I’ve been working on FusionCore for the last few months… it’s a ROS 2 Jazzy sensor fusion package that aims to bridge the gap left by the deprecation of robot_localization.

There wasn’t anything user-friendly available for ROS 2 Jazzy. It merges IMU, wheel encoders, and GPS/GNSS into a single, reliable position estimate at 100Hz. No need for manual covariance matrices…. just one YAML config file.

  • It uses an Unscented Kalman Filter (UKF) with a complete 3D state…. and it’s not just a port of robot_localization.
  • It features native GNSS fusion in ECEF coordinates, so you won’t run into UTM zone issues.
  • It supports dual antenna heading right out of the box….
  • It automatically estimates IMU gyroscope and accelerometer bias.
  • It includes HDOP/VDOP quality-aware noise scaling, which means bad GPS fixes are automatically down-weighted.
  • It’s under the Apache 2.0 license, making it commercially safe.
  • And it’s built natively for ROS 2 Jazzy….. not just a port.

GitHub: https://github.com/manankharwar/fusioncore

image
image892×618 38.7 KB

I respond to issues within 24 hours. If you’re working on a wheeled robot with GPS on ROS 2 Jazzy and hit problems….. open an issue or reply here.

7 Likes
ROS News for the Week of March 31st, 2026
ROS News for the Week of March 23rd, 2026
2

Thanks for the package! I really like the simplicity and cleanliness of the design.

However, it seems to be too simple so far in several areas:

  • IMUs rarely come in base_link frame. You should use imu_transformer.
  • GNSS also rarely comes in base_link. This is, however, a much harder problem, because if the antenna is offset from the axis of the robot, its readings are for a point that follows different trajectory than base_link. This problem itself adds a few 100s lines to robot_localization, yet it is a very important one to resolve (and no, please, do not take inspiration from r_l which just saves the very first heading as an offset). Until you’re sure with the heading of the robot, fusing off-center GNSS is not good.
  • Why do you ignore covariances in the messages? Some sensors/algorithms actually put useful values there… Most importantly GNSS has often off-diagonal elements that are quite important (and DOPs are only very lightly related to the quality of localization). Or a wheel-slip estimating odometry.
  • But I agree if a sensor publishes bogus covariance, it is nice to have an easy way to override it.
  • Some IMUs provide very good orientation estimates. Why not use them?
  • What about using multiple sources of each type?

Out of which box? I don’t see anything heading-related in the ROS node (only code in the core, but no ROS interface).

If you’re also missing a message to represent the heading, please consider using compass/compass_msgs/msg/Azimuth.msg at master · ctu-vras/compass · GitHub . One day, I’d like to standardize it. The more users it has, the more feedback I have and the better the standard will be.

1 Like
3

Hello… thank you, this is exactly the feedback I needed. You are so wholesome.

Responding honestly to each point:
IMU frame transform: You’re absolutely right, this is missing and it’s the highest priority fix. The node currently assumes IMU data arrives in base_link frame which is almost never true. Adding proper TF-based frame transformation before fusion. (no matter how much innovation we think… it all boils down to experience… Kudos to you).

GNSS antenna offset: Acknowledged: this is a known hard problem I haven’t solved yet. FusionCore currently assumes the antenna is at base_link. On the roadmap but I want to do it properly. I think I have an understanding that will resolve this.

Ignoring message covariances: Fair. I’ll use message covariances when they’re non-zero and meaningful, and fall back to config params when they’re not. This is a straightforward fix.

IMU orientation estimates: Very good point… adding orientation as a direct measurement input, not just raw gyro/accel.

Dual antenna heading not in ROS node: You’re correct…. the core C++ has the math but it’s not wired to a ROS topic yet. Fixing this in the next release. I’ll look at compass_msgs… standardization makes sense.

FusionCore is early. The core math works. The ROS integration has real gaps your feedback made clear. Working through this list now.

Thanks for taking the time.

2 Likes
4

On the topic, I’m genuinely curious if there is a fully featured “standard” way to output dual antenna data in the first place. The compass message you mention works, but only captures a very small amount of the data these units typically output with heading. For instance a dual antenna setup also provides either a pitch or roll measurement, not just heading.

In the past I’ve made a custom message (which admittedly contains some redundant data): DifferentialMeasurement — marti_sensor_msgs 1.6.1 documentation

Theoretically you can represent it with a generic nav_msgs/Odometry message or with multiple fixes, but I haven’t found those line up well with the actual output of GPS devices I’ve used.

For instance you can see a typical GPS output here: https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.51_InterfaceDescription_UBXDOC-963802114-13124.pdf#[{“num”%3A2829%2C"gen"%3A0}%2C{“name”%3A"XYZ"}%2C59.527%2C129.637%2Cnull]

And here: NovatelDualAntennaHeading — novatel_gps_msgs: Rolling 4.2.1 documentation

2 Likes
5

Hi peci1!! Thank you again for the detailed review. I spent the last few days working through your list properly.

Here’s what’s been fixed:

IMU frame transform: the node now reads frame_id from the IMU message, looks up the TF rotation to base_link, and transforms angular velocity and linear acceleration before fusing. If the transform is missing, it prints the exact static_transform_publisher command to fix it and falls back gracefully.

GNSS antenna offset: implemented with a proper observability guard. The lever arm correction only activates when heading has been independently validated from a real source. Specifically, heading_validated_ is only set true when: a dual antenna heading message is received, an AHRS/IMU with magnetometer publishes orientation, or the robot has traveled >= 5 meters at sufficient speed with low yaw rate. Before any of these, lever arm is disabled regardless of what yaw variance says…. so the filter cannot fake its way into applying the correction.

Message covariances: GNSS now uses the full 3x3 covariance matrix when position_covariance_type == 3, including off-diagonal elements. Wheel odometry reads twist.covariance per-axis when available. Both fall back to config params when covariance is zero or unknown. Config params still work as overrides for sensors with bogus covariance.

IMU orientation: accepted as a direct measurement. Added imu.has_magnetometer config flag…. when false (default, 6-axis IMUs), orientation fuses roll/pitch but does not validate heading. When true (9-axis: BNO08x, VectorNav, Xsens), heading is validated. This prevents gyro drift from silently activating lever arm correction.

Multiple sources: second GNSS receiver configurable via gnss.fix2_topic. Multiple IMU sources via separate TF-aware callbacks.

Dual antenna heading: fully wired to a ROS topic (gnss.heading_topic, default /gnss/heading).

Uses sensor_msgs/Imu orientation quaternion: acknowledged this is slightly awkward but it’s what most u-blox and Septentrio drivers publish.

compass_msgs/Azimuth: added support. The upstream package is ROS 1 only so I ported the identical message definition to ROS 2 Jazzy natively. FusionCore now accepts compass_msgs/Azimuth on a configurable topic, handles ENU/NED convention conversion, RAD/DEG units, and warns when magnetic north reference is used instead of geographic.

Happy to contribute the ROS 2 port back upstream if that’s useful to you.

Do you mind testing it out and/or if you have any questions or if you spot any red flags then please let me know…

1 Like
6

Ah Great point… you’re right that compass_msgs/Azimuth only captures a subset of what dual antenna units actually output. Heading + variance is enough for wheeled robots with horizontally mounted antennas, but pitch, baseline length, and fix type are genuinely useful data that FusionCore currently doesn’t consume.

This is on my radar. The pitch measurement would fuse directly into the filter’s pitch state…. same pattern as heading into yaw…. and baseline validation would strengthen the fix quality check. The blocker is the message format question you raise: NovAtel has their format, u-blox has theirs, compass_msgs covers heading only. I’d rather wait for a bit more community consensus on what the standard should look like than hardcode one vendor’s format.

If you’re working with a specific receiver and want to test this…. open an issue and let’s figure out the right interface together. Happy to prioritize it if there’s real hardware to test against.