ROS 2 robot_localization Guide 2026
Fuse IMU, wheel odometry, and GPS into a smooth odom → base_link / map → odom transform using the robot_localization EKF and UKF nodes.
State Estimation Architecture
Dual-EKF Pattern (recommended)
IMU ──────────────────────────────────────┐
Wheel Odometry ─────────────────────────── ekf_node (local) ──► /odometry/filtered (odom frame)
GPS ──► navsat_transform_node ────────────┐
Fused Odometry ─────────────────────────── ekf_node (global) ──► /odometry/global (map frame)
The dual-EKF pattern is recommended by the robot_localization maintainers: one node fuses local sensors for odom→base_link, a second fuses GPS for map→odom.
Installation
sudo apt install ros-jazzy-robot-localization # GPS driver (optional) sudo apt install ros-jazzy-nmea-navsat-driver
Package provides ekf_node, ukf_node, and navsat_transform_node.
Local EKF Configuration (odom frame)
Create config/ekf_local.yaml:
ekf_filter_node_odom:
ros__parameters:
frequency: 30.0 # Hz — output rate
sensor_timeout: 0.1 # seconds — declare sensor lost
two_d_mode: true # set false for UAV / 3D robots
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom # local EKF outputs odom frame
# Odometry source (nav_msgs/Odometry)
odom0: /odom
odom0_config: [true, true, false, # x y z
false, false, false, # roll pitch yaw
false, false, false, # vx vy vz
false, false, true, # vroll vpitch vyaw
false, false, false] # ax ay az
odom0_differential: false
odom0_relative: false
# IMU source (sensor_msgs/Imu)
imu0: /imu/data
imu0_config: [false, false, false,
true, true, true, # roll pitch yaw absolute
false, false, false,
true, true, true, # angular velocity
true, true, false] # linear accel (skip z with 2D mode)
imu0_differential: false
imu0_relative: false
imu0_remove_gravitational_acceleration: trueSensor Config Array — What Each Position Means
| Index | Field | Typical for odom | Typical for IMU |
|---|---|---|---|
| 0-2 | x y z | true true false | false false false |
| 3-5 | roll pitch yaw | false false false | true true true |
| 6-8 | vx vy vz | false false false | false false false |
| 9-11 | vroll vpitch vyaw | false false true | true true true |
| 12-14 | ax ay az | false false false | true true false |
Global EKF + navsat_transform_node (map frame)
Create config/ekf_global.yaml:
ekf_filter_node_map:
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: map # global EKF outputs map frame
odom0: /odometry/filtered # fused output from local EKF
odom0_config: [true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
odom0_differential: false
odom1: /odometry/gps # GPS converted by navsat_transform
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
navsat_transform_node:
ros__parameters:
frequency: 30.0
delay: 3.0
magnetic_declination_radians: 0.0
yaw_offset: 0.0
zero_altitude: true
broadcast_utm_transform: false
use_odometry_yaw: false
wait_for_datum: falsenavsat_transform_node Topics
| Topic | Type | Direction |
|---|---|---|
| /imu/data | sensor_msgs/Imu | subscribe |
| /gps/fix | sensor_msgs/NavSatFix | subscribe |
| /odometry/filtered | nav_msgs/Odometry | subscribe |
| /odometry/gps | nav_msgs/Odometry | publish |
| /gps/filtered | sensor_msgs/NavSatFix | publish (optional) |
Launch File
# localization.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
pkg = get_package_share_directory("my_robot_bringup")
ekf_local_config = os.path.join(pkg, "config", "ekf_local.yaml")
ekf_global_config = os.path.join(pkg, "config", "ekf_global.yaml")
return LaunchDescription([
Node(
package="robot_localization",
executable="ekf_node",
name="ekf_filter_node_odom",
output="screen",
parameters=[ekf_local_config],
),
Node(
package="robot_localization",
executable="navsat_transform_node",
name="navsat_transform_node",
output="screen",
parameters=[ekf_global_config],
remappings=[
("imu", "/imu/data"),
("gps/fix", "/gps/fix"),
("odometry/filtered","/odometry/filtered"),
("odometry/gps", "/odometry/gps"),
],
),
Node(
package="robot_localization",
executable="ekf_node",
name="ekf_filter_node_map",
output="screen",
parameters=[ekf_global_config],
remappings=[("odometry/filtered", "odometry/global")],
),
])Key Configuration Parameters
| Parameter | Default | Notes |
|---|---|---|
| frequency | 30.0 | EKF publish rate in Hz. Match your slowest sensor. |
| sensor_timeout | 0.1 | Seconds before a silent sensor triggers a warning. |
| two_d_mode | false | Set true for ground robots — ignores z, roll, pitch. |
| odom0_differential | false | Use velocity deltas instead of absolute poses. |
| imu0_remove_gravitational_acceleration | false | Must be true when fusing linear accel from IMU. |
Using UKF Instead of EKF
Replace ekf_node with ukf_node and add:
alpha: 0.001 # spread of sigma points (0.001 – 1.0) kappa: 0.0 # secondary scaling, usually 0 beta: 2.0 # distribution parameter (2 = Gaussian)
UKF handles non-linear motion models better but is slightly more expensive. For most ground robots, EKF is sufficient.
Diagnostics
# Check the filtered odometry
ros2 topic echo /odometry/filtered
# Monitor TF tree
ros2 run tf2_tools view_frames
# Inspect covariance
ros2 topic echo /odometry/filtered --field pose.covariance
# Set datum (origin lat/lon/yaw)
ros2 service call /set_datum robot_localization/srv/SetDatum \
"{geo_pose: {position: {latitude: 37.5, longitude: 127.0, altitude: 0.0}, orientation: {x: 0, y: 0, z: 0, w: 1}}}"Common Issues
Pose jumps on GPS update
GPS covariance too small. Increase odom1 measurement noise covariance or inflate GPS positional uncertainty.
map→odom TF missing
navsat_transform_node hasn't received first GPS fix. Check /gps/fix and verify datum is set.
EKF diverges over time
Increase process_noise_covariance for IMU axes. Check imu0_remove_gravitational_acceleration: true.
Position drifts in reverse
Wheel odometry yaw wrong direction. Set odom0_differential: true or negate yaw in odometry publisher.
Sensor dropout warnings
Increase sensor_timeout to match actual publish rate, or fix the lagging sensor.
Next Steps
- Feed
/odometry/filteredinto Nav2 SMAC Planner for smooth path planning. - Add slam_toolbox or AMCL on top of this fusion for map-based localization.
- Tune the 15×15 process_noise_covariance matrix using rosbag data for best results.