The Kalman gain

Where the weighting comes from: two variances, one line of arithmetic, no taste involved.

U6.3State estimationUniversity35 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: Kalman filters explained

In the last lesson the weighting was 0.1, chosen because it looked about right. The Kalman filter's contribution is that the right weighting is not a matter of taste. It follows from how uncertain each number is.

The scalar filter, in full

Carry two numbers: the estimate x, and its variance p, which is how uncertain you are, in squared units.

# predict
x = x + v * dt
p = p + Q                       # the prediction adds uncertainty

# correct
k = p / (p + R)                 # the Kalman gain
x = x + k * (measured - x)
p = (1 - k) * p                 # the measurement removes uncertainty

Six lines, and it is the optimal estimator for a linear system with Gaussian noise. Not a good one: the optimal one, in the sense that no other estimator has a smaller expected squared error.

Reading the gain

k = p / (p + R) is a ratio of uncertainties, and it behaves the way common sense says it should.

  • Estimate very uncertain (p large): k approaches 1. Take the measurement and more or less start again.
  • Estimate confident (p small): k approaches 0. Ignore the measurement.
  • Both similar: k near 0.5. Split the difference.
The Kalman gain against the variance p, for R = 25: near 0 when p is small, a half when p equals R, towards 1 when p is large025507510012515000.51p, the estimate's variance (cm²)gain kp = R = 25: split the differencefirst tick: 0.80settled: 0.132k = p / (p + R)
The gain depends only on how the two uncertainties compare. The demo starts unsure, p = 100.5 against R = 25, so its first gain is 0.80 (red). As p shrinks it slides down the curve to where it settles, 0.132 (green). The gain never reaches 1 or 0: it only heads towards them.

And it is automatic. At the start, when p is large, the filter grabs onto measurements. After a while, when p has settled, it barely moves. That is exactly the behaviour you would hand-code, and here it falls out of the arithmetic.

from bugbot import *
connect()

DT, START_GAP = 0.1, 110.0
R, Q = 25.0, 0.5
est, p = 0.0, 100.0

forward(60)
for i in range(70):
    est += flow()[1] * DT
    p += Q
    measured = START_GAP - distance()
    k = p / (p + R)
    est += k * (measured - est)
    p = (1 - k) * p
    plot("measured", measured)
    plot("estimate", est)
    plot("gain", k * 100)       # as a percentage, so it shows beside readings in cm
    wait(DT)
stop()
print("gain settled at", round(k, 3))

Run this in the simulator

A gain between 0 and 1 beside readings of up to about 80 cm would be moved to a scale of its own on the right of the chart. Plotted as a percentage it shares the readings' scale instead, so the two can be read against each other: 80 on the chart is a gain of 0.8.

Watch the gain line. It starts at 80 percent, because on the first tick p is 100.5 against an R of 25, and 100.5 / 125.5 is 0.80. It falls quickly, to 45 percent on the second tick and 14 percent after a second, and settles at 13.2 percent. Once it has settled, the Kalman filter is doing exactly what an exponential filter with that alpha would do. The difference is that it found the alpha itself, and it will change it if the situation changes.

The steady state gain

For constant Q and R, p converges, and so does k. The settled value is

p = (Q + sqrt(Q^2 + 4*Q*R)) / 2,    k = p / (p + R)

This is worth knowing for two reasons. It tells you what your Q and R actually mean in terms of smoothing. And on a small microcontroller you can compute that gain once, hard-code it, and skip the variance arithmetic entirely. Many shipped systems do exactly that.

The variance p over the first three seconds: each tick it rises by Q and then falls with the correction, until the rise and the fall are the same size012305101520time, secondsp, cm²first tick: 100.5, far off the topsettled: 3.8 after predict,3.3 after correcttick 4: 8.5 to 6.3
Each tick, the prediction adds Q = 0.5 to p, and the correction takes away a share k of it. At first the correction takes far more than Q adds, so p falls fast. As p falls, k falls, and the correction takes less, until it takes exactly the 0.5 the prediction added. That balance is the settled value.

Task: a Kalman filter in one dimension

Run the scalar filter while driving at the wall. Plot measured, estimate and gain, with the gain as a percentage so that it shows. Print my y: and final gain:, the gain itself as a number between 0 and 1. No position().

from bugbot import *
connect()

DT, START_GAP = 0.1, 110.0
R = 25.0
Q = 0.5
est, p = 0.0, 100.0

Challenges

  1. Start est at 30, which is wrong, and p at 0.01, which says you are sure of it. How long does the filter take to believe the measurements, and how does that compare with p at 100?
  2. Work out the steady state gain from the formula and check it against the one you measured.
  3. Double R and halve it. Which way does the gain go, and does that match the sentence "R is how noisy the measurement is"?