# Sensor Fusion on LeKiwi

I've been gradually converting my [LeKiwi](https://github.com/adityakamath/lekiwi_ros2) 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](https://foxglove.dev/blog/upgrading-the-lekiwi-into-a-lidar-equipped-explorer), a [pan-tilt mechanism](https://kamathrobotics.com/pan-tilt-controls-using-ros-2) with a depth camera, and wired up [wheel odometry using `ros2_control`](https://kamathrobotics.com/wiring-up-ros-2-control-for-lekiwi).

The wheel odometry works well (especially on short runs), thanks to the [STS3215 servo motors](https://www.feetechrc.com/2020-05-13_56655.html) 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](https://www.adafruit.com/product/2472?srsltid=AfmBOopfl_2l3l1Yp9M0RjAYZtzsXMM8xtyjXXwTAbBdBf9_IBs_pyvl) IMU and vibe-coded a [`ros2_control` hardware interface](https://kamathrobotics.com/bno055-imu-hardware-abstraction) for it using the [bno055\_hardware\_interface](https://github.com/adityakamath/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`](https://github.com/cra-ros-pkg/robot_localization). Later in this post, I also compare `robot_localization` with the newer [`fusioncore`](https://github.com/manankharwar/fusioncore) project.

![](https://cdn.hashnode.com/uploads/covers/63a0f156a59cad9bdbba046e/44d27825-dd29-492e-853c-85ad00c9e3ed.webp align="center")

## 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](https://kamathrobotics.com/bno055-imu-hardware-abstraction), at least for this section.

### The `ros2_control` Hardware Interface

I added the `<ros2_control>` block to the URDF in [`lekiwi_description`](https://github.com/adityakamath/lekiwi_ros2/tree/main/lekiwi_description), declaring the BNO055 as a `sensor` type — read-only, with no command interfaces:

```xml
<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`](https://control.ros.org/kilted/doc/ros2_controllers/imu_sensor_broadcaster/doc/userdoc.html) in [`lekiwi_control`](https://github.com/adityakamath/lekiwi_ros2/tree/main/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](https://control.ros.org/master/doc/ros2_controllers/omni_wheel_drive_controller/doc/userdoc.html) 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.

```yaml
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:

```python
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`](https://docs.ros.org/en/kilted/p/robot_localization/) EKF node in [`lekiwi_navigation`](https://github.com/adityakamath/lekiwi_ros2/tree/main/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:

```yaml
# 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:

```yaml
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:

```yaml
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:

```yaml
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:

```bash
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:

```plaintext
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`](https://github.com/cra-ros-pkg/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`](https://github.com/locusrobotics/fuse), a graph-based optimization framework from [Locus Robotics](https://locusrobotics.com/), 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`](https://github.com/manankharwar/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](https://docs.nav2.org/). 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](https://shop.luxonis.com/products/oak-d-s2?srsltid=AfmBOopmRxbWbGnU-vsFw8OCdZj0EhS0D_qXGLTqSUAtuUd5wUWEq5OH) 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](https://docs.openvins.com/) (VIO) and [visual SLAM](https://docs.ros.org/en/kilted/p/rtabmap_demos/) - some fun concepts to experiment with. My final objective, however, is to eventually integrate it with my [Oculus Quest 2](https://developers.meta.com/horizon/blog/introducing-oculus-quest-2-the-next-generation-of-all-in-one-vr/) 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.

![](https://cdn.hashnode.com/uploads/covers/63a0f156a59cad9bdbba046e/00679b8a-9e3c-44d2-aae3-4c397d8f2e8c.jpg align="center")

This mecanum robot has a [Teensy 4.1](https://www.pjrc.com/store/teensy41.html) in its base, driving the motor controllers. I want to use this platform to tinker with [Zenoh-Pico](https://zenoh.io/blog/2021-10-04-zenoh-pico-guide/) and [Pico-ROS](https://github.com/Pico-ROS/Pico-ROS-software), as an alternative to [micro-ROS](https://micro.vulcanexus.org/). 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`](https://github.com/adityakamath/lekiwi_ros2): top-level directory containing `lekiwi_control`, `lekiwi_description`, `lekiwi_navigation`, and `lekiwi_bringup`
    
*   [`bno055_hardware_interface`](https://github.com/adityakamath/bno055_hardware_interface): the `ros2_control` sensor plugin and diagnostics node
    

<div data-node-type="callout">
<div data-node-type="callout-emoji">⚠</div>
<div data-node-type="callout-text">Note that the <code>lekiwi_ros2</code> repository is in active development, and may include breaking changes. So, please use it with caution.</div>
</div>

As always, feedback and contributions are welcome!