The scalar Kalman filter

The exponential filter from the last lesson has one fixed number, alpha, that you pick by hand and never revise. But think about what a sensible person does with two conflicting pieces of information. If you are fairly sure where you are and a new reading is wildly different, you doubt the reading. If you are lost and a decent reading arrives, you grab it. The Kalman filter formalises this instinct: it tracks how uncertain it is, and uses that uncertainty to decide, at every single step, how much to trust the new measurement. We start with the smallest possible version, one number to estimate, so every piece is visible.

Two numbers: the estimate and its variance

The filter's memory has two entries. The estimate x is its best guess of the state, say the position of a robot along a line. The variance P says how wrong that guess might be: a small P means confident, a large P means unsure. Treating the belief as a bell curve with centre x and variance P is the key modelling choice, and it is why the maths ends up so clean.

Every cycle has two steps, always in the same order: predict, then update.

Predict: move the belief forward

Suppose we command the robot to drive forward by u metres. Our model of the world says the new position is the old position plus u:

x_pred = x + u

P_pred = P + Q

The mean moves by exactly the commanded amount. The variance, however, grows by Q, the process noise. Why? Because the command is never executed perfectly: wheels slip, the floor is uneven, the battery sags. Each prediction adds a little uncertainty. Without Q, a filter would become more and more sure of itself as time passes, and eventually stop listening to the sensor altogether. Q is how you tell the filter that the world is not perfectly predictable.

Update: blend in the measurement

Now a measurement z arrives, with its own noise variance R. First compute how surprising it is, the innovation, then the Kalman gain K:

innovation = z - x_pred

K = P_pred / (P_pred + R)

x = x_pred + K * innovation

P = (1 - K) * P_pred

Read the gain as a ratio of uncertainties. P_pred is the uncertainty in the prediction, R is the uncertainty in the sensor, and K is the share of the total that belongs to the prediction. Rewrite the estimate and the blending is clear:

x = (1 - K) * x_pred + K * z

It is a weighted average of the prediction and the measurement, and the weights are set by who is more certain:

The last formula, P = (1 - K) * P_pred, shows that the variance always shrinks at an update. Equivalently, 1 / P = 1 / P_pred + 1 / R: information from two sources adds up. A measurement can never hurt the estimate, even a poor one, as long as R honestly describes it.

A worked example by hand

Set Q = 1 and R = 4, and start from x = 0 with P = 1. The robot is commanded to move 2 m, and the sensor then reads 3.5 m.

Predict: x_pred = 0 + 2 = 2 and P_pred = 1 + 1 = 2. Update: K = 2 / (2 + 4) = 0.3333. The innovation is 3.5 - 2 = 1.5, so x = 2 + 0.3333 * 1.5 = 2.5 and P = (1 - 0.3333) * 2 = 1.3333. The measurement said 3.5, the prediction said 2, and the answer 2.5 sits closer to the prediction because the sensor variance is twice the predicted variance.

Step two: the robot moves another 2 m and the sensor reads 4.0. Predict: x_pred = 4.5, P_pred = 1.3333 + 1 = 2.3333. Now K = 2.3333 / 6.3333 = 0.3684, the innovation is 4.0 - 4.5 = -0.5, so x = 4.5 - 0.3684 * 0.5 = 4.3158 and P = (1 - 0.3684) * 2.3333 = 1.4737. Let us check with code.

scalar_kf_hand.py

ResetRun

def predict(x, P, u, Q): return x + u, P + Q def update(x, P, z, R): K = P / (P + R) x = x + K * (z - x) P = (1 - K) * P return x, P, K x, P = 0.0, 1.0 Q, R = 1.0, 4.0 moves = [2.0, 2.0] meas = [3.5, 4.0] for k, (u, z) in enumerate(zip(moves, meas), start=1): x, P = predict(x, P, u, Q) print("step %d predict: x=%.4f P=%.4f" % (k, x, P)) x, P, K = update(x, P, z, R) print("step %d update: K=%.4f x=%.4f P=%.4f" % (k, K, x, P))

The output matches the hand calculation line for line: gain 0.3333 and variance 1.3333 after the first step, gain 0.3684 and estimate 4.3158 after the second. Notice that P went up from 1.0 to 1.3333 to 1.4737 instead of shrinking: the 1 we add in every predict step fights the reduction from every update, and the two will settle at a balance.

The same filter on a parked robot

Now the situation from lesson 1: a robot parked 50 cm from a wall, a sensor with noise of 3 cm (so R = 9), and a deliberately bad first guess of 40 cm with a huge variance of 100. The robot does not move, so we use a tiny Q = 0.01.

scalar_kf_wall.py

ResetRun

import random random.seed(4) TRUE_POS = 50.0 # robot parked 50 cm from a wall R = 9.0 # measurement variance (std 3 cm) Q = 0.01 # tiny process noise: the robot barely moves x, P = 40.0, 100.0 # bad first guess, very unsure total = 0.0 for k in range(1, 101): z = TRUE_POS + random.gauss(0, 3.0) total += z P = P + Q # predict (robot stays put) K = P / (P + R) # update x = x + K * (z - x) P = (1 - K) * P if k in (1, 2, 3, 5, 10, 30, 100): print("k=%3d z=%6.2f K=%.3f x=%6.2f P=%.3f" % (k, z, K, x, P)) print("plain average of all 100 readings: %.2f" % (total / 100))

On the very first reading the gain is 0.917: the filter knows it is lost (P is 100) and almost entirely adopts the measurement, jumping from 40 to about 49.3. The gain then falls quickly to about 0.48, 0.32, 0.20 and 0.10 at readings 2, 3, 5 and 10. As the filter grows confident, each new reading moves the estimate less, and P shrinks from 100 to under 1. Look at the pattern K is following: roughly one over k. That is exactly the gain of a running average, where reading k gets a weight of 1 / k. The Kalman filter found this on its own from the variances.

Then Q shows its job. A true running average would keep shrinking its gain towards zero, but here the gain bottoms out near 0.033 and P settles around 0.3. The filter always keeps a little room to react, in case the robot is nudged. The final estimate is near 50 cm and the plain average of all readings also gives about 50.0, but only the filter would have followed a robot that started rolling.