Sensor Fusion: Odometry, IMU, and GPS

No single sensor is good enough on its own. Wheel odometry drifts, an IMU gives a good heading but slowly wanders, and GPS is noisy and drops out under trees or in tunnels. A Kalman filter combines them so each covers for the others.

IMU heading GPS position (1 Hz) GPS noise σ Odometry heading bias

How the filter uses each sensor

Odometry drives the prediction step, giving smooth short-term motion. The IMU corrects heading, which is where odometry errors grow fastest. GPS corrects absolute position, so the long-term error stays bounded. Each correction is weighted by the Kalman gain:

$$K = P^- H^\top (H P^- H^\top + R)^{-1}$$

so a noisy GPS with large $R$ pulls the estimate only a little, while a precise one pulls hard.

What to notice

The odometry-only estimate, amber, bends away from the truth more and more. With GPS on, the fused estimate, red, stays near the truth. Inside the tunnel, shaded, GPS disappears and the fused estimate drifts on odometry and IMU, then snaps back when GPS returns. Turn off the IMU and the drift inside the tunnel gets worse. This is the job done by packages like robot_localization in ROS 2.