Learn AI Create the Future — with Pranay
Main site

Robotics, Control and Autonomy

Sensor fusion and the Kalman filter

The Kalman filter blends a prediction of where a robot should be with a noisy sensor reading, trusting whichever one is more certain.

Read these first

On this page 6
  1. Why it exists
  2. How it works
  3. Where you have already seen it
  4. An honest note
  5. Remember this
  6. What to learn next

One lesson, three depths. Pick the one that fits you today — you can switch any time.

Beginner — No maths. Plain English.

A Kalman filter combines a guess with a measurement, trusting whichever one is more certain.

Picture catching a ball thrown across a crowded room. People keep blocking your view. When you can see the ball, you correct your aim. When you cannot, you still track it in your head, using what you know about how thrown things move. You are never fully guessing, and never fully trusting your eyes alone. You blend both, all the time.

That blend is the whole idea. Robots do the same thing to know where they are.

Why it exists

No single sensor tells a robot the truth.

A wheel encoder — a sensor that counts wheel rotations — is smooth but drifts. Small errors pile up. After a few minutes, the robot's guess of its own position can be badly wrong — even though every single reading looked reasonable.

GPS is honest on average but jumps around, sometimes by several metres, especially near tall buildings or indoors. A camera gives a sharp picture but says nothing when the lighting is bad.

Before the Kalman filter, engineers had two poor choices. Trust one sensor, and live with its specific flaw. Or average several sensors, treating every one as equally reliable — even the bad one. Neither works well.

How it works

PREDICT                          CORRECT
"Robot moved forward             A new, noisy sensor reading arrives.
 about 1m since last tick"  -->  Blend the prediction and the reading,
 (using known motion)             trusting whichever is more certain.
                                        |
                                        v
                              updated belief: best position estimate,
                              plus how confident we are in it
                                        |
                                (repeat every tick, forever)

The filter never fully believes the sensor, and never fully believes its own prediction. It keeps a running measure of how confident it is in each, and shifts trust between them automatically.

Where you have already seen it

  • Google Maps keeps your blue dot moving smoothly through a tunnel with no GPS signal, using your phone's motion sensors to predict. It corrects hard the moment GPS returns.
  • Drone flight controllers use it to stay stable in gusty wind, blending motor commands with tilt sensors many times per second.
  • It flew the Apollo missions to the Moon in 1969, guiding the spacecraft using sparse, noisy sightings of stars.

An honest note

The filter below is a toy: one robot, one dimension, invented numbers. A real vehicle's state estimator is tuned against months of recorded sensor logs, and reviewed by engineers who understand exactly how it fails. Getting this wrong in a car or a drone means a wrong belief about where the machine is in the physical world. That is why production autonomy stacks treat this component as safety-critical, and validate it far beyond anything shown here.

Remember this

  • No single sensor is trustworthy enough on its own; a Kalman filter blends several.
  • It works in two steps, forever repeated: predict from what you know, then correct using what you measured a moment ago.
  • It keeps track of its own uncertainty, and leans on the more certain source at every step.

What to learn next

Developer — Code and libraries.

Setup

bash
pip install numpy

Minimal runnable code

A robot rolls forward at a steady 1 metre per second. A cheap sensor reports its position, but with noise of about ±2 metres on every reading.

kalman_1d.py
import numpy as np

rng = np.random.default_rng(1)

# Ground truth: the robot really does move at 1 m/s.
true_position = np.arange(0, 20, 1.0)

# A noisy sensor: correct on average, but off by up to a few metres each reading.
measurements = true_position + rng.normal(0, 2.0, size=true_position.shape)

# --- A minimal 1D Kalman filter: position only, constant-velocity model ---
estimate = 0.0        # our best guess of position
uncertainty = 100.0   # how unsure we are (starts very unsure)
process_noise = 0.05  # how wrong the motion model itself can be, per step
sensor_noise = 4.0    # how noisy we believe the sensor is (variance)

filtered = []
for z in measurements:
    predicted_estimate = estimate + 1.0                 # predict: robot moved ~1m
    predicted_uncertainty = uncertainty + process_noise

    kalman_gain = predicted_uncertainty / (predicted_uncertainty + sensor_noise)
    estimate = predicted_estimate + kalman_gain * (z - predicted_estimate)  # correct
    uncertainty = (1 - kalman_gain) * predicted_uncertainty

    filtered.append(estimate)

filtered = np.array(filtered)
print(f"average error, raw sensor readings:  {np.mean(np.abs(measurements - true_position)):.2f} m")
print(f"average error, after Kalman filter:  {np.mean(np.abs(filtered - true_position)):.2f} m")
print(f"final Kalman gain (trust in new readings, by the end): {kalman_gain:.2f}")
Output
average error, raw sensor readings:  0.99 m
average error, after Kalman filter:  0.38 m
final Kalman gain (trust in new readings, by the end): 0.11

What actually happened

The filtered position is noticeably closer to the truth than any raw reading. It never sees the true position — only noisy measurements and a rough motion model — yet it beats both on their own.

  • kalman_gain starts near 1, trusting the first measurement completely, since the filter began knowing nothing. It shrinks toward 0 as the filter grows confident in its own running estimate.
  • predicted_uncertainty grows a little every step, because the motion model itself is not perfect (process_noise). Then it shrinks again once a measurement arrives and corrects it.
  • Swap sensor_noise = 4.0 for sensor_noise = 0.1 and rerun. The gain rises, because a trustworthy sensor deserves more weight.

Common mistakes

Setting process_noise to zero. The filter then trusts its own prediction completely and stops listening to new measurements at all — kalman_gain collapses to 0 and never recovers. Real motion always has some uncertainty; say so.

Confusing the two noise numbers. sensor_noise describes the sensor. process_noise describes the robot's own unpredictability. Swapping them tunes the filter to trust the wrong thing.

Assuming this scales to more sensors for free. Fusing many sensors needs a full matrix version of this filter, not several 1D filters bolted together. Correlated errors between sensors — common when two sensors share a power supply or vibration source — break that shortcut.

Try it yourself

Set process_noise = 5.0, a jumpy, unpredictable motion model. Watch kalman_gain settle at a much higher number. A filter that expects its own predictions to be shaky leans harder on every new measurement, which is the correct behaviour.

What to learn next

Researcher — Mathematics and papers.

The linear Kalman filter

For a linear system with Gaussian noise, the filter is the optimal (minimum mean-squared-error) estimator. State x_k evolves as:

x_k = F x_{k-1} + w_k,        w_k ~ N(0, Q)      (process model)
z_k = H x_k + v_k,            v_k ~ N(0, R)      (measurement model)

F is the state transition matrix, H maps state to measurement space, Q is process noise covariance, R is measurement noise covariance.

Predict:

x_hat_k|k-1 = F x_hat_{k-1|k-1}
P_k|k-1     = F P_{k-1|k-1} F^T + Q

Update:

K_k    = P_k|k-1 H^T ( H P_k|k-1 H^T + R )^-1        (Kalman gain)
x_hat_k|k = x_hat_k|k-1 + K_k ( z_k - H x_hat_k|k-1 )
P_k|k     = ( I - K_k H ) P_k|k-1

P is the state covariance — the filter's own estimate of its uncertainty. K_k is the matrix generalisation of the scalar gain in the developer example, computed fresh every step from Q and R.

Nonlinear extensions

Robots rarely move in straight lines, so F and H are usually nonlinear in practice. Three standard fixes:

  • Extended Kalman Filter (EKF). Linearise F and H around the current estimate using their Jacobians, every step. Cheap, and the default choice for decades of production robotics.
  • Unscented Kalman Filter (UKF) (Julier and Uhlmann, 1997). Propagate a small set of deterministically chosen sample points through the true nonlinear function instead of linearising. More accurate for strongly nonlinear systems, at a higher constant cost.
  • Particle filter. Represent the belief as a weighted set of samples rather than a Gaussian. Handles multimodal beliefs — genuinely being unsure whether the robot is in room A or room B — which a Kalman filter, Gaussian by construction, cannot represent.

Complexity

The full matrix update costs O(n^3) per step for an n-dimensional state, dominated by the matrix inverse in the gain computation. For most robot pose problems n is small (position, velocity, orientation — a handful of dimensions), so this runs comfortably in real time. Large-scale SLAM (simultaneous localisation and mapping) systems avoid the cubic cost with sparse factor graphs instead of a dense Kalman filter.

Current practice

Modern robot state estimation increasingly uses factor graph optimisation (GTSAM, Ceres) rather than a sequential Kalman filter. It can revisit and correct past estimates when new information arrives — something a forward-only filter cannot do. The Kalman filter remains standard for tightly real-time, resource-constrained loops: flight controllers, wheel odometry, and any inner control loop running at hundreds of hertz on modest hardware.

Key references

  • Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering.
  • Julier, S. J., Uhlmann, J. K. (1997). A New Extension of the Kalman Filter to Nonlinear Systems.
  • Thrun, S., Burgard, W., Fox, D. (2005). Probabilistic Robotics. MIT Press — the standard textbook treatment, including particle filters and SLAM.
  • Dellaert, F., Kaess, M. (2017). Factor Graphs for Robot Perception. Foundations and Trends in Robotics.

What to learn next