Kalman filter for robots: a practical introduction

A Kalman filter is bookkeeping for uncertainty. It does not make a bad sensor good; it combines a model and a measurement according to how much each one can be trusted.

This guide explains the idea in plain language, works through a one-dimensional example with real numbers, covers the extended version used for heading, and shows where the filter belongs in a ROS 2 robot.

The idea in one paragraph

You predict where the robot should be using the last estimate and the motion command. You measure where it is with a sensor. Then you blend the two in proportion to their uncertainty: a confident prediction dominates a noisy measurement, and a noisy prediction gives way to a good measurement. Repeat at every time step.

One dimension, real numbers

A robot drives along a line. The encoder says it moved 1.00 m in the last second, with a standard deviation of 0.05 m. A landmark range says it is at 5.02 m, with a standard deviation of 0.10 m.

# Prediction
x = 4.00 + 1.00 = 5.00
P = 0.05^2 + 0.01 = 0.0025 + 0.01 = 0.0125

# Kalman gain
K = P / (P + R) = 0.0125 / (0.0125 + 0.01) = 0.556

# Correction
x = 5.00 + 0.556 * (5.02 - 5.00) = 5.011
P = (1 - 0.556) * 0.0125 = 0.00555

The result sits closer to the prediction because the encoder was more confident. If the range sensor were the better one, the gain would be above 0.5 and the estimate would move towards the measurement. That is the whole idea; the matrix version just does it for several states at once.

The matrix version

# Predict
x = F @ x + B @ u
P = F @ P @ F.T + Q

# Update
y = z - H @ x          # innovation
S = H @ P @ H.T + R
K = P @ H.T @ inv(S)
x = x + K @ y
P = (I - K @ H) @ P

Read it as the same five steps. F is how the state evolves, B maps the control input, H maps the state to what the sensor measures, Q is model uncertainty and R is sensor uncertainty. If you can write those four matrices for your robot, you can use any library.

Why Q and R decide everything

The filter is only as good as its noise settings. A process noise that is too small makes the filter trust the model and ignore the sensor; too large and it chases noise. The same applies to the measurement noise in reverse.

Start from datasheets and measurement. Encoder resolution and wheel slip set Q; sensor accuracy sets R. Then check the innovation, which is the difference between measurement and prediction. If the innovation is consistently larger than the filter expects, a noise value is wrong or the model is.

A minimal filter you can run

The one-dimensional case is small enough to write in a few lines and useful enough to debug a real robot. Run it offline on logged data before putting it on the robot.

def step(x, P, move, Q, z, R):
    x = x + move            # predict state
    P = P + Q               # predict covariance
    K = P / (P + R)         # gain
    x = x + K * (z - x)     # correct state
    P = (1 - K) * P         # correct covariance
    return x, P

Feed it a straight-line drive with encoder and range data and compare the result with the raw measurements. When the filtered path is smoother without lagging, the noise settings are close. When it lags, reduce Q; when it jitters, increase R or reduce Q.

When the model is not linear

Heading makes the model non-linear because rotation mixes x and y. The extended Kalman filter linearises around the current estimate using Jacobians, then runs the same five steps. Most robotics libraries implement this for you; what you must get right is the frame convention and the angle wrap.

• Wrap angles to minus pi to plus pi after every update.
• Keep every quantity in one frame, or transform deliberately.
• Check the sign of the yaw rate against the encoder direction.
• Use a small dt and measure it; a jittery dt adds noise to the prediction.

Fusing an IMU with wheel encoders

This is the most common robot fusion problem. Wheels give speed and distance that drift slowly and fail when they slip. The IMU gives heading rate that is smooth in the short term and drifts in the long term. Fused, they cover each other’s weaknesses.

In ROS 2

You rarely write the filter from scratch for a mobile robot. The common route is an extended Kalman filter node that subscribes to wheel odometry and an IMU and publishes a fused odometry transform. It needs two things from you: correct frame names and sane covariance values in the input messages.

• Publish wheel odometry with a covariance that reflects slip, not a token zero.
• Publish IMU orientation and angular velocity with calibrated noise.
• Keep one node responsible for the odometry to base transform.
• Check the filter output against a tape measure and a known turn.
• Do not fuse two sources of the same information and expect improvement.

When you do not need a Kalman filter

A complementary filter is enough when you only need tilt from an IMU. A least-squares fit is better for a static calibration. A median filter handles spikes. Reaching for a Kalman filter because it sounds rigorous usually adds tuning work without improving the result.

Mistakes that break the estimate

A tuning recipe

• Log raw sensors and the filter output together.
• Drive a straight line and a known turn; compare with a tape measure.
• Set R from the datasheet, then adjust until the output is smooth but responsive.
• Increase Q only if the estimate lags a real change.
• Test on two surfaces, at two speeds and with a full payload.
• Record the settings; they are part of the robot’s configuration.

Related: SLAM for beginners, ROS 2 sensor fusion, wheel encoders, SLAM Toolbox, PID control explained.

Feeding ArUco marker poses into a Kalman filter keeps drift under control in a small lab.

The Kalman filter is the heavier option; start with a complementary filter and move up only when the simple version is not enough.

Sources and further reading

• Kalman Filter — worked numerical tutorials.
• ROS 2 documentation — tf2 and sensor message conventions.
• robot_localization — ROS state estimation package.
• Cambridge University Press — Probabilistic Robotics, Thrun, Burgard and Fox.