FusionCore UKF: fusing ZED IMU and visual odometry for stable mobile robot localization

Software
1

Hey everyone,

I’ve been building a ROS 2 state estimator called FusionCore and wanted to share how it integrates with ZED cameras for mobile robot setups. @Myzhar pointed me here after I opened an issue (#429) on the ZED ROS2 wrapper.

What FusionCore does?

FusionCore is a 22-state UKF that fuses IMU, wheel odometry, visual odometry, and optional GPS into a single clean odom → base_link estimate. It runs gyro and accelerometer bias estimation in the filter state, adapts its noise covariance automatically, and gates outliers with a chi-squared test per sensor.

1111
11111305×758 53.3 KB

How it works with ZED

The ZED node publishes two things FusionCore can consume directly:

  • /zed/zed_node/imu/data → FusionCore IMU input

  • /zed/zed_node/odom → FusionCore secondary odometry (encoder2.topic)

If you also have wheel encoders, those go in as the primary source and ZED visual odometry becomes the secondary. FusionCore outputs /fusion/odom (nav_msgs/Odometry) and broadcasts the odom → base_link TF at 100Hz.

image
image782×558 31.1 KB

Config for ZED + wheel encoders

fusioncore:
  ros__parameters:
    base_frame: base_link
    odom_frame: odom
    publish_rate: 100.0
    publish.force_2d: true

    imu.has_magnetometer: false
    imu.gyro_noise: 0.005
    imu.accel_noise: 0.1
    imu.frame_id: "zed_imu_link"

    encoder.vel_noise: 0.05
    encoder.yaw_noise: 0.02

    encoder2.topic: "/zed/zed_node/odom"

    outlier_rejection: true
    outlier_threshold_gnss: 16.27
    outlier_threshold_imu: 15.09
    outlier_threshold_enc: 11.34
    outlier_threshold_hdg: 10.83

    adaptive.imu: true
    adaptive.encoder: true
    adaptive.gnss: true

    reference.use_first_fix: false
    reference.x: 0.0
    reference.y: 0.0
    reference.z: 0.0

Starting FusionCore

FusionCore is a ROS 2 lifecycle node: it needs to go through configure → activate before it starts publishing. Two ways to handle this:

If you use Nav2:

ros2 launch fusioncore_ros fusioncore_nav2.launch.py
fusioncore_config:=/path/to/zed_robot.yaml

The launch file handles configure and activate automatically, then starts Nav2 once FusionCore is publishing. Update your Nav2 config to read from /fusion/odom instead of /odometry/filtered.

If you don’t use Nav2:

# Terminal 1 — start the node with remaps
ros2 run fusioncore_ros fusioncore_node --ros-args \
  --params-file /path/to/zed_robot.yaml \
  -r /odom/wheels:=/your/wheel/odom \
  -r /imu/data:=/zed/zed_node/imu/data

# Terminal 2 — trigger lifecycle transitions
ros2 lifecycle set /fusioncore configure
ros2 lifecycle set /fusioncore activate

After activating, /fusion/odom should be publishing and ros2 topic hz /fusion/odom should show ~100Hz.

Full lifecycle documentation: https://manankharwar.github.io/fusioncore/getting-started/

TF architecture

ZED → /zed/zed_node/imu/data  ─┐
ZED → /zed/zed_node/odom      ─┤→ FusionCore → odom → base_link TF
Wheels → /odom/wheels         ─┘              → /fusion/odom

SLAM (rtabmap, slam_toolbox)  → map → odom TF

FusionCore sits underneath your SLAM stack. They don’t conflict.

Adding GPS for outdoor robots

Add a GPS receiver and FusionCore fuses all three: ZED IMU, visual odometry, and GPS… into a global-frame estimate. No UTM projection, no separate navsat_transform node. Add to your config:

    input.gnss_crs: "EPSG:4326"
    output.crs: "EPSG:4978"
    output.convert_to_enu_at_reference: true
    reference.use_first_fix: true

Then remap /gnss/fix to your GPS topic. Full GPS configuration guide: https://manankharwar.github.io/fusioncore/configuration/

Trajectory

Where it helps most with ZED

ZED visual odometry is excellent but can drift briefly during fast rotation, low-texture scenes, or lighting transitions. FusionCore bridges those gaps using IMU dead-reckoning and wheel encoder velocity. The result is an odometry signal that doesn’t stutter when the camera loses tracking momentarily.

Happy to answer questions about specific ZED setups.

2

Hi @manankharwar
Welcome to the StereoLabs community.

Fusion of odometry data from multiple sensors with a ZED camera is a frequent request in this forum.

Your solution looks promising, and it would be great if other members of our community could test and validate it in different environments and operating conditions.

We look forward to hearing the new results.

1 Like
3

Thanks @Myzhar! Happy to be here.

If anyone in the community runs into issues getting it configured with their specific ZED setup… different camera models, frame IDs, or sensor combinations…. feel free to post here or open an issue on GitHub. I’ll respond quickly

4

Your package seems really promising, especially as a solution to the growing pains of integrating robot_localization in modern systems. I am going to be spending this week evaluating and comparing your FusionCore against Wolf, TIER IV’s EagleEye and Robot Localization for Ackermann (car-like) vehicles. I will test on two platforms:

  1. An F1/10 scale car used indoors with a wheel encoder, IMUs (from VESC and RealSense D435i), and either VSLAM or LaserScan as a secondary encoder.
  2. 2. A full-size van with a GPS, Zed 2i, IMU, and optionally wheel encoder or by-wire odometry. This also has a 360 LIDAR but I don’t think that is relevant to this test or your package.

I read through your documentation, and it seems you already have support for modifying the vehicle model to Ackermann, so I will use that.

  1. Any tips for indoor vs. outdoor configurations? For context, the wheel encoder (odom from wheels and steering servo) does not output reliable covariance. We also have odometry from LaserScans and PointClouds (for the van). I am happy to propose improvements and contribute to your repository.
  2. Also, since your package is meant to be used primarily with GPS + IMU + wheel encoder, can the /tf publication be disabled if another node fuses all the odom topics/tfs into one? How does it perform without GPS, i.e., indoor cases? It handles multiple “encoders,” but how about multiple IMUs, or does that require starting up another instance/node of the FusionCore (which isn’t a major issue)?
  3. What is the CPU consumption and expected publication frequency on a Jetson Orin Nano and a high-powered workstation? In your documentation, you state 100 Hz, but the robot_localization package claims north of 100 Hz but struggles to even run at 10 Hz on a Jetson Orin Nano with 4 sensors.
2 Likes
5

Hi @privvyledge, really glad you’re running this comparison. Let me go through everything.

Ackermann support

Yes, supported. Set in your config:

motion_model: "Ackermann"
motion_model_params.wheelbase: 0.32   # F1/10: measure front-to-rear axle centre in metres

For the van, swap the wheelbase to your vehicle spec (Ford Transit 130" variant is ~3.30m, 148" is ~3.75m).

One thing to be upfront about: the current Ackermann prediction applies the non-holonomic constraint (zero lateral velocity and acceleration each step), same as DifferentialDrive. The wheelbase is stored for a planned feature where steering angle is used as a direct input, but it does not affect the prediction today. For your setup where VESC and ZED already publish processed twist.angular.z, the constraint is what matters and it works correctly.

Disabling /tf publication

Just pushed publish.tf: false. Pull latest main and set it wherever another node owns the odom -> base_link transform:

publish.tf: false   # /fusion/odom still publishes normally

Indoor without GPS

Works fine. Set reference.use_first_fix: false and FusionCore starts at origin, running entirely on IMU and wheel encoder. For longer runs, plugging in a second velocity source via encoder2.topic reduces drift significantly since there is no global anchor. ZUPT helps too: when the filter detects standstill it locks velocity to zero, which stops IMU bias from drifting during stops.

Unreliable covariance from wheel encoder

Handled automatically. FusionCore checks twist.covariance on every message. If it is zero or negative (VESC default), it falls back to the encoder.vel_noise and encoder.yaw_noise values in your config. No driver patching needed.

LaserScan and PointCloud as encoder2

FusionCore’s encoder2 input expects nav_msgs/Odometry with velocity in the twist field, not raw scans or clouds. If you run KISS-ICP or another scan-matching node on your LiDAR, its odometry output (/kiss/odometry) plugs straight in:

encoder2.topic: "/kiss/odometry"
encoder2.vel_noise: 0.02    # ICP is tighter than wheel odom
encoder2.yaw_noise: 0.005

Raw LaserScan or PointCloud needs to go through a scan-matching node first. The node does the geometry; FusionCore fuses the resulting velocity.

Multiple IMUs

One IMU per FusionCore instance. For the F1/10 with both a VESC IMU and a RealSense D435i: use the D435i as primary. It has a lower noise floor and more stable axes at high angular rates. Remap at launch:

-r /imu/data:=/camera/imu        # realsense2_camera v3.x
# or
-r /imu/data:=/camera/camera/imu  # realsense2_camera v4.x on Humble

If you genuinely need both fused, run imu_filter_madgwick to merge them into one topic first, then feed that to FusionCore.

CPU on Jetson Orin Nano

I don’t have a profiling number from a Jetson specifically yet. Structurally: the UKF propagates 45 sigma points per cycle (2n+1 for n=22 states), no dynamic allocation in the hot path, runs at 100Hz. It should be meaningfully lighter than robot_localization’s EKF on embedded hardware but I can’t give you a real number until I profile it. If you can share your CPU usage from the evaluation that would be very useful, and I’ll run a proper Jetson profile to add to the docs.

Everything is on main now, here is exactly where to look

Pull the latest main and you’ll have everything:

git pull origin main
colcon build --packages-select fusioncore_ros

Things to pay attention to specifically for your setup:

  1. The D435i IMU topic changed between driver versions. Use /camera/imu with realsense2_camera v3.x, or /camera/camera/imu with v4.x on Humble. Check with ros2 topic list | grep imu after launching the camera node.

  2. The van config sets imu.remove_gravitational_acceleration: true because ZED’s driver subtracts gravity before publishing. Most IMU drivers do not. If you swap to a different IMU on the van, check linear_acceleration.z at rest: ~9.8 means set it false, ~0.0 means set it true.

  3. encoder2.vel_noise and encoder2.yaw_noise are separate from the primary encoder noise, so you can tune ICP and wheel odom independently.

Ready-to-use configs (just updated on main)

One recent fix worth knowing about: if you use init.stationary_window: 2.0 (both platform configs have it enabled), make sure you’re on latest main. There was a bug where the stationary window hung indefinitely if the IMU driver published zero timestamps at startup. Fixed in 79a4753, wall clock is used now instead of message timestamps.

Both have inline notes on topic remaps, wheelbase measurement, and covariance handling. The van config sets imu.remove_gravitational_acceleration: true since ZED’s driver subtracts gravity before publishing (opposite of most IMU drivers).

Would love to see your results. Happy to tune configs during your evaluation if something looks off, and contributions are very welcome.

One more thing: FusionCore is already running on ground agricultural robots and I’m working with Nature Robotics and Clearpath Robotics as hardware partners. You’re exactly the kind of tester I want involved, and from what you’ve described (F1/10 racing platform plus a full-size van with real-world GPS and LiDAR) this is a genuinely valuable evaluation.

I know there will be a lot of companies and teams reaching out as the project grows, but I’d rather have a direct conversation with you specifically. Not a support thread: a proper back-and-forth where I understand exactly what your stack looks like and can make sure FusionCore works precisely for your use case. If something is missing I will add it. If something needs tuning I’ll work through it with you.

If you’re open to it, happy to continue over DM.

2 Likes
6

One of the most requested features from evaluators was the ability to fuse two IMUs simultaneously. That is now live on main.

imu2.topic: "/vesc/imu"
imu2.frame_id: ""
imu2.remove_gravitational_acceleration: false

FusionCore treats both sensors as independent measurements of the same state. No pre-merging with imu_filter_madgwick required. The f1tenth_indoor.yaml config already has the parameter commented in with an example. Full docs at the Ackermann hardware page under "Which IMU to use if you have two.

1 Like