Kalman Filter, Live
Lesson 2 · Correct

The correct step: fuse the prediction with the GPS

Slide 3 slid the bell forward and made it wider. Now a GPS reading arrives, and the filter does its second step, the correct step. Picture opening your eyes for a second in a dark room: you see a blur that says you are a bit further left than you thought. You do not jump to the blur, and you do not ignore it. You shift part of the way, more if your guess was shaky, less if the blur was poor. This is lesson 1's two-thermometer problem again: the prediction plays sensor 1, the GPS plays sensor 2, and the gain K says how far to shift.

Lesson 2 · slide 4Correct
K = P⁻ / (P⁻ + R)    x̂ (new) = x̂⁻ + K (z − x̂⁻)    P (new) = (1 − K) P⁻
Key ideas
  1. The correct step is lesson 1's fusion rule, with the prediction as sensor 1 and the GPS as sensor 2.
  2. The innovation z − x̂⁻ is the surprise: how far the GPS is from what we expected.
  3. The Kalman gain K says what fraction of the surprise to believe.
  4. P = (1 − K)P⁻ is always smaller than both P⁻ and R. Looking always makes you more certain.

A quick peek

You have walked a few steps with your eyes closed. You think you are near the sofa. Now you open your eyes for a second, but the room is dark and you only see a blur. The blur suggests you are a little further left. You do not jump to the blur, and you do not ignore it. You shift part of the way. How far depends on how good your guess was and how good the peek was.

Lesson 1, renamed

In lesson 1 we fused z₁ (variance σ₁²) with z₂ (variance σ₂²). Rename the pieces:

sensor 1 = the prediction:   z₁ → x̂⁻,   σ₁² → P⁻
sensor 2 = the GPS:   z₂ → z,   σ₂² → R

Then the fusion rule becomes the correct step. Its answers, x̂ and P (with no minus), are the values after the correction. They become the starting x̂ and P of the next predict step.

K = P⁻ / (P⁻ + R)
x̂ = x̂⁻ + K (z − x̂⁻)
P = (1 − K) P⁻

The middle line expands to x̂⁻ + Kz − Kx̂⁻, which is the same as (1 − K)x̂⁻ + K·z: a weighted average, with share 1 − K for the prediction and share K for the GPS.

The last line is lesson 1's fused variance in disguise. First work out 1 − K:

1 − K = (P⁻ + R)/(P⁻ + R) − P⁻/(P⁻ + R) = R/(P⁻ + R)
so (1 − K)P⁻ = P⁻R / (P⁻ + R), the fused variance from lesson 1

Take the prediction from slide 3, 35 m with P⁻ = 5, and a GPS reading of 38 m with R = 9:

K = 5 / (5 + 9) = 5/14 ≈ 0.357
x̂ = 35 + 0.357 · (38 − 35) ≈ 36.07 m    P = (1 − 5/14) · 5 = 45/14 ≈ 3.21
The corrected bell starts at the prediction and moves about a third of the way toward the GPS. It ends up taller and narrower than both.

The innovation and the gain

The gap z − x̂⁻ is called the innovation. It is the news in the measurement: the part we did not already expect. Above, it is 38 − 35 = 3 m. If the GPS agrees with the prediction, the innovation is zero and the estimate does not move.

The Kalman gain K, between 0 and 1, is the share of that news we accept. If the prediction is shaky (P⁻ big), K is near 1 and we jump toward the GPS. If the GPS is poor (R big), K is near 0 and we mostly keep the prediction. Keep the same prediction and change only the GPS:

survey GPS, R = 0.5:   K = 5 / 5.5 ≈ 0.91  →  x̂ ≈ 35 + 0.909 · 3 ≈ 37.73 m,   P ≈ 0.45
GPS between tall buildings, R = 45:   K = 5 / 50 = 0.1  →  x̂ = 35 + 0.1 · 3 = 35.3 m,   P = 4.5
Orange: the prediction. Green: a very good GPS (R = 0.5), a tall, narrow bell. K ≈ 0.9, so the corrected bell lands almost on top of the GPS.
Orange: the prediction. Green: a poor GPS (R = 45), a low, wide bell. K = 0.1, so the corrected bell barely leaves the prediction.

The variance always shrinks

Since 0 < K < 1, the new P = (1 − K)P⁻ is smaller than P⁻. It is also smaller than R: P = R · P⁻/(P⁻ + R), and P⁻/(P⁻ + R) < 1, so P is smaller than R. In the first example, P ≈ 3.21 is below both P⁻ = 5 and R = 9. After the correct step we are more certain than the prediction and more certain than the GPS.

One full step: predict, then correct

Put slide 3 and this slide together. Start at 30 m with P = 4, move u = 5 m with Q = 1, then correct with the GPS reading 38 m (R = 9):

predict: 30 → 35 m,   P: 4 → 5    correct: 35 → 36.07 m,   P: 5 → 3.21
One full step. The bell slides right and widens (predict). The GPS bell appears. Then the two fuse into a narrower corrected bell (correct).

The predict step grows P, the correct step shrinks it. That tug of war is what stops the drift.

A bigger example: three readings in a row

The robot is parked, so there is nothing to predict (u = 0 and Q = 0). Start from 35 m with P = 5. Three GPS readings arrive, 38, 37 and 36.5 m, each with R = 9.

reading 1, z = 38:   K = 5 / 14 ≈ 0.357  →  x̂ ≈ 36.07 m,   P ≈ 3.21
reading 2, z = 37:   K = 3.21 / 12.21 ≈ 0.263  →  x̂ ≈ 36.32 m,   P ≈ 2.37
reading 3, z = 36.5:   K = 2.37 / 11.37 ≈ 0.208  →  x̂ ≈ 36.35 m,   P = 1.875

Each reading gets a smaller gain, because the filter is already surer. Check with lesson 1's precisions: 1/5 + 1/9 + 1/9 + 1/9 = 0.533, and 1/0.533 = 1.875. Every reading adds its precision to the pile. A thousand readings would keep adding, the same way.

Where this shows up

Every phone that shows a steady blue dot runs a correct step each time a GPS fix arrives. In a city, tall buildings bounce the signal and the GPS gets worse. A good filter raises R there, so K drops and the phone leans on its own prediction until the signal is clean again.

Next: put predict and correct in a loop and run it on the drifting robot. What happens to P over many steps, and how do Q and R change the result?

Examples

Example 1 · Prediction 35 m, GPS 38 m

From the last slide: x̂ = 30 m with P = 4, the encoder says u = 5 m with Q = 1. The GPS then reads z = 38 m with R = 9.

Show solutionHide solution
predict: x̂⁻ = 30 + 5 = 35 m,   P⁻ = 4 + 1 = 5
gain: K = 5 / (5 + 9) = 5/14 ≈ 0.357
correct: x̂ = 35 + 0.357 · (38 − 35) ≈ 36.07 m,   P = 5 · (1 − 5/14) = 45/14 ≈ 3.21

The estimate moved only about a third of the way to 38, because the GPS (R = 9) was less trustworthy than the prediction (P⁻ = 5), so it got the smaller share. The variance dropped from 5 to 3.21: smaller than both.

Example 2 · A GPS that agrees

The prediction is x̂⁻ = 20 m with P⁻ = 2. The GPS reads exactly z = 20 m, with R = 2.

Show solutionHide solution
innovation: 20 − 20 = 0    K = 2 / (2 + 2) = 0.5
x̂ = 20 + 0.5 · 0 = 20 m,   P = (1 − 0.5) · 2 = 1

No surprise, so the estimate does not move. But P still halves: a reading that agrees with you is still evidence.

Example 3 · A big surprise

The prediction is x̂⁻ = 50 m with P⁻ = 1. The GPS reads z = 58 m with R = 4.

Show solutionHide solution
innovation: 58 − 50 = 8 m    K = 1 / (1 + 4) = 0.2
x̂ = 50 + 0.2 · 8 = 51.6 m,   P = 0.8 · 1 = 0.8

The surprise is large, but the filter trusts its prediction four times more than the GPS. So it only takes a fifth of the news: 1.6 m of the 8 m.

Example 4

Do one full step. x̂ = 10 m, P = 2. The encoder says u = 4 m with Q = 2. Then the GPS reads z = 13 m with R = 4.

Show solutionHide solution

Predict: x̂⁻ = 10 + 4 = 14 m, P⁻ = 2 + 2 = 4.

Correct: K = 4 / (4 + 4) = 0.5, x̂ = 14 + 0.5 · (13 − 14) = 13.5 m, P = 0.5 · 4 = 2.

P went 2 → 4 → 2. When the growth and the shrink balance like this, the filter has reached a steady state (more on the next slide).

Example 5

The prediction has P⁻ = 4. How good must the GPS be (what R) for the gain to be K = 0.8?

Show solutionHide solution

K = 4 / (4 + R) = 0.8 gives 4 + R = 5, so R = 1.

The GPS must be four times better in variance than the prediction.

Exercise Find the GPS variance R that makes the filter move exactly halfway from the prediction to the GPS (K = 0.5). Change R and run. Edit the code and press Run.
slide_4.py Loading Python…
1
Output · Ctrl+Enter runs
Press Run to execute this slide's code. Ctrl+Enter works too.

Enjoying lesson 2? Get the full course: all 4 lessons through the matrix filter and the EKF, your progress saved lesson by lesson, every solution and 5 extra problems per lesson. $20 once, yours for life.

Questions and comments

Stuck on something, or found a mistake? Ask here and I'll answer. You sign in with GitHub.

The full course

From noisy sensors to the EKF, by running it

Lessons 1 and 2 are free. Finish all four and you will be able to:

You get: all 4 lessons with Python labs · every worked example and solution · 5 extra problems per lesson · your progress tracked · Steppe, your AI tutor inside every lesson, day or night · every future update.

$20 once · lifetime access · 14-day money-back guarantee
Continue to checkout →

Already a member? Log in to the course