manankharwar/fusioncore: ROS 2 sensor fusion SDK: UKF, 3D native, proper GNSS, zero manual tuning. Apache 2.0. · GitHub

ROS 2 sensor fusion SDK. Combines IMU, wheel encoders, and GPS into one reliable position estimate. Self-tuning noise covariance. Apache 2.0.

What problem does this solve?

Every mobile robot needs to know where it is. It gets this from multiple sensors: IMU, wheel encoders, GPS: each of which is imperfect in its own way. IMUs drift. Wheels slip. GPS jumps. You need software that intelligently combines all three into one trustworthy position estimate.

That software is called a sensor fusion package. The standard one for ROS, robot_localization, lacks native ECEF GPS fusion, IMU bias estimation, and adaptive noise covariance. Its designated replacement (fuse) has incomplete GPS support with no ECEF handling or RTK quality gating as of early 2026. No clear accessible replacement exists for either.

FusionCore is built to fill that gap.

FusionCore vs robot_localization on the NCLT dataset (University of Michigan): same IMU + wheel odometry + GPS, no manual tuning. Six sequences, same pipeline:

Sequence
FC ATE RMSE
RL-EKF ATE RMSE
RL-UKF

2012-01-08
5.6 m
23.4 m
NaN divergence at t=31 s

2012-02-04
9.7 m
20.6 m
NaN divergence at t=22 s

2012-03-31
4.2 m
10.8 m
NaN divergence at t=18 s

2012-08-20
7.5 m
9.4 m
NaN divergence

2012-11-04
28.7 m
10.9 m
NaN divergence

2013-02-23
4.1 m
5.8 m
NaN divergence

FusionCore wins 5 of 6 sequences. On 2012-11-04 (fall, degraded GPS), FC’s Mahalanobis outlier gate still loses despite inertial coast mode (Q inflation on consecutive rejections): GPS was sufficiently degraded for long enough that accumulated drift could not be fully recovered. RL-EKF has no rejection gate and self-corrects immediately. RL-UKF diverged with NaN on all six sequences. Full methodology, configs, and reproduce instructions in benchmarks/.

Capability
robot_localization
Fuse
FusionCore

Core filter
EKF or UKF
Factor graph
UKF (22D quaternion state)

3D support
Yes
Yes
Full 3D, native

IMU bias estimation
No built-in states
Plugin-dependent
Gyro + accel bias states

GPS fusion
navsat_transform node
Plugin, no ECEF/RTK
ECEF-native, single node

Dual antenna heading
No
No
Yes

IMU frame transform
Manual (YAML)
Manual (YAML)
Automatic via TF

Message covariances
Used
Partial
Full 3×3 GNSS + odometry

GNSS antenna offset
Ignored
Ignored
Lever arm + observability guard

Outlier rejection
mahalanobis_threshold
Robust loss functions
Chi-squared gating, all sensors

GPS fix quality gating
No
No
GPS / DGPS / RTK_FLOAT / RTK_FIXED

Adaptive noise
Manual
Manual
Auto from innovation sequence

TF validation at startup
Basic
No
Startup check + fix commands

Multiple GNSS receivers
Workaround
Workaround
Native, independent lever arms

compass_msgs/Azimuth
No
No
Yes (ENU/NED, rad/deg)

Delay compensation
history_length
Factor graph inherent
Full IMU replay, 500ms

Ground constraint
Not built-in
Not built-in
VZ=0 pseudo-measurement

ZUPT
Not built-in
Not built-in
Auto when stationary

Sensor dropout detection
Basic
Basic
Per-sensor SensorHealth enum

/diagnostics
Basic
Basic
Per-sensor health + outliers

Published covariance
Yes
Yes
Full UKF P matrix

Filter reset service
No
No
~/reset (no restart needed)

Maintenance
Reduced since 2023
Active
Active, 24h response

License
BSD-3
BSD-3
Apache 2.0

ROS 2 Jazzy
Ported from ROS 1
Native
Native, from scratch

• ROS 2 Jazzy Jalisco (primary) or ROS 2 Kilted (community tested)
• A colcon workspace (~/ros2_ws)

Clone into your workspace

This is a monorepo with 4 independent ament_cmake packages: compass_msgs, fusioncore_core, fusioncore_ros, and fusioncore_gazebo. Each has its own package.xml. Colcon finds them by scanning src/ recursively. The repo root has no package.xml and is not itself a package. The repo must live inside src/ for colcon to find the packages.

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
rosdep install –from-paths src –ignore-src -r -y
colcon build
source install/setup.bash

Real robot users (no Gazebo): fusioncore_gazebo depends on ros_gz_sim which pulls in Gazebo and its GUI components. On headless machines (Raspberry Pi, server) this install is large, unnecessary, and may fail. Skip it by adding a COLCON_IGNORE file before building:

touch ~/ros2_ws/src/fusioncore/fusioncore_gazebo/COLCON_IGNORE

This tells colcon to skip that package entirely. fusioncore_core and fusioncore_ros have no Gazebo dependency and build fine without it.

cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build –packages-select fusioncore_core –cmake-args -DBUILD_TESTING=ON
colcon test –packages-select fusioncore_core
colcon test-result –verbose

Expected output: 39 tests, 0 errors, 0 failures, 0 skipped

# Terminal 1: launch the node
ros2 launch fusioncore_ros fusioncore.launch.py

# Terminal 2: configure and activate the lifecycle node
ros2 lifecycle set /fusioncore configure
ros2 lifecycle set /fusioncore activate

# Verify it’s publishing at 100Hz
ros2 topic hz /fusion/odom
# expected: average rate: 100.000

FusionCore uses a ROS 2 lifecycle node. Configure first (load parameters, validate TF tree, check transforms), then activate (start processing sensor data). This prevents the filter from starting with bad initial values or missing transforms.

WSL2 note: If ros2 lifecycle set returns “Node not found”, use the launch file’s built-in auto-configure instead. The Gazebo launch file (fusioncore_gazebo.launch.py) configures and activates the node automatically via EmitEvent(ChangeState(…)) 15 seconds after startup, bypassing DDS discovery latency that affects WSL2.

Verifying all features work

You can test every FusionCore feature without a physical robot using fake sensor data. Replace ~/YOUR_WS with your actual workspace path (e.g. ~/ros2_ws, ~/fusioncore_ws). Open 4 terminals:

Terminal 1: Launch FusionCore:

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 launch fusioncore_ros fusioncore.launch.py

Terminal 2: Configure and activate:

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash

# Publish required TF transforms (stays running in background)
ros2 run tf2_ros static_transform_publisher –frame-id base_link –child-frame-id imu_link &
ros2 run tf2_ros static_transform_publisher –frame-id odom –child-frame-id base_link &
sleep 1

ros2 lifecycle set /fusioncore configure
sleep 1
ros2 lifecycle set /fusioncore activate

Terminal 3: Feed fake sensors:

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash

# Fake IMU at 100Hz (stationary, gravity pointing up)
ros2 topic pub /imu/data sensor_msgs/msg/Imu “{
header: {frame_id: ‘base_link’},
angular_velocity: {x: 0.0, y: 0.0, z: 0.0},
linear_acceleration: {x: 0.0, y: 0.0, z: 9.81},
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
}“ –rate 100 &

# Fake wheel encoder at 50Hz (stationary: triggers ZUPT)
ros2 topic pub /odom/wheels nav_msgs/msg/Odometry “{
header: {frame_id: ‘odom’},
twist: {twist: {linear: {x: 0.0}, angular: {z: 0.0}}}
}“ –rate 50 &

# Fake GPS at 5Hz (Hamilton, Ontario)
ros2 topic pub /gnss/fix sensor_msgs/msg/NavSatFix “{
header: {frame_id: ‘base_link’},
status: {status: 0},
latitude: 43.2557,
longitude: -79.8711,
altitude: 100.0,
position_covariance: [1.0, 0, 0, 0, 1.0, 0, 0, 0, 4.0],
position_covariance_type: 2
}“ –rate 5

Terminal 4: Verify each feature:

Check what topics and services are live:

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic list | grep fusion
ros2 service list | grep fusioncore

You should see /fusion/odom, /fusion/pose, and /fusioncore/reset.

Test /fusion/pose: what Nav2, AMCL, and slam_toolbox expect:

ros2 topic echo /fusion/pose –once

You should see a pose message with a full 6×6 covariance matrix from the UKF.

Test /diagnostics: per-sensor health at 1Hz:

ros2 topic echo /diagnostics –once

You should see 4 status entries: fusioncore: IMU, fusioncore: Encoder, fusioncore: GNSS, fusioncore: Filter. Each shows OK or WARN with outlier counts and heading status.

Test ZUPT: velocity should stay near zero while stationary:

ros2 topic echo /fusion/odom –field twist.twist.linear

Values should be essentially zero (~1e-10) even while the IMU is running. This confirms ZUPT is suppressing velocity drift when the robot is not moving.

Test the reset service: reinitializes filter without restarting the node:

ros2 service call /fusioncore/reset std_srvs/srv/Trigger

Expected: success: True, message: ‘FusionCore filter reset. GPS reference cleared.’

Within 1 second of calling reset, the node log (Terminal 1) should print:

Filter reset via ~/reset service.
GNSS reference set: lat=43.255700 lon=-79.871100

with no GNSS fix rejected warnings. GPS re-fuses immediately after reset.

Subscribes to:

Topic
Type
What it is

/imu/data
sensor_msgs/Imu
IMU angular velocity and linear acceleration

/odom/wheels
nav_msgs/Odometry
Wheel encoder velocity

/gnss/fix
sensor_msgs/NavSatFix
GPS position

/gnss/heading
sensor_msgs/Imu
Dual antenna heading (optional)

gnss.azimuth_topic
compass_msgs/Azimuth
Azimuth heading (optional, preferred standard)

gnss.fix2_topic
sensor_msgs/NavSatFix
Second GPS receiver (optional)

Publishes:

Topic
Type
What it is

/fusion/odom
nav_msgs/Odometry
Fused position + orientation + velocity + covariance at 100Hz

/fusion/pose
geometry_msgs/PoseWithCovarianceStamped
Same pose: compatible with AMCL, slam_toolbox, Nav2 pose initializer

/diagnostics
diagnostic_msgs/DiagnosticArray
Per-sensor health, outlier counts, heading status at 1Hz

/tf
TF
odom -> base_link for Nav2

Services:

Service
Type
What it does

~/reset
std_srvs/Trigger
Re-initializes the filter and clears the GPS reference anchor without restarting the node. Useful after teleportation in simulation or after a catastrophic GPS jump in the field.

fusioncore:
ros__parameters:
base_frame: base_link
odom_frame: odom
publish_rate: 100.0

imu.gyro_noise: 0.005 # rad/s: from your IMU datasheet
imu.accel_noise: 0.1 # m/s²
imu.has_magnetometer: false # true for 9-axis IMUs (BNO08x, VectorNav, Xsens)
# false for 6-axis: yaw from gyro integration drifts
imu.remove_gravitational_acceleration: false # set true if robot drifts in Z while stationary
# most IMUs report raw specific force (gravity included)
# FusionCore removes gravity using current filter orientation

encoder.vel_noise: 0.05 # m/s
encoder.yaw_noise: 0.02 # rad/s

gnss.base_noise_xy: 1.0 # meters: scaled automatically by HDOP
gnss.base_noise_z: 2.0 # meters
gnss.heading_noise: 0.02 # rad: for dual antenna
gnss.max_hdop: 4.0 # reject fixes worse than this
gnss.min_satellites: 4
gnss.min_fix_type: 1 # minimum fix quality: 1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED
# note: sensor_msgs/NavSatFix status=2 maps to RTK_FIXED only.
# RTK_FLOAT (3) is unreachable via NavSatFix: use 2 or 4.

# Antenna lever arm: offset from base_link to primary GPS antenna in body frame
# x=forward, y=left, z=up (meters). Leave at 0 if antenna is above base_link.
# Lever arm correction only activates when heading is independently validated.
gnss.lever_arm_x: 0.0
gnss.lever_arm_y: 0.0
gnss.lever_arm_z: 0.0

# Second GPS receiver lever arm (if using gnss.fix2_topic)
gnss.lever_arm2_x: 0.0
gnss.lever_arm2_y: 0.0
gnss.lever_arm2_z: 0.0

# Optional second GPS receiver
gnss.fix2_topic: ““

# Heading topics: pick one or both
gnss.heading_topic: “/gnss/heading“ # sensor_msgs/Imu
gnss.azimuth_topic: ““ # compass_msgs/Azimuth (preferred)

# Mahalanobis outlier rejection
outlier_rejection: true
outlier_threshold_gnss: 16.27 # chi2(3, 0.999): 3D position
outlier_threshold_hdg: 10.83 # chi2(1, 0.999): 1D heading
outlier_threshold_enc: 11.34 # chi2(3, 0.999): 3D encoder
outlier_threshold_imu: 15.09 # chi2(6, 0.999): 6D IMU

# Adaptive noise covariance
adaptive.imu: true
adaptive.encoder: true
adaptive.gnss: true
adaptive.window: 50
adaptive.alpha: 0.01

ukf.q_position: 0.01
ukf.q_orientation: 1.0e-9 # quaternion regularization only: do NOT increase this
ukf.q_velocity: 0.1
ukf.q_angular_vel: 0.1
ukf.q_acceleration: 1.0
ukf.q_gyro_bias: 1.0e-5
ukf.q_accel_bias: 1.0e-5

Upgrading from an older config? If your YAML has ukf.q_orientation: 0.01, change it to 1.0e-9 or delete the line. The old value corrupts quaternion math at typical IMU rates and causes yaw drift and Z-axis rise in simulation.

GPS Coordinate Reference System (CRS)

FusionCore uses PROJ to convert incoming GNSS fixes between coordinate systems. The defaults handle any standard GPS receiver (WGS84 lat/lon → ECEF). Change these only if your receiver outputs a different CRS.

# PROJ coordinate reference system
input.gnss_crs: “EPSG:4326“ # CRS of incoming NavSatFix messages
# EPSG:4326 = WGS84 lat/lon (standard GPS)
# EPSG:32617 = UTM zone 17N (some RTK receivers)
output.crs: “EPSG:4978“ # internal computation CRS
# EPSG:4978 = ECEF XYZ (default, globally valid)
output.convert_to_enu_at_reference: true # true when output.crs is ECEF
# false when output.crs is already a local projected CRS
reference.use_first_fix: true # anchor local ENU origin to first GPS fix
reference.x: 0.0 # fixed origin in output.crs (when use_first_fix: false)
reference.y: 0.0
reference.z: 0.0

Agricultural RTK example: receiver outputs UTM zone 17N (easting/northing) directly:

input.gnss_crs: “EPSG:32617“
output.crs: “EPSG:32617“
output.convert_to_enu_at_reference: false
reference.use_first_fix: true

How FusionCore handles the hard problems

IMUs are almost never mounted at base_link. FusionCore reads frame_id from every IMU message, looks up the TF rotation to base_link, and rotates angular velocity and linear acceleration before fusing. If the transform is missing you get the exact command to fix it:

[WARN] Cannot transform IMU from imu_link to base_link.
Fix: ros2 run tf2_ros static_transform_publisher –frame-id base_link –child-frame-id imu_link

During configure, FusionCore checks that all required TF transforms exist before the filter starts. Missing transforms print the exact fix command: no silent failures, no mysterious drift:

— TF Validation —
[OK] imu_link -> base_link
[MISSING] base_link -> odom Fix: ros2 run tf2_ros static_transform_publisher –frame-id odom –child-frame-id base_link
———————

Mahalanobis outlier rejection

Before fusing any GPS fix, FusionCore computes how statistically implausible the measurement is given the current state estimate. The Mahalanobis distance d² = νᵀ · S⁻¹ · ν is compared against chi-squared thresholds at the 99.9th percentile. Fixes that exceed the threshold are rejected without updating the filter.

This handles GPS jumps, multipath errors, and encoder slip spikes. The filter position stays stable during rejection: verified by injecting a 500m GPS jump in testing and observing zero position change.

GNSS position covariance is floored before the gate is evaluated. This prevents RTK-grade receivers (typical σxy ~3mm) from triggering self-rejection when the filter has not yet converged to RTK-level accuracy.

Zero velocity updates (ZUPT)

When the robot is stationary: encoder speed below 0.05 m/s and angular rate below 0.05 rad/s: FusionCore fuses a zero velocity pseudo-measurement with very tight noise. This stops the IMU from drifting the velocity estimate while the robot is sitting still. Every serious inertial navigation system does this. Without ZUPT, IMU noise accumulates into a false velocity estimate over time even when the robot has not moved.

All MEMS IMUs have a small accelerometer and gyro bias that is unknown at startup. By default the filter learns it over ~60 seconds, causing a small position offset at startup. Setting init.stationary_window: 2.0 makes the filter collect 2 seconds of IMU data before starting, estimate the bias directly, and initialize with the correct values: reducing the startup transient from ~10cm to under 1cm. The robot must be stationary during the window; if it moves, the filter falls back to zero bias automatically.

Adaptive noise covariance

FusionCore tracks a sliding window of 50 innovation sequences per sensor and estimates the actual noise covariance from the data. The noise matrix R is slowly updated toward the estimated true value using an exponential moving average with alpha=0.01. After a few minutes of operation, R converges to the real sensor characteristics automatically. No manual YAML tuning required.

GPS antenna offset (lever arm)

If the GPS antenna is not at base_link: mounted on top of the robot, forward of center: its readings correspond to a different trajectory than base_link. FusionCore corrects for this using the rotation matrix from the current state: p_antenna = p_base + R * lever_arm. Lever arm correction only activates when heading has been independently validated: applying it with wrong heading makes things worse, not better.

Each GPS receiver has its own independent lever arm. Primary receiver uses gnss.lever_arm_x/y/z, secondary receiver uses gnss.lever_arm2_x/y/z.

FusionCore tracks a heading_validated_ flag that is only set true from a genuine independent source:

• DUAL_ANTENNA: dual antenna heading message received
• IMU_ORIENTATION: 9-axis AHRS published full orientation (only when imu.has_magnetometer: true: 6-axis IMUs drift in yaw and don’t count)
• GPS_TRACK: robot has traveled >= 5 meters at speed >= 0.2 m/s with yaw rate <= 0.3 rad/s

Before any of these, lever arm is disabled regardless of what yaw variance says.

FusionCore maps sensor_msgs/NavSatFix.status to an internal fix type enum and rejects fixes below a configurable minimum quality. The default accepts any valid GPS fix. Set to 4 to require RTK_FIXED:

gnss.min_fix_type: 4 # require RTK_FIXED: reject basic GPS entirely

When a fix is rejected due to quality, the rejection log shows the fix type and threshold:

[WARN] GNSS fix rejected (fix_type=1, min=4, hdop=1.20, quality check or Mahalanobis gate)

Important: sensor_msgs/NavSatFix has no STATUS_RTK_FLOAT. Status 2 maps to RTK_FIXED. Setting min_fix_type: 3 will silently starve the filter. Use 2 or 4 as meaningful thresholds.

FusionCore publishes /diagnostics at 1Hz compatible with rqt_robot_monitor and Nav2. Four status entries: IMU, Encoder, GNSS, Filter. Each shows OK or WARN with outlier counts, heading source, distance traveled, position uncertainty, and update count.

ros2 service call /fusioncore/reset std_srvs/srv/Trigger

Reinitializes the UKF state and clears the GPS reference anchor. The robot re-anchors on the next GPS fix. No node restart required.

FusionCore uses the covariance values sensors actually publish. GPS: full 3×3 matrix when position_covariance_type == 3. Wheel odometry: reads twist.covariance per-axis. IMU orientation: reads orientation_covariance from the message.

FusionCore stores a ring buffer of 100 IMU messages (1 second at 100Hz). When a delayed GPS fix arrives, it restores the closest state snapshot before the fix timestamp, re-fuses the fix at the correct time, then replays all buffered IMU messages forward to now. This eliminates motion-model approximation error for delayed measurements.

Non-holonomic ground constraint

For wheeled ground robots, FusionCore fuses a VZ = 0 pseudo-measurement on every encoder update. This prevents vertical velocity from drifting due to IMU noise. Do not use for aerial vehicles or robots that move vertically.

FusionCore tracks the last update time for each sensor independently. If a sensor goes silent for longer than stale_timeout (default 1.0 second), get_status() returns SensorHealth::STALE for that sensor. The filter continues running on the remaining sensors and recovers automatically when the missing sensor resumes.

FusionCore ships a ROS 2 native port of compass_msgs/Azimuth (upstream is ROS 1 only). Handles ENU/NED convention conversion, RAD/DEG units, and warns when magnetic north reference is used instead of geographic.

FusionCore ships with a Gazebo Harmonic simulation world so you can test the full fusion pipeline without physical hardware. It includes a differential drive robot with a 100Hz IMU and GPS, in an outdoor environment with the GPS origin set to Hamilton, Ontario.

Gazebo Harmonic’s built-in NavSat sensor has a known bug (gz-sim issue #2163) where it periodically outputs GPS fixes at completely wrong coordinates. Rather than fight a broken sensor, the simulation derives GPS from Gazebo’s ground truth world pose and adds realistic Gaussian noise (0.5m horizontal, 0.3m vertical 1-sigma).

Prerequisites for simulation

Gazebo Harmonic and the ROS-Gazebo bridge are not installed by rosdep automatically. Install them first:

sudo apt install ros-jazzy-ros-gz

cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build
source install/setup.bash
ros2 launch fusioncore_gazebo fusioncore_gazebo.launch.py

Drive the robot and watch the fused position:

# Terminal 2: drive in a circle
source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist “{linear: {x: 0.5}, angular: {z: 0.3}}“ –rate 10

# Terminal 3: watch position
source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic echo /fusion/odom –field pose.pose.position

python3 ~/ros2_ws/src/fusioncore/fusioncore_gazebo/launch/integration_test.py

Four automated tests: IMU drift rate, outlier rejection, GPS correction after drift, full circle return. All four pass on a clean session.

Hardware configs in fusioncore_ros/config/: noise values pulled from datasheets, comments explain every parameter:

Combine any hardware config with an environment preset (env_open.yaml, env_urban.yaml, env_canopy.yaml) to tune GPS trust for your operating conditions without touching the hardware config.

To add your robot’s config, open a Hardware Config Request or submit a PR: see CONTRIBUTING.md.

Migrating from robot_localization

Complete parameter mapping, topic changes, and step-by-step instructions: docs/migration_from_robot_localization.md

Using FusionCore with Nav2

FusionCore is a drop-in odometry source for Nav2. It publishes everything Nav2 needs out of the box: no remapping, no extra nodes.

What FusionCore publishes that Nav2 uses:

FusionCore output
Nav2 use

/fusion/odom (nav_msgs/Odometry)
Set as odom_topic in nav2_params.yaml

odom → base_link TF
Nav2 reads this directly: no config needed

/fusion/pose (PoseWithCovarianceStamped)
AMCL initial pose, slam_toolbox pose input

/diagnostics
Nav2-compatible diagnostic format

Step 1: Point Nav2 at FusionCore’s odometry:

In your nav2_params.yaml, set odom_topic to /fusion/odom wherever it appears (typically amcl, bt_navigator, velocity_smoother):

amcl:
ros__parameters:
odom_topic: /fusion/odom

bt_navigator:
ros__parameters:
odom_topic: /fusion/odom

velocity_smoother:
ros__parameters:
odom_topic: /fusion/odom

Step 2: Launch FusionCore alongside Nav2:

fusioncore_nav2.launch.py handles the full sequence: starts FusionCore, configures it, activates it, then starts Nav2 once the TF is live.

ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
fusioncore_config:=/path/to/your/fusioncore.yaml \
nav2_params:=/path/to/your/nav2_params.yaml

With an environment preset:

ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
fusioncore_config:=/path/to/your/fusioncore.yaml \
nav2_params:=/path/to/your/nav2_params.yaml \
env_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/env_urban.yaml

The launch file configures FusionCore after 2 s, activates it on the configuring → inactive transition, then starts Nav2 after 5 s: guaranteeing odom → base_link TF is publishing before Nav2’s costmaps initialize.

That’s it. No additional nodes, no coordinate transforms, no remapping. FusionCore’s odom → base_link TF is what Nav2’s costmaps and planners track. GPS waypoint navigation via Nav2’s fromLL service works automatically once FusionCore has a GPS fix.

For GPS waypoint navigation (nav2_waypoint_follower with fromLL):

# FusionCore exposes the fromLL service once it has a GPS fix
ros2 service call /fromLL fusioncore_ros/srv/FromLL \
“{ll_point: {latitude: 43.2557, longitude: -79.8711, altitude: 0.0}}“

fusioncore/
├── fusioncore_core/ # Pure C++17 math library. Zero ROS dependency.
│ ├── include/fusioncore/
│ │ ├── ukf.hpp # Unscented Kalman Filter: 45 sigma points
│ │ ├── state.hpp # 22-dimensional state vector (quaternion orientation)
│ │ ├── fusioncore.hpp # Public API: FusionCore, FusionCoreConfig
│ │ └── sensors/
│ │ ├── imu.hpp # Raw IMU + orientation measurement models
│ │ ├── encoder.hpp # Wheel encoder measurement model
│ │ └── gnss.hpp # GPS: ECEF, lever arm, covariance, quality gating
│ └── src/
│ ├── ukf.cpp # UKF: sigma points, predict, update, predict_measurement
│ └── fusioncore.cpp # Manager: outlier rejection, adaptive noise,
│ # snapshots, observability, delay compensation
├── fusioncore_ros/ # ROS 2 Jazzy wrapper
│ ├── src/fusion_node.cpp # Lifecycle node: all sensor callbacks, TF validation,
│ │ # ZUPT, diagnostics, /fusion/pose, reset service
│ ├── config/fusioncore.yaml # Default configuration
│ ├── config/duatic_mecanum.yaml
│ └── launch/fusioncore.launch.py
└── fusioncore_gazebo/ # Simulation world
├── worlds/fusioncore_test.sdf
├── models/fusioncore_robot/
├── launch/fusioncore_gazebo.launch.py
├── launch/gz_pose_to_gps.py
└── launch/integration_test.py

• Filter: Unscented Kalman Filter, 45 sigma points
• State vector: 22-dimensional: position (x,y,z), orientation (quaternion qw,qx,qy,qz), linear velocity, angular velocity, linear acceleration, gyroscope bias (x,y,z), accelerometer bias (x,y,z)
• GPS coordinate system: Configurable via PROJ: default ECEF (EPSG:4978, globally valid); supports any PROJ-compatible input CRS including UTM zones
• Bias estimation: Continuous online estimation, no calibration required
• GPS quality scaling: Noise covariance scaled by HDOP/VDOP, or full 3×3 message covariance when available
• Outlier rejection: Mahalanobis chi-squared gating at 99.9th percentile per sensor dimension
• Adaptive noise: Sliding window innovation tracking, exponential moving average R update
• Delay compensation: IMU ring buffer replay retrodiction up to 500ms
• ZUPT: Automatic zero velocity updates when stationary
• Output rate: 100Hz
• Language: C++17
• License: Apache 2.0

Working and tested:

• Hardware testing in progress: industrial mecanum manipulator (Duatic), agricultural RTK robot (Southern Ontario)
• UKF core: 39 unit tests passing via colcon test
• UKF numerical stability: P symmetrization + identity-shift Cholesky repair + angular velocity variance cap
• IMU + encoder + GPS fusion
• Automatic IMU bias estimation
• ECEF GPS conversion with quality-aware noise scaling
• Dual antenna heading: both sensor_msgs/Imu and compass_msgs/Azimuth
• IMU frame transform via TF
• TF validation at startup with exact fix commands
• GPS lever arm with heading observability guard: independent params for primary and secondary receivers
• Full 3×3 GPS covariance support
• Wheel odometry covariance support
• Multiple GPS receivers
• Heading observability tracking: DUAL_ANTENNA / IMU_ORIENTATION / GPS_TRACK
• GPS fix quality gating: configurable gnss.min_fix_type (GPS / DGPS / RTK_FIXED)
• Mahalanobis outlier rejection: GPS jumps verified rejected in testing
• Adaptive noise covariance: automatic R estimation from innovation sequence
• GPS delay compensation: full IMU replay retrodiction up to 500ms
• Non-holonomic ground constraint: VZ=0 pseudo-measurement for wheeled robots
• Zero velocity updates (ZUPT): automatic when encoder speed < 0.05 m/s
• Per-sensor diagnostics: /diagnostics at 1Hz with outlier counts and heading status
• /fusion/pose: PoseWithCovarianceStamped for Nav2 / AMCL / slam_toolbox
• Filter reset service: ~/reset clears filter and GPS reference without node restart
• Sensor dropout detection: per-sensor staleness tracking via SensorHealth enum
• PROJ CRS coordinate transform: configurable input/output CRS via PROJ library (WGS84, UTM, ECEF, any EPSG code)
• ROS 2 Jazzy lifecycle node at 100Hz
• Gazebo Harmonic simulation world

Known limitations:

• GNSS antenna lever arm is fixed and known: does not estimate it from data
• In Gazebo simulation, residual y-axis drift (~0.3m) can occur from Gazebo physics: not a filter error
• Mecanum drive lateral velocity is not predicted by the motion model

Roadmap:

• Ackermann and omnidirectional steering motion models
• Mecanum drive motion model
• Auto-derive GNSS lever arm from TF header.frame_id

Apache 2.0. Includes explicit patent license grant that BSD-3 does not provide. Commercially safe

Issues answered within 24 hours. Open a GitHub issue or find the original discussion on ROS Discourse.

This project exists because of a community thread from December 2024 asking for a robot_localization alternative with native ROS 2 Jazzy support. If you hit a problem: open an issue. That feedback drives the roadmap.