Sensor Fusion on LeKiwi

I've been gradually converting my LeKiwi mobile robot to run using ROS 2, and upgrading the original hardware piece by piece. Over the past few months, I've added a LiDAR, a pan-tilt mechanism with a depth camera, and wired up wheel odometry using ros2_control.

The wheel odometry works well (especially on short runs), thanks to the STS3215 servo motors providing accurate and reliable state feedback. But omnidirectional robots have a fundamental problem with dead reckoning: wheels slip sideways, and you can physically move the base without any encoder movement. Once the heading drifts, the dead-reckoned position estimate diverges along with it. You need something to anchor the heading.

And that something, in most cases, is an IMU. I recently added a BNO055 IMU and vibe-coded a ros2_control hardware interface for it using the bno055_hardware_interface package. Now it is time to integrate it into the LeKiwi stack and fuse measurements with wheel odometry using an Extended Kalman Filter from robot_localization. Later in this post, I also compare robot_localization with the newer fusioncore project.

Integrating the BNO055 IMU

The BNO055 is a 9-DOF absolute orientation sensor that does on-chip sensor fusion. It combines an accelerometer, gyroscope, and magnetometer, and outputs a quaternion in NDOF (Nine Degrees of Freedom) mode. The output is gravity-referenced and utilizes the magnetometer to correct drift, ensuring the orientation remains accurate over time without accumulating errors that can occur with raw gyro integration. This post focuses on the sensor fusion side and assumes you're already familiar with the hardware interface implementation covered in the earlier post, at least for this section.

The ros2_control Hardware Interface

I added the <ros2_control> block to the URDF in lekiwi_description, declaring the BNO055 as a sensor type — read-only, with no command interfaces:

<ros2_control name="lekiwi_imu" type="sensor">
  <hardware>
    <plugin>bno055_hardware_interface/BNO055HardwareInterface</plugin>
    <param name="i2c_bus">1</param>
    <param name="i2c_addr">28</param>
    <param name="axis_remap">P1</param>
    <param name="sensor_mode">NDOF</param>
    <param name="enable_mock_mode">false</param>
  </hardware>
  <sensor name="bno055">
    <!-- Orientation quaternion -->
    <state_interface name="orientation.x"/>
    <state_interface name="orientation.y"/>
    <state_interface name="orientation.z"/>
    <state_interface name="orientation.w"/>
    <!-- Angular velocity -->
    <state_interface name="angular_velocity.x"/>
    <state_interface name="angular_velocity.y"/>
    <state_interface name="angular_velocity.z"/>
    <!-- Linear acceleration -->
    <state_interface name="linear_acceleration.x"/>
    <state_interface name="linear_acceleration.y"/>
    <state_interface name="linear_acceleration.z"/>
  </sensor>
</ros2_control>

The plugin exposes ten state interfaces: a quaternion for orientation, three axes of angular velocity, and three axes of linear acceleration. Once the controller manager loads this hardware interface, these state interfaces become available for broadcasting.

Broadcasting to ROS 2 topics

To make the IMU data available to the rest of the stack, I configured the imu_sensor_broadcaster in lekiwi_control. This controller reads the state interfaces and publishes a sensor_msgs/Imu message to /imu_sensor_broadcaster/imu.

I actually discovered the imu_sensor_broadcaster while working on the omniwheel controller for the LeKiwi, and that's what motivated me to create the BNO055 hardware interface in the first place - I simply wanted to try it out.

imu_sensor_broadcaster:
  ros__parameters:
    sensor_name: bno055  # Must match URDF <sensor name="...">
    frame_id: imu_frame
    
    # ±2.5° magnetometer heading accuracy → 0.002 rad²
    static_covariance_orientation: [0.002, 0.0, 0.0,
                                    0.0, 0.002, 0.0,
                                    0.0, 0.0, 0.002]
    
    # Gyro noise density at 100 Hz → 1e-5 (rad/s)²
    static_covariance_angular_velocity: [1.0e-5, 0.0, 0.0,
                                         0.0, 1.0e-5, 0.0,
                                         0.0, 0.0, 1.0e-5]
    
    # Accelerometer noise
    static_covariance_linear_acceleration: [2.0e-4, 0.0, 0.0,
                                            0.0, 2.0e-4, 0.0,
                                            0.0, 0.0, 2.0e-4]

The covariance values come from the BNO055 datasheet and represent the measurement uncertainty for each sensor component. These values indicate to the EKF how much to trust each measurement during fusion; higher covariance means the filter trusts that measurement less. In robot_localization, these covariances remain static throughout the operation, so getting them right at the start matters.

The broadcaster spawns alongside the other controllers in the launch file:

imu_broadcaster_spawner = Node(
    package='controller_manager',
    executable='spawner',
    arguments=['imu_sensor_broadcaster', '-c', '/controller_manager'],
)

# Delayed to ensure controller manager is ready
delayed_base_controller_spawner = TimerAction(
    period=2.5,
    actions=[base_controller_spawner, imu_broadcaster_spawner],
)

https://www.youtube.com/watch?v=MdPBkdxYOkc

EKF using robot_localization

For LeKiwi, I use the robot_localization EKF node in lekiwi_navigation to fuse wheel odometry with IMU data. The integration centers on a key design choice: the wheel controller provides velocities in the body frame, while the IMU provides drift-free heading. The EKF combines these to produce a corrected odometry estimate.

First, I disable TF publishing from base_controller so the EKF owns the odom → base_footprint transform:

# controller_config.yaml
base_controller:
  ros__parameters:
    enable_odom_tf: false

The controller still publishes /base_controller/odom as a topic, but only the EKF publishes the transform.

From wheel odometry, I fuse only body-frame velocities (vx, vy). Position and heading are deliberately excluded - the controller computes them by dead reckoning, which drifts when the robot gets bumped or manually rotated. By giving the EKF velocities instead, the filter integrates them using the IMU's correct heading:

odom0_config: [false, false, false,  # position - excluded
               false, false, false,  # orientation - excluded
               true,  true,  false,  # vx, vy - fused
               false, false, false,  # angular velocities - excluded
               false, false, false]  # accelerations - excluded

From the IMU, I fuse absolute orientation (roll, pitch, yaw) and angular rates. Linear acceleration is excluded - motor vibration injects too much noise, and wheel velocities are already cleaner:

imu0_config: [false, false, false,  # position - excluded
              true,  true,  true,   # roll, pitch, yaw - fused
              false, false, false,  # linear velocities - excluded
              true,  true,  true,   # angular velocities - fused
              false, false, false]  # linear acceleration - excluded

The filter runs at 50 Hz to match the controller_manager, and I configure it for 3D operation (two_d_mode: false) so it tracks roll and pitch, which is not necessary at the moment but would be useful on uneven surfaces:

frequency: 50
two_d_mode: false
sensor_timeout: 0.1
imu0_differential: false  # BNO055 NDOF gives global orientation
imu0_remove_gravitational_acceleration: false  # BNO055 firmware already strips gravity

For debugging, the stack supports three launch modes: normal (fuses both sources), IMU-only (bench testing when wheels spin freely), and odometry-only (verifies encoders in isolation, auto-enables in mock mode). Even in single-sensor modes, the EKF still runs - it's not fusing anything, but this keeps TF ownership consistent across all modes rather than conditionally enabling TF from the individual sensor nodes:

ros2 launch lekiwi_bringup lekiwi.launch.py # Normal (default)
ros2 launch lekiwi_bringup lekiwi.launch.py imu_only:=true
ros2 launch lekiwi_bringup lekiwi.launch.py odom_only:=true

Validation and testing

The fusion pipeline (focusing on the IMU) looks like this:

BNO055 (I2C) 
  → bno055_hardware_interface 
    → imu_sensor_broadcaster 
      → robot_localization EKF (also includes wheel odometry)
        → /odometry/filtered + odom→base_footprint TF

I tested the sensor fusion by manually driving the LeKiwi around a loop, rotating it while stationary, and giving it gentle bumps during motion. Meanwhile, I monitored the /base_controller/odom and /odometry/filtered side by side on Foxglove. This is by no means perfect, and still needs a SLAM implementation on top to provide the map to odom transform. Still, as internal odometry goes, the sensor fusion is quite effective.

https://www.youtube.com/watch?v=PFRJnBID5ME

I'll be honest, the only real advantage of the IMU is to detect external impacts such as bumps or manual pushes when not using SLAM. In the future, it should also be useful to detect motion over bumps and on ramps, since the IMU measures in 9 DoF.

The wheel odometry by itself is quite impressively accurate, so it can dead reckon very well. If I add a SLAM method directly on top of the wheel odometry, it will still be able to correct the localization in case of external movement. So technically, on flat surfaces, fusing the IMU is not necessary, but it is definitely a fun learning experience. And it gave me a chance to experiment with vibe-coding hardware interfaces, which was quite enlightening.

Why robot_localization

robot_localization is technically deprecated - no active roadmap, though community PRs still land, even in the latest ROS 2 distributions.

The proposed successor is Fuse, a graph-based optimization framework from Locus Robotics, deployed in thousands of warehouse robots globally and genuinely field-tested at scale. However, the challenge here is that Fuse is much more complex to get started with and configure. I will skip this for now.

fusioncore is newer - ROS 2-native with a 22-state UKF, automatic IMU bias estimation, adaptive noise, outlier rejection, and advanced GPS features. However, despite an impressive features list, fusioncore is unproven in real-world deployments, and its documentation contains misleading claims, especially about robot_localization's capabilities, raising concerns. I did try it out and had decent results, but did not dig deeper.

Despite the alternatives, and the somewhat promising results from fusioncore, I chose robot_localization anyway. The deciding factor: per-axis fusion selectivity. robot_localization lets me exclude specific measurement axes - fuse only wheel velocities (not position/heading) and only IMU orientation/rates (not linear acceleration), for example. fusioncore always fuses everything; you can only down-weight unwanted axes via noise inflation, which is an approximation rather than exclusion. Additionally, fusioncore's improvements over robot_localization lie in its GPS-related features, which, for the current state of LeKiwi, add no value.

For the LeKiwi use-case - indoor, no GPS, BNO055 handles calibration and sensor-fusion on-chip - robot_localization's maturity, active deployments, years of field testing, and especially the per-axis control, outweigh fusioncore's advanced features.

I will write a blog post about working with fusioncore soon, and will try to include an evaluation of Fuse as well, but that is for another day.

What's next?

The robot_localization setup covers odometry within the odom frame. The next step towards full autonomy is adding a map layer using Nav2. First, I want to go back to the pan-tilt mechanism and finish the implementation. The control part is complete, but since I've added an OAK-D S2 depth camera to the pan-tilt component, I want to utilize it fully and integrate at least a basic launch file into the LeKiwi ROS 2 stack.

The OAK-D S2 includes 2 monochrome cameras for depth/disparity calculation, an RGB camera, an on-board processor for inferencing, and an IMU. This makes it perfect for visual-inertial odometry (VIO) and visual SLAM - some fun concepts to experiment with. My final objective, however, is to eventually integrate it with my Oculus Quest 2 headset - use the head tracking data to move the pan-tilt mechanism, while streaming RGB video to the headset, and use the handheld controllers to teleoperate the LeKiwi.

I've also ended up with another robot, maybe two. I purchased an Ikea Råskog trolley to attach to the LeKiwi, inspired by the XLeRobot project. As soon as I brought it home, I realized that the middle shelf fit perfectly on top of a mecanum wheeled robot I had brought home from work. Meanwhile, I also had two spare hoverboard wheels, which looked perfect for the bottom shelf and the caster wheels - this is still a work in progress.

This mecanum robot has a Teensy 4.1 in its base, driving the motor controllers. I want to use this platform to tinker with Zenoh-Pico and Pico-ROS, as an alternative to micro-ROS. But this is for another day; for now, it's simply a regular trolley for storage, but with motors and mechanum wheels instead of caster wheels.

But that's for another time. For now, I'm back to working with the LeKiwi. If you want to poke around, here are the repos:

• lekiwi_ros2: top-level directory containing lekiwi_control, lekiwi_description, lekiwi_navigation, and lekiwi_bringup

• bno055_hardware_interface: the ros2_control sensor plugin and diagnostics node

As always, feedback and contributions are welcome!