Sensor Fusion: IMU + Odometry (EKF)

Back in Week 17, wheel encoders gave your robot odometry — a running estimate of position built by adding up small movements over time. The catch, covered there too, is that odometry drifts: a wheel slips on a smooth floor, a wheel diameter is off by a millimeter, and the error accumulates forever because nothing ever corrects it. The IMU from Week 11 drifts too, in the opposite way — its gyroscope integrates angular velocity into an angle that wanders over time, while its accelerometer is instantly noisy but never accumulates error. Neither sensor alone is trustworthy for very long. Fused together, though, each one’s weakness is exactly what the other is good at.

That’s the idea behind this week’s tool: the Extended Kalman Filter (EKF), the standard way ROS 2 robots combine wheel odometry, an IMU, and (optionally) GPS into one pose estimate that’s more reliable than any single sensor feeding it.

What You’ll Learn

• Why fusing two drifting sensors produces an estimate that drifts less than either one alone
• The intuition behind a Kalman filter’s predict/correct cycle — no linear algebra required
• How ROS 2’s robot_localization package implements this with the ekf_node
• How to write an ekf.yaml config that fuses /odom and /imu/data
• How to launch the filter and check its output in RViz2
• The pitfalls that make a fused estimate worse than the raw sensors it’s built from

Why Fusion Beats Either Sensor Alone

Each sensor here has a distinct error signature:

Sensor	Good at	Bad at
Wheel odometry	Position and heading over short time spans	Long-term drift from slip and wheel-diameter error; useless for detecting a rotation the wheels didn’t cause (a bump, a curb)
IMU gyroscope	Fast, accurate rotation rate, instantly	Integrating rate into angle accumulates drift over time
IMU accelerometer	Long-term gravity reference (which way is “down”), and detecting motion the wheels can’t see	Noisy moment-to-moment; easily confused by vibration

None of these is individually enough, but the errors don’t overlap: odometry is trustworthy short-term and IMU rotation-rate data is trustworthy instantaneously, so blending them gives you both. That blending job is exactly what a Kalman filter does.

The Kalman Filter, Intuitively

A Kalman filter keeps a running best guess of the robot’s state (position, heading, velocity) and repeats a two-step cycle, forever:

1. Predict. Using the robot’s motion model (and the last known velocity), project the state forward by one small timestep. This step alone would drift exactly like raw odometry.
2. Correct. When a new sensor reading arrives — a new /odom message, a new /imu/data message — blend it into the prediction, weighted by how much that sensor is trusted at that moment (its covariance, a number each message already carries describing its own uncertainty).

The “Extended” in Extended Kalman Filter just means it handles a robot’s motion model, which involves rotation and is nonlinear, by locally approximating it as linear at each step — the plain Kalman filter only handles motion that’s linear to begin with. You don’t need to derive any of this math to use it; ROS 2’s robot_localization package implements the predict/correct cycle for you. What you configure is which sensors feed it and which parts of their data to trust.

Setting Up robot_localization

Install the package for your ROS 2 distro (these examples use Jazzy):

sudo apt install ros-jazzy-robot-localization

The core node is ekf_node, from the robot_localization package. It subscribes to any number of odometry and IMU topics, fuses them, and publishes a corrected nav_msgs/msg/Odometry on /odometry/filtered. Everything about what to fuse lives in one YAML file:

# config/ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /odom
    odom0_config: [true,  true,  false,
                    false, false, true,
                    false, false, false,
                    false, false, false,
                    false, false, false]
    odom0_differential: false

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  false, false, false]
    imu0_differential: false
    imu0_remove_gravitational_acceleration: true

Each *_config array has 15 booleans, one per state variable, always in this order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Setting a position to true tells the filter “trust this sensor for this specific value”; false means “ignore whatever this sensor reports here.” In the config above, wheel odometry contributes x, y, and yaw velocity (wheels can’t sense absolute heading on their own, so raw yaw position is left off), and the IMU contributes yaw and yaw velocity — its strongest, most trustworthy values. two_d_mode: true locks out z, roll, and pitch entirely, which is the right call for any ground robot that doesn’t need to track height or tilt. imu0_remove_gravitational_acceleration: true matters specifically for raw IMUs: without it, gravity shows up as a constant fake forward/backward acceleration whenever the robot tilts even slightly.

Launch it either directly:

ros2 run robot_localization ekf_node --ros-args --params-file config/ekf.yaml

or, more commonly, as part of a launch file alongside the rest of the robot’s nodes:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=['config/ekf.yaml'],
        )
    ])

Once it’s running, subscribe to /odometry/filtered exactly the way you’d subscribe to raw /odom — it’s the same nav_msgs/msg/Odometry message type, just with the IMU’s corrections folded in.

Checking the Result in RViz2

The filtered output is easiest to trust once you’ve seen it against the raw inputs:

ros2 run rviz2 rviz2
# Add display -> By display type -> Odometry
# Add it twice: once for /odom, once for /odometry/filtered
# Set a different color for each, and drive the robot around

Two paths should appear. The raw /odom path will visibly swim during fast turns or wheel slip; the /odometry/filtered path should track it closely most of the time but stay smoother through exactly those moments — that’s the IMU’s yaw-rate data correcting what the wheels alone got wrong. If the filtered path is noisier than the raw one, or drifts off in its own direction, something in the config is set up backwards — see the pitfalls below.

Common Pitfalls

Fusing the same physical quantity from two sources without _differential. If both odom0 and imu0 are configured to feed absolute yaw position rather than one of them being made a rate, the two disagree about heading and the filter has no principled way to pick a winner — it averages them into an estimate worse than either input. Let one sensor own each state variable, as in the config above: odometry owns x/y position, the IMU owns yaw and yaw rate.

Forgetting imu0_remove_gravitational_acceleration. A stationary robot on a slight incline will otherwise register a small constant “forward” acceleration from gravity, which the filter dutifully integrates into a slowly growing false velocity.

Mismatched or default covariances. Every Odometry and Imu message carries a covariance matrix describing how much to trust each field — if your driver publishes all zeros (a common default that actually means “perfectly certain”), the filter will over-trust that sensor’s noise. Check your driver’s documentation for how to set realistic covariance values, or estimate them empirically by keeping the sensor still and measuring its readings’ variance.

Publishing /tf from two places at once. publish_tf: true makes ekf_node publish the odom → base_link transform. If your robot’s own odometry node also publishes that same transform, ROS 2’s tf2 tree ends up with two competing parents for the same frame — pick exactly one publisher (see tf2, from Week 23, for how the transform tree is supposed to stay a strict hierarchy).

Fusing before checking frame conventions. odom0 and imu0 must report in frames consistent with base_link_frame (typically base_link or base_footprint) — an IMU mounted upside-down or rotated 90° relative to the robot’s expected axes will feed the filter confidently wrong data rather than an obvious error.

Recap

What you want	How to do it
Fuse wheel odometry + IMU into one pose estimate	robot_localization’s ekf_node, configured via YAML
Choose which sensor contributes which value	The 15-element odom0_config / imu0_config boolean arrays
Restrict to ground-robot motion	two_d_mode: true
Correct for gravity in raw accelerometer data	imu0_remove_gravitational_acceleration: true
Read the fused result	Subscribe to /odometry/filtered, same message type as /odom
Verify it’s actually helping	Compare /odom vs /odometry/filtered paths live in RViz2

Fusing two imperfect, drifting sensors into one better estimate is a small-scale preview of a bigger idea: no single sensor on a real robot tells the whole story, and combining perspectives is how you build a trustworthy one. Next week applies that same “look closer at what one sensor actually gives you” lens to a specific piece of hardware: Depth Cameras in Practice (RealSense / OAK).