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

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.

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)
    wait(DT)
stop()
print("gain settled at", round(k, 3))

Run this in the simulator

Watch the gain line. It starts near 1, falls quickly, and settles at a steady value. 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.

Task: a Kalman filter in one dimension

Run the scalar filter while driving at the wall. Plot measured, estimate and gain. Print my y: and final gain:. 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 p at 0.01 and watch the filter refuse to believe anything for the first few seconds.
  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"?