The Kalman filter without heavy math

TL;DR:

• A Kalman filter keeps a best estimate and its uncertainty (variance), and updates both in two steps: predict, then update.

• The Kalman gain K = P / (P + R) says how much to trust the new measurement compared with the prediction.

• The result always has less uncertainty than either input, and the filter works best when the noise is roughly Gaussian and the system roughly linear.

Your odometry says the robot is at 10.0 m. Your range sensor says 10.6 m. Which do you believe? Average them? Take the better one? The Kalman filter gives a principled answer: weigh each by how uncertain it is, and keep track of how uncertain the result is. It is a famous algorithm with a reputation for hard math, but the one-dimensional version is mostly a weighted average.

What it does on a robot

Robots rarely measure what they want directly. Position comes from several noisy sources. A Kalman filter is a recipe for combining a model of how the robot moves (the prediction) with noisy measurements (the update) to produce a better estimate than either. It runs in a loop, uses little memory and is cheap enough for a small microcontroller. It is used for GPS and IMU fusion, drone attitude, wheel odometry with a gyroscope, and as a part of many SLAM systems.

Uncertainty as a variance

The filter keeps two numbers for each quantity: the estimate x and its variance P, which is the square of the standard deviation σ. A small variance means a confident estimate. All the filter's work is to update these two numbers.

The two steps

Predict. Use the motion model to move the estimate forward. If the robot is commanded to move 1.0 m, then x ← x + 1.0. Motion is never exact, so the variance grows: P ← P + Q, where Q is the variance of the motion noise.

Update. A measurement z arrives with variance R. Compute the Kalman gain:

K = P / (P + R)

Then correct the estimate and shrink the variance:

x ← x + K (z − x) P ← (1 − K) P

If the measurement is very noisy (R large), K is near 0 and the estimate barely moves. If the measurement is very precise (R small), K is near 1 and the estimate jumps to the measurement. The term z − x is the innovation, the surprise in the measurement.

Worked example: fusing two noisy position estimates

The prediction says x = 10.0 m with standard deviation 0.5 m, so P = 0.25. The sensor says z = 10.6 m with standard deviation 0.3 m, so R = 0.09.

1. Gain: K = 0.25 / (0.25 + 0.09) = 0.25 / 0.34 = 0.735.

2. New estimate: x = 10.0 + 0.735 × (10.6 − 10.0) = 10.0 + 0.441 = 10.441 m.

3. New variance: P = (1 − 0.735) × 0.25 = 0.265 × 0.25 = 0.0662, so σ = 0.257 m.

Check with the inverse-variance formula: 1 / (1/0.25 + 1/0.09) = 1 / (4 + 11.11) = 1 / 15.11 = 0.0662. ✓ The estimate sits closer to the sensor (10.6) because the sensor was more certain, and the uncertainty is below both 0.5 m and 0.3 m.

Now the robot moves a commanded 1.0 m, with motion noise variance Q = 0.04.

1. Predict: x = 10.441 + 1.0 = 11.441 m. P = 0.0662 + 0.04 = 0.1062, so σ = 0.326 m. The uncertainty grew, as it should when the robot moved without measuring.

2. A new measurement z = 11.3 m with R = 0.09. Gain: K = 0.1062 / (0.1062 + 0.09) = 0.1062 / 0.1962 = 0.541.

3. Update: x = 11.441 + 0.541 × (11.3 − 11.441) = 11.441 − 0.0764 = 11.365 m. P = (1 − 0.541) × 0.1062 = 0.0487, σ = 0.221 m.

Over the cycle, the uncertainty goes up when the robot moves and down when it measures, the same sawtooth as in localization.

Beyond one dimension

For a robot with position, velocity and heading, x becomes a vector and P becomes a matrix (a covariance matrix) that also records how errors in different quantities are related. The formulas have the same shape with matrix operations. If the motion or measurement is not linear, as in most robots, the extended Kalman filter approximates it by a straight line around the current estimate.

Where it goes wrong

• Wrong noise values. If Q and R are guesses, they need tuning. A Q that is too small makes the filter stubborn: it keeps trusting its model while reality drifts away.

• Non-Gaussian noise. A sensor that sometimes gives wild outliers breaks the assumption. Reject readings whose innovation is too large.

• Nonlinear systems. The extended version can diverge if the linear approximation is poor.

• Wrong model. If the motion model is wrong, the filter will confidently follow the wrong path.

• Several guesses. A Kalman filter keeps one hypothesis, so it cannot represent "I could be at either of these doors". A particle filter can.

• Timing. Measurements must be matched to the right time, or the innovation is wrong.

Safety: A filter that believes its own estimate can send a robot or drone into trouble when a sensor fails silently. Check the innovation and the variance, and fall back to a safe action (stop, hover, land) if the estimate becomes inconsistent or the uncertainty exceeds a limit. Keep the emergency stop independent of the filter.

How real robots use it

Drones fuse a gyroscope, an accelerometer, a barometer and GPS with Kalman-type filters. Wheeled robots fuse encoder odometry with an IMU. In SLAM, the same ideas estimate both the robot and the landmarks together, covered in SLAM explained. Even a smoothing filter like those in sensor noise is a simple relative of it.

Try it yourself

Write a ten-line program of the one-dimensional filter above. Simulate a robot that moves 1 m per step, with odometry noise of standard deviation 0.2 m and a range sensor of standard deviation 0.3 m. Plot the true position, the odometry-only estimate and the filter's estimate over 50 steps. The filter should stay close to the truth while odometry drifts away.

Frequently asked questions

What does the Kalman gain mean?

It is the weight given to the new measurement, between 0 and 1. It is large when the prediction is uncertain and the sensor is precise, and small when the opposite holds.

Why does the variance shrink after an update?

Two independent estimates together carry more information than either alone, so the combined uncertainty is smaller than both.