Kalman filters explained

What a Kalman filter does, what the Kalman gain, Q and R each change, and how it fuses two sensors, shown on a robot measuring a wall with a noisy sensor. Change the numbers, press Run and watch the estimate smooth the noise, lag behind or jump about.

Guidefree, runs in your browser

A Kalman filter makes the best guess it can of something you cannot measure exactly. It has two imperfect sources to work from: a prediction of how the thing should have changed, and a noisy measurement of where it is now. It is in phone navigation, aircraft, drones, self-driving cars and most mobile robots, and it helped guide the Apollo spacecraft to the Moon. On this page a small robot drives towards a wall and measures the gap with a distance sensor that is about 5 cm out on every reading. Each demo below is a real program you can change and run.

The chart under each robot shows up to three lines. measured is what the distance sensor said. estimate is what the filter believes. truth is the real gap, which the simulator knows exactly (position()) and a real robot does not. If the robot knew the truth, it would not need a filter.

The idea in one line

Every step, the filter does two things: predict, then correct.

predict:  x = x + (the change you expect)
          p = p + Q
correct:  k = p / (p + R)
          x = x + k × (reading - x)
          p = (1 - k) × p

• x is the estimate: the filter's best guess.
• p is how unsure the filter is of that guess, as a variance (the spread, squared). It grows every time the filter predicts, because predictions are never perfect, and it shrinks every time it takes a reading.
• Q is how far wrong a prediction can be in one step, also squared.
• R is how noisy the measurement is. A sensor that is typically 5 cm out has R = 5 × 5 = 25.
• k is the Kalman gain: how far the estimate moves towards each new reading. 0 means ignore the reading, 1 means take it completely.

The gain is the clever part. p / (p + R) compares how unsure the filter is of its own guess with how unsure the reading is, and leans towards whichever is better.

Two sensors, one estimate

Here the prediction comes from a second sensor: the optical flow sensor under the robot, which measures how fast the mat slides past. Ten times a second the filter moves its estimate by how far the flow sensor says the robot went, then pulls it a little way towards the distance reading.

from bugbot import *
connect()

# change these numbers and press Run
R = 25.0     # sensor noise: 5 cm, squared
Q = 0.1      # how far off a prediction can be

set_noise(1, depth=5)   # distance() is 5 cm out
GAP = 81.0   # the true gap at the start, cm
DT = 0.1
x = distance()   # first guess: one reading
p = R            # as unsure as one reading

drive(60, 0, 0)
for tick in range(80):              # 8 seconds
    if tick == 50:
        stop()
    # predict: the gap shrinks as we move
    x = x - flow()[1] * DT
    p = p + Q
    # correct: pull towards the new reading
    z = distance()
    k = p / (p + R)
    x = x + k * (z - x)
    p = (1 - k) * p
    plot("measured", z)
    plot("estimate", x)
    plot("truth", GAP - position()[1])
    wait(DT)
stop()

Each sensor is bad in a different way. The flow sensor is smooth, but it drifts: on this robot it reads about 2.5 percent high, so on its own it would slowly wander away from the truth. The distance sensor never drifts, but every reading is noisy. The filter uses the flow sensor for the short term and the distance sensor for the long term, and the result is better than either. This is called sensor fusion, and it is the most common job a Kalman filter does.

The estimate ends about 3 cm below the truth. That comes from the readings, not the filter: the first one was 4 cm low, and over these 8 seconds they happen to average about 2 cm low. A filter can smooth out noise, but it cannot tell when the readings all lean the same way.

The Kalman gain: how much to believe a reading

Now the filter starts from a guess of 0 cm, which is wrong by 81 cm, but it is told how unsure that guess is: P0 = 1000, a spread of about 30 cm. The chart shows the gain as a percentage.

from bugbot import *
connect()

# change these numbers and press Run
X0 = 0.0     # the first guess at the gap, cm
P0 = 1000.0  # how unsure that guess is
R = 25.0     # sensor noise: 5 cm, squared
Q = 0.1      # how far off a prediction can be

set_noise(1, depth=5)
GAP = 81.0   # the true gap at the start, cm
DT = 0.1
x = X0
p = P0

drive(60, 0, 0)
for tick in range(80):              # 8 seconds
    if tick == 50:
        stop()
    x = x - flow()[1] * DT          # predict
    p = p + Q
    z = distance()                  # correct
    k = p / (p + R)
    x = x + k * (z - x)
    p = (1 - k) * p
    plot("estimate", x)
    plot("truth", GAP - position()[1])
    plot("gain %", k * 100)
    wait(DT)
stop()

On the first step p is huge next to R, so the gain is 1000 / 1025, about 0.98, and the filter takes the reading almost whole. Every reading after that makes it surer of itself, p shrinks, and the gain falls. After about 3 seconds it has settled: each new reading moves the estimate 6 percent of the way.

Now try P0 = 1. The guess is just as wrong, but the filter is sure of it. The gain starts at 4 percent, and after 4 seconds the estimate is still 10 cm below the truth. Both runs end with the same gain of 6 percent, because the settled gain depends only on Q and R, not on where the filter started.

Q too small: smooth, but late

Take the flow sensor away and the filter has nothing to predict with. The best it can do is assume the gap stays the same, and Q is then the only thing telling it the gap might change. Here Q is small, which says: the gap hardly changes.

from bugbot import *
connect()

# change these numbers and press Run
R = 25.0     # sensor noise: 5 cm, squared
Q = 0.05     # how much the gap can change

set_noise(1, depth=5)
GAP = 81.0   # the true gap at the start, cm
x = distance()
p = R

drive(60, 0, 0)
for tick in range(80):              # 8 seconds
    if tick == 50:
        stop()
    # predict: no model, so guess it stays put
    p = p + Q
    # correct
    z = distance()
    k = p / (p + R)
    x = x + k * (z - x)
    p = (1 - k) * p
    plot("measured", z)
    plot("estimate", x)
    plot("truth", GAP - position()[1])
    wait(0.1)
stop()

The gain settles at about 4 percent, so the filter treats the falling readings as noise and creeps after them. Try Q = 2: the estimate keeps up to within about 5 cm while the robot drives, but it wobbles far more. Without a prediction you have to choose between smooth and on time. The flow sensor in the first demo is what gave both.

R too small: jumpy

Keep Q at 0.05 and tell the filter the sensor is far better than it is. R = 0.05 claims the readings are good to about 2 mm, when they are about 5 cm out.

from bugbot import *
connect()

# change these numbers and press Run
R = 0.05     # claims 0.2 cm of noise
Q = 0.05     # how much the gap can change

set_noise(1, depth=5)
GAP = 81.0   # the true gap at the start, cm
x = distance()
p = R

drive(60, 0, 0)
for tick in range(80):              # 8 seconds
    if tick == 50:
        stop()
    # predict: no model, so guess it stays put
    p = p + Q
    # correct
    z = distance()
    k = p / (p + R)
    x = x + k * (z - x)
    p = (1 - k) * p
    plot("measured", z)
    plot("estimate", x)
    plot("truth", GAP - position()[1])
    wait(0.1)
stop()

A filter that trusts the sensor too much passes the noise straight through. Only the ratio of Q to R decides the gain: set Q = 25 and R = 25 and you get exactly the same line as this one. So if the estimate is too jumpy you can lower Q or raise R, and if it lags you can do the opposite. The difference is that R can be measured, and Q usually cannot.

A slow sensor and a fast one

A phone's GPS gives a position about once a second, while its motion sensors report many times faster. The Kalman filter runs the prediction on every step and corrects only when a reading arrives. Here the distance sensor is read only every 2 seconds. The chart shows the error (estimate minus truth) and the filter's own idea of how big that error could be: two standard deviations, 2 × √p, either side of zero.

from bugbot import *
connect()

# change these numbers and press Run
EVERY = 20   # ticks between readings
R = 25.0     # sensor noise: 5 cm, squared
Q = 0.5      # cautious for this flow sensor

set_noise(1, depth=5)
GAP = 81.0   # the true gap at the start, cm
DT = 0.1
x = distance()
p = R

drive(60, 0, 0)
for tick in range(80):              # 8 seconds
    if tick == 50:
        stop()
    x = x - flow()[1] * DT          # predict
    p = p + Q
    if tick % EVERY == 0:           # correct
        z = distance()
        k = p / (p + R)
        x = x + k * (z - x)
        p = (1 - k) * p
    err = x - (GAP - position()[1])
    plot("error", err)
    plot("+2 sd", 2 * p ** 0.5)
    plot("-2 sd", -2 * p ** 0.5)
    wait(DT)
stop()

This sawtooth, uncertainty growing while the robot relies on its own motion and falling at each fix, is the picture of every navigation system from a ship's log and a star sight to a phone in a tunnel. Try EVERY = 100: the filter only ever gets the first reading, and the band grows steadily to about 14 cm. Try EVERY = 1 and it settles at about 4 cm. Q = 0.5 is cautious for this flow sensor, which is why the error stays well inside the band.

How to choose Q and R

1. Measure R. Stand the robot still, take 100 readings, and work out their variance: the average of the squared differences from the mean. For the sensor on this page, 100 readings give about 29, close to the 25 it was set to.
2. Set Q to roughly the square of how far the thing could change in one step without your prediction knowing. A robot that might slip half a centimetre per step without the flow sensor seeing it has Q of about 0.25.
3. Start p large if you do not know where you are, and small if you do.
4. Run it and watch the chart. If the estimate trails behind, raise Q. If it jumps about with the readings, lower Q.
5. Plot the difference between each reading and the prediction. It should jump either side of zero. If it stays on one side, your prediction is wrong in a way no choice of Q and R will fix.

The University lessons below build this filter a step at a time, then take it into two dimensions and use it to steer a robot to a target it cannot see.

Questions

What is a Kalman filter in simple terms?

It is a way of combining a prediction with a measurement to get a better estimate than either gives on its own. Each step it predicts where things should be, then moves that prediction part of the way towards the new measurement. How far it moves depends on how much it trusts each one, and it keeps track of that trust as it goes.

Why do we need Kalman filters?

Because every sensor is wrong in some way. Some are noisy, like the distance sensor on this page, and some drift, like the flow sensor. Averaging readings removes noise but makes the answer late when things are moving. A Kalman filter uses a prediction of the movement to stay on time, and the measurements to stop the prediction drifting, so it gets a smooth answer that keeps up.

What is the Kalman gain?

It is the fraction of the way the estimate moves towards each new reading, worked out as k = p / (p + R). When the filter is unsure of its own guess (p large) the gain is near 1 and it follows the reading. When it is confident (p small) the gain is near 0 and it mostly ignores the reading. With fixed Q and R the gain settles to a steady value, 6 percent in the first demo on this page.

What do Q and R mean in a Kalman filter?

R is the measurement noise: the variance of the sensor's readings, which you can measure by keeping still and taking plenty of them. Q is the process noise: how much the real state can change in one step in ways your prediction does not know about, such as slips, bumps or a push. Small Q or large R gives a smooth estimate that is slow to respond. Large Q or small R gives a quick estimate that is noisy.

What is a simple example of a Kalman filter in Python?

The one-dimensional filter used on this page is six lines inside a loop:

x, p = first_reading, R
while True:
    x = x + change          # predict
    p = p + Q
    z = read_sensor()       # correct
    k = p / (p + R)
    x = x + k * (z - x)
    p = (1 - k) * p

change is how far you expect the value to move in one step, and it can be 0 if you have no idea. Every demo above is a complete, runnable version of this.

Is a Kalman filter the same as a low pass filter?

Once the gain has settled, the correct step is the same sum as a simple low pass filter (an exponential moving average) with the gain as its weight. The difference is the prediction. A low pass filter lags behind anything that moves, as in the "Q too small" demo. A Kalman filter moves its estimate with the prediction first, so it can smooth heavily and still keep up. It also works out the gain for itself from Q and R, and changes it when readings stop or the situation changes.

What is sensor fusion?

Combining two or more sensors into one estimate that is better than any of them. In the first demo the flow sensor, which is smooth but drifts, and the distance sensor, which is noisy but does not drift, are fused by a Kalman filter. Phones fuse GPS with motion sensors in the same way, and drones fuse gyros, accelerometers and a compass.

What is the difference between a Kalman filter and a particle filter?

A Kalman filter keeps one best guess and one measure of how unsure it is, which is the same as assuming the error follows a single bell curve. A particle filter keeps hundreds or thousands of separate guesses, called particles, and scores each one against every reading. That lets it hold several possibilities at once, for example a robot that could be in either of two identical corridors, and it copes with sensors and movements that a Kalman filter cannot describe. It costs far more computing.

What is an extended Kalman filter?

The ordinary Kalman filter assumes everything is linear: the prediction and the measurement are just sums and multiples of the state. Most real robots are not like that. A robot that turns moves by amounts that depend on the sine and cosine of its heading. The extended Kalman filter (EKF) handles this by working out a straight-line approximation of the model around the current estimate at every step, using its derivatives, and then running the ordinary filter on that. It is the standard filter in robot and drone navigation.

Why is my Kalman filter lagging behind?

Either Q is too small or R is too large, so the filter trusts its prediction more than it should, or the prediction itself is missing the motion. If your filter only predicts "stays the same", as in the "Q too small" demo, any real movement looks like noise to it. Add a prediction of the motion if you can measure it, or raise Q until the estimate keeps up.

Learn it step by step

These lessons build the same ideas one at a time, each with tasks the simulator marks.

1. U4.1 A reading is a distribution Noise and filtering, University
2. U4.3 The low pass filter Noise and filtering, University
3. U4.5 The price of filtering Noise and filtering, University
4. U6.1 Two sources, one state State estimation, University
5. U6.2 Predict and correct State estimation, University
6. U6.3 The Kalman gain State estimation, University
7. U6.4 Q and R State estimation, University
8. U6.5 Covariance and the ellipse State estimation, University
9. U6.6 Fusing a fix State estimation, University
10. U6.7 Project: navigate on the estimate State estimation, University