In Chapter 8, diff_drive_controller estimated ARCHO's path from the rotation of the two
wheel encoders and published it on /odom. That looked great — until one day you notice
that after half an hour of driving around the warehouse, the robot's reported position is several
meters away from its actual position. This phenomenon is called Drift, and it's
exactly the problem this chapter solves.
Wheel Odometry is like counting your steps with your eyes closed — accurate and fast in the short term, but every small wheel slip on the ground turns into accumulated error. An IMU is like your inner-ear sense of balance — it can tell how much you've turned, but not how far you've moved forward. Neither is complete on its own; but when combined, they compensate for each other's weaknesses. This combination is called Sensor Fusion.
Let's look at exactly what gets published on /odom:
The Odometry message has three main parts:
| Part | Meaning |
|---|---|
pose | The robot's current estimated position and orientation |
twist | Instantaneous linear and angular velocity |
covariance | The degree of uncertainty in this estimate (fully explained in the next section) |
Wheel Odometry assumes the wheels roll on the ground exactly as much as they rotate — no more, no
less. But in reality, the warehouse floor may be slippery, the wheels may spin in place a little,
or the actual wheel radius may differ slightly from the value written in the previous chapter's
controllers.yaml due to wear. Each of these small errors accumulates moment by moment
— exactly like rounding error in a long calculation.
The IMU (whose physical Frame we built in Chapter 4) measures linear acceleration and angular velocity at a high rate. Unlike Wheel Odometry, the IMU makes no assumptions about wheel slip — it senses the body's actual motion directly. But the IMU has its own weakness too: high noise over the long term causes its own position estimate to drift as well, just of a different kind.
| Sensor | Strength | Weakness |
|---|---|---|
| Wheel Odometry | Accurate in the short term, stable rate | Wheel slip causes cumulative Drift |
| IMU | Senses the body's actual rotation and acceleration directly | Noisy and prone to Drift over the long term, especially for position |
To combine these two sources, we use a standard ROS 2 package called robot_localization, which implements an Extended Kalman Filter (EKF).
# archo_bringup/config/ekf.yaml
ekf_filter_node:
ros__parameters:
frequency: 50.0
sensor_timeout: 0.1
two_d_mode: true
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /diff_drive_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
imu0: /imu/data
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
imu0_remove_gravitational_acceleration: true
Each _config field has fifteen true/false values, corresponding in order to X, Y, Z,
Roll, Pitch, Yaw, Vx, Vy, Vz, Vroll, Vpitch, Vyaw, Ax, Ay, Az. true means "use this
value from this sensor." For odom0, for example, only Vx, Vy, and Vyaw (linear and
angular velocity) are enabled — because we trust the wheel speeds, not their absolute position.
For imu0, Yaw, Vyaw, and longitudinal acceleration (Ax) are enabled — because the IMU
is more accurate at sensing rotation than linear motion.
The IMU's accelerometer always measures Earth's gravity too, even when the robot is completely stationary. If this component isn't removed, the filter thinks the robot is constantly accelerating — one of the most common causes of "a stationary robot slowly drifting in RViz."
Covariance is a number that says how trustworthy a measurement is. A very small number means "I'm nearly certain"; a large number means "treat this measurement with caution." The EKF uses these numbers to decide which sensor to trust more, and at which moment.
Suppose ARCHO is moving across a very smooth surface — Wheel Odometry is nearly error-free, so it has low Covariance and the EKF trusts it more. But when it passes over a small bump, wheel slip increases; here the IMU (which doesn't sense this kind of disturbance) is relatively weighted more heavily. All this balancing happens automatically and moment-to-moment, performed by the EKF.
A reasonable question: why don't we feed /scan data directly into the EKF too? Because
raw LiDAR is a LaserScan — a list of distances in various directions — not a Pose or
Odometry. The EKF needs data with motion structure (position, velocity, acceleration). If we want to
use LiDAR to correct position, we first have to convert it into a Pose via SLAM or map matching
(which we'll see in Chapters 10 and 11); it's that Pose, not the raw Scan, that enters the Fusion.
ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_tools view_frames
ros2 topic echo /odometry/filtered
As we saw in Chapter 5, this error usually means the Timestamps aren't synchronized, or
use_sim_time isn't set consistently across all the involved Nodes (including
ekf_filter_node). In simulation, never forget this setting.
/odometry/filtered topic is published at a stable rate (e.g., 50 Hz).odom → base_link Transform is available with no extrapolation error.
ARCHO no longer relies on a single sensor source for position estimation. Wheel Odometry and the
IMU, each with their own strengths and weaknesses, are combined by robot_localization
to produce a more stable estimate on /odometry/filtered — a foundation that SLAM and
Nav2 in later chapters simply cannot work correctly without.
ARCHO Project now has a complete ekf.yaml, and the odom → base_link chain, which we only defined in Chapter 5, is now actually populated with fused Wheel Odometry and IMU data.
In Chapter 10, ARCHO enters an unknown environment for the first time with no pre-built map, and uses SLAM Toolbox to simultaneously explore the environment and build a map — precisely where this chapter's accurate position estimate is an absolute prerequisite.