Fusing GNSS, IMU and Wheel Odometry With robot_localization: A Working EKF Config

TL;DR: A robust robot_localization ekf gnss imu setup on ROS 2 commonly uses two EKF instances plus navsat_transform_node. The local EKF fuses wheel odometry and IMU into a continuous odom->base_link transform. The global EKF adds GPS-derived odometry to publish map->odom. This article shows a full YAML, covariance considerations, launch wiring, and how to verify fusion is actually working, not just running. Tested conceptually on a Leo Rover-class UGV.

Why a Two-EKF Architecture Is the Right Answer for GNSS Fusion

Field robots need two things at once: a smooth, high-rate local pose that never jumps, and a drift-free global pose anchored to Earth. A single EKF can provide either a continuous local estimate or a globally corrected estimate, but not both frame behaviors simultaneously. If you inject GPS directly into your only filter, satellite-fix corrections can shift the estimate, and controllers using that frame may receive discontinuous poses. The robot_localization ekf gnss imu pattern solves this with the ROS REP-105 frame convention: odom is continuous, map is globally corrected.

The robot_localization package by Tom Moore implements an EKF and UKF, plus navsat_transform_node, which converts NavSatFix messages into an odometry message in the filter’s world-frame coordinates. You run two EKF nodes:

• Local EKF (ekf_local): fuses wheel odometry (linear X) and IMU (angular Z, orientation). Publishes odom->base_link.
• Global EKF (ekf_global): fuses wheel odometry and IMU, plus the GPS odometry from navsat_transform_node. Publishes map->odom.

This is a common REP-105-compatible topology for outdoor navigation and works well on skid-steer platforms like the Leo Rover, where wheel slip makes pure odometry unreliable over long traverses.

What Sensors and Frames You Need Before Touching YAML

Get the plumbing right first. Bad frames or bad units waste hours of tuning. You need the following inputs available on ROS 2 topics:

• /odom/wheel – nav_msgs/Odometry from the base controller, expressed in odom frame, with a valid child frame base_link. Linear velocity in m/s.
• /imu/data – sensor_msgs/Imu with orientation (if magnetometer-fused) or at least angular velocity and linear acceleration. Frame: imu_link, rigidly linked to base_link via your URDF.
• /gps/fix – sensor_msgs/NavSatFix from your GNSS receiver (u-blox ZED-F9P, Ardusimple, etc.), typically at 5-10 Hz.

Verify TF completeness with ros2 run tf2_tools view_frames. The IMU and GPS antenna must have static transforms to base_link. An incorrect antenna offset can introduce a position error that varies with vehicle heading.

Covariance: the Number That Actually Determines Fusion Behavior

An EKF weights each measurement by the inverse of its covariance. If your wheel odometry reports covariance 1e-9, the filter trusts it almost absolutely and may largely ignore other measurements. This is a common failure in robot_localization ekf gnss imu setups. Use realistic values:

• Wheel odometry linear X: variance around 0.05 (m/s)² can be a reasonable starting point for a skid-steer on mixed terrain.
• IMU angular Z: consult the datasheet and account for bandwidth, filtering, vibration, and bias. A gyro noise density alone cannot be converted to a variance without those assumptions.
• GPS position: RTK-fixed can be around 0.0004 m² (2 cm sigma) under good conditions, RTK-float around 0.25 m², and single-point GNSS around 4.0 m² or worse. Many receivers already populate position_covariance; validate rather than blindly trust those values.

The Full Working YAML

Save this as ekf_dual.yaml. It defines both filters and the navsat node in one file. Comments explain each block.

### Local EKF: odom -> base_link, continuous, no GPS
ekf_local:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.2
    two_d_mode: true              # UGV on roughly flat terrain
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom             # local filter anchors to odom

    # Wheel odometry: use only linear X velocity
    odom0: /odom/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, false,
                   false, false, false]
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 10

    # IMU: use yaw, angular velocity Z, linear accel X
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  true,  false, false]
    imu0_differential: false
    imu0_relative: true           # yaw offset zeroed at startup
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]

### Global EKF: map -> odom, fuses GPS
ekf_global:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.2
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: map              # global filter anchors to map

    odom0: /odom/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, false,
                   false, false, false]
    odom0_queue_size: 10

    # GPS odometry from navsat_transform_node
    odom1: /odometry/gps
    odom1_config: [true,  true,  false,
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    odom1_queue_size: 10

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  true,  false, false]
    imu0_relative: true
    imu0_remove_gravitational_acceleration: true
    imu0_queue_size: 10

### Converts NavSatFix -> Odometry in the local world frame
navsat_transform:
  ros__parameters:
    frequency: 30.0
    delay: 3.0                    # wait for filter to settle
    magnetic_declination_radians: 0.0  # set this only if IMU yaw is magnetic-north referenced
    yaw_offset: 0.0               # IMU yaw follows ENU convention: zero is east
    zero_altitude: true
    broadcast_utm_transform: false
    publish_filtered_gps: true
    use_odometry_yaw: false       # trust IMU yaw, not odom yaw
    wait_for_datum: false

The odom_config and imu_config vectors follow the order: X, Y, Z, roll, pitch, yaw, vX, vY, vZ, vRoll, vPitch, vYaw, aX, aY, aZ. Only fuse what your sensor actually measures well. Fusing wheel Y velocity on a skid-steer, for example, injects noise, not information.

How to Launch the Nodes in the Right Order

Order matters. If navsat_transform_node starts before it receives valid filtered odometry, IMU orientation, and NavSatFix data, it waits until it can establish its transform and datum. A minimal launch file starts everything together and lets the nodes wait for messages:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    params = '/opt/ros_ws/config/ekf_dual.yaml'
    return LaunchDescription([
        Node(package='robot_localization', executable='ekf_node',
             name='ekf_local', parameters=[params],
             remappings=[('odometry/filtered', '/odometry/local')]),
        Node(package='robot_localization', executable='ekf_node',
             name='ekf_global', parameters=[params],
             remappings=[('odometry/filtered', '/odometry/global')]),
        Node(package='robot_localization', executable='navsat_transform_node',
             name='navsat_transform', parameters=[params],
             remappings=[
                ('imu', '/imu/data'),
                ('gps/fix', '/gps/fix'),
                ('odometry/filtered', '/odometry/local'),
                ('odometry/gps', '/odometry/gps'),
             ]),
    ])

navsat_transform_node consumes the local EKF output and produces /odometry/gps, which the global EKF fuses. Using the global EKF output as the navsat input would create a feedback loop, because that filter already consumes /odometry/gps.

How to Verify the Fusion Actually Works

A running node is not a working filter. Use these checks before trusting the output for navigation or field research:

1. TF tree: ros2 run tf2_tools view_frames should show map -> odom -> base_link. Two publishers on the same edge means both EKFs are misconfigured.
2. Continuity: plot /odometry/local pose. Drive a straight 10 m line. The trajectory must be smooth, with no GPS-induced jumps.
3. GPS correction visible: inspect the map->odom transform in RViz2 or with tf2_echo. It should change gradually as global GPS corrections arrive. To compare /odometry/global and /odometry/local visually, use a common fixed frame such as map.
4. Loop closure test: drive a 50 m loop and return to the start. The local pose will usually show drift. The global pose should return to within the actual GNSS accuracy achievable in that environment.
5. Covariance sanity: ros2 topic echo /odometry/global --field pose.covariance. Diagonal terms for X and Y should remain finite and should respond plausibly to GNSS quality; they do not necessarily equal the raw GPS variance.
6. Reject bad fixes: simulate a GNSS outage by stopping valid fixes or filtering them out. The global EKF should continue dead reckoning via wheel and IMU. When valid fixes return, map->odom should converge according to the reported measurement covariance.

A public reference repository with this exact configuration, plus a rosbag playback for offline testing, is available at github.com/fictionlab/leo_localization_example (community-maintained example configs for Leo Rover class platforms).

When You Should Deviate From This Template

The config above assumes a 2D outdoor UGV with a decent 9-DoF IMU and a single-antenna GNSS. Depart from it in these cases:

• Dual-antenna GNSS heading: feed its heading through a correctly configured orientation source, such as a sensor_msgs/Imu message or a converted heading topic. Magnetic declination is unnecessary when the heading is already true-north or ENU referenced. This reduces magnetometer reliance near steel structures.
• Rough terrain / drone-like: set two_d_mode: false and fuse pitch, roll, Z. Your process noise for Z must grow accordingly.
• Visual-inertial odometry available: add it as a second odom source in the local EKF. VIO can help during wheel-slip events where wheel odometry is unreliable.
• Indoor-outdoor transitions: gate GPS by fix status. Use a relay node that forwards only valid NavSatFix messages, for example with status >= 0, a known covariance type, and covariance below an application-specific threshold.

Field robotics deployments in agriculture, inspection, and environmental monitoring, common in research applications, may benefit from dual-antenna heading where magnetic disturbances or heading accuracy are significant concerns.

Frequently Asked Questions

Do you need two EKF instances, or can one filter fuse GPS directly?

You can technically fuse GPS in a single EKF, but then its world frame is globally corrected and can move when GPS corrections arrive. A two-EKF setup preserves the continuous odom->base_link transform required by REP-105 while isolating global corrections into map->odom.

Why is your global pose drifting despite GPS fusion?

Three common causes: GPS covariance too high or invalid (the filter gives it little weight), IMU yaw convention or offset is wrong (positions rotate around the robot), or the GNSS fixes are poor despite being reported. Verify magnetic declination only when the IMU heading is magnetic-north referenced, and confirm the IMU reports the ROS ENU convention.

What frequency should the EKF run at?

Choose a frequency appropriate for the required output rate and CPU budget. For many ground robots, 30-50 Hz is adequate even when an IMU publishes faster, because the filter processes incoming measurements as they arrive and publishes predictions at its configured frequency. Increase it when your controller or estimator timing requires a faster output.

How should you handle GPS outages under trees or in urban canyons?

The global EKF falls back to wheel and IMU dead reckoning when valid GPS measurements stop arriving. Drift depends heavily on wheel calibration, terrain, slip, turns, and IMU quality; it cannot be reliably summarized by one percentage. When valid fixes return, the filter incorporates them according to their covariance.

Can you use this config with Nav2 out of the box?

Yes, provided the rest of your Nav2 configuration and TF tree are consistent. Point Nav2’s odom_topic at /odometry/local and set the global costmap’s global_frame to map. The map->odom transform published by ekf_global lets Nav2 relate global and local frames.

Does two_d_mode discard useful IMU data?

It constrains Z, roll, and pitch in the state, which reduces noise on flat terrain. If your robot pitches or rolls significantly, or if terrain geometry matters to the application, disable it. Otherwise, it can provide cleaner planar estimates.

How do you tune process_noise_covariance without guessing?

Start with the defaults shipped in robot_localization or a documented baseline for your platform. Drive a known trajectory and compare the estimate with independent reference data where possible. If the filter responds too slowly to credible measurements, increase process noise on the affected state; if it is unnecessarily jittery, decrease it. Change one diagonal at a time and use /diagnostics, recorded topics, and innovation behavior in debug logs to validate the result.

Build Your Own Field-Ready Localization Stack

A working robot_localization ekf gnss imu configuration is the foundation of any outdoor autonomy stack, but every platform, sensor suite, and environment shifts the tuning. If you are integrating GNSS-fused localization on a custom UGV, or need a reference implementation validated on real hardware, Fictionlab’s engineering team supports integration projects through custom robotics services, from sensor selection to field-tested EKF configurations.