Kalman Filter, Live
Lesson 3 · The matrix filter

The matrix Kalman filter: predict and correct with matrices

Lesson 2 built the 1D filter: predict, then correct, with a gain K that decides how much to trust the sensor. Slide 4 gave us the model as matrices. Now we put the two together. Picture a friend jogging past a row of windows. You only see where she is, a quick glimpse at each window. But from how far she got between two windows, you also learn how fast she runs. The matrix Kalman filter does exactly that. It is the same five equations as before: numbers become vectors, variances become covariance matrices, and dividing by a number becomes multiplying by an inverse matrix (explained below). The reward: a GPS that only sees position also fixes the velocity.

Lesson 3 · slide 5The matrix filter
x⁻ = F x + B u   P⁻ = F P Fᵀ + Q    K = P⁻Hᵀ(H P⁻ Hᵀ + R)⁻¹    x = x⁻ + K(z − H x⁻)   P = (I − K H) P⁻
Key ideas
  1. Two noise terms make the model honest: process noise with covariance Q, and measurement noise with covariance R.
  2. Predict moves the state with F and grows the covariance with F P Fᵀ + Q.
  3. Correct compares the reading z with what we expected to see, H x⁻. The gap is the innovation.
  4. The Kalman gain K = P⁻Hᵀ(H P⁻ Hᵀ + R)⁻¹ is the 1D gain P⁻/(P⁻ + R) written with matrices.
  5. Through the off-diagonal of P⁻, a position reading also corrects the velocity.

Adding noise to the model

The state-space model of slide 4 was perfect physics. Real robots get bumped and real sensors are noisy, so we add one noise term to each equation:

xₖ₊₁ = F xₖ + B uₖ + wₖ,   w ~ N(0, Q)
zₖ = H xₖ + vₖ,   v ~ N(0, R)

w is the process noise: gusts, wheel slip, everything the model leaves out. Its covariance Q plays the role Q played in 1D. v is the measurement noise, with covariance R, the matrix version of the GPS variance R. The true state is hidden: we never see it. We keep a best estimate x and its covariance P, and update both every tick. From here, x means our estimate of the state.

GPS with σ = 2 m:   R = 2² = 4 (a 1 by 1 matrix)    wind adds variance 1 to position and to velocity each tick:   Q = 1001

The five equations, side by side

Predict state:   x⁻ = x + u   →   x⁻ = F x + B u
Predict uncertainty:   P⁻ = P + Q   →   P⁻ = F P Fᵀ + Q
Gain:   K = P⁻ / (P⁻ + R)   →   K = P⁻Hᵀ (H P⁻ Hᵀ + R)⁻¹
Correct state:   x = x⁻ + K(z − x⁻)   →   x = x⁻ + K(z − H x⁻)
Correct uncertainty:   P = (1 − K) P⁻   →   P = (I − K H) P⁻

The left side is lesson 2. The right side is the same rhythm for vectors. If you only need a recipe, this is it: write F, H, Q and R for your system and run the loop. The rest of the slide shows where each line comes from, on one running example: x = (30, 5) (30 m, 5 m/s), P with 4 and 1 on the diagonal, F = 1101, Q = I, H = 10, R = 4, and a GPS reading z = 38. The first worked example below collects the whole cycle.

The covariance through one cycle of the running example. It grows twice in the predict step (the move, then Q) and shrinks once when the GPS reading comes in.

Predict: the transform rule plus Q

Move the state with the model: x⁻ = F x + B u. Moving a vector by F moves its covariance by F P Fᵀ, the rule from slide 3. Then add Q for the bumps the model ignores. The ellipse grows, because we moved without looking.

x⁻ = F (30, 5) = (30 + 5, 5) = (35, 5)
P⁻ = F P Fᵀ + Q = 5111 + 1001 = 6112
The predicted uncertainty P⁻, drawn around the predicted state (35, 5): position error across, velocity error up. Wide (position variance 6, so σ = √6 ≈ 2.45 m) and leaning a little up to the right.

Correct: fuse in the sensor's space, then come back

The GPS reads one number, but the state has two. We cannot fuse things of different sizes, so first we turn the prediction into a predicted reading: H x⁻, with variance H P⁻ Hᵀ (slide 3 again). Now we have two estimates of the same reading: H x⁻ from the model, and z from the sensor with variance R.

predicted reading H x⁻ = 35, variance H P⁻ Hᵀ = 6    GPS z = 38, variance R = 4

Fuse them with the lesson 1 rule. Its gain was σ₁² / (σ₁² + σ₂²). Here σ₁² is H P⁻ Hᵀ, the variance of the model's predicted reading, and σ₂² is R, the sensor's variance. Call the total S = H P⁻ Hᵀ + R. With matrices, "divide by S" becomes "multiply by the inverse S⁻¹".

A quick word on the inverse. For one number, S⁻¹ is 1/S. For a matrix, S⁻¹ is the matrix that undoes S: S⁻¹S = I. For a diagonal matrix, flip each diagonal entry: 100010⁻¹ = 0.1000.1. Bigger ones you leave to the computer: np.linalg.inv in the lab.

gain for the reading: H P⁻ Hᵀ S⁻¹    here S = 6 + 4 = 10, and the gain is 6 / 10 = 0.6

But we want to correct the state, not the reading. The rule in one plain sentence: each state number is moved by (its covariance with the reading) ÷ S. For H = 10, the reading is the position, so "covariance with the reading" means "covariance with the position": the first column of P⁻ (6 and 1). We divide that column by S. A number strongly linked to the position moves a lot; an unlinked one does not move. In matrix form:

K = P⁻ Hᵀ S⁻¹    x = x⁻ + K (z − H x⁻)

K is the Kalman gain. The gap z − H x⁻ is the innovation: how much the reading surprised us.

P⁻Hᵀ = first column of P⁻ = (6, 1),   K = (6, 1) / 10 = (0.6, 0.1)
innovation 38 − 35 = 3,   x = (35, 5) + 3 · (0.6, 0.1) = (36.8, 5.3)

Why this works (optional)

Two facts make the rule above the right one. You can skip this part on a first read.

First, P⁻Hᵀ really is the covariance between each state number and the predicted reading. The state's deviation is d and the reading's deviation is H d. Since (H d)ᵀ = dᵀHᵀ, we get d (H d)ᵀ = d dᵀ Hᵀ, and its average is P⁻ Hᵀ, by the same reasoning as slide 3.

Second, this K gives exactly the fused reading from the lesson 1 rule. One step at a time:

1. Fused reading = H x⁻ + (gain for the reading)·(z − H x⁻)
2. Split off the first H: H P⁻ Hᵀ S⁻¹ = H · (P⁻ Hᵀ S⁻¹). Call the bracket K = P⁻ Hᵀ S⁻¹
3. Fused reading = H x⁻ + H K (z − H x⁻)
4. Pull out the common H: fused reading = H [x⁻ + K (z − H x⁻)]
5. So the state in the bracket gives the fused reading: x = x⁻ + K (z − H x⁻)

This K moves each state number in proportion to how strongly it is linked to the position, so an unlinked number does not move at all.

Keep reading with the full course

Finish lesson 3 and you will be able to: track position and velocity in 2D with the matrix Kalman filter.

Still locked here:

  • The rest of this explanation
  • 3 more examples with step-by-step solutions
  • Five extra members-only problems with worked solutions

$20 once, lifetime access. Lessons 1 and 2 stay free.

Examples

Example 1 · One full cycle in numbers

Δt = 1 s. Start at x = (30, 5): 30 m, 5 m/s. P = 4001, F = 1101, Q = I, H = 10, R = 4. The GPS reads z = 38.

Show solutionHide solution
1. Predict (no push, so B u = 0): x⁻ = F x = (35, 5)
P⁻ = F P Fᵀ + Q = 5111 + 1001 = 6112
2. Gain: S = H P⁻ Hᵀ + R = 6 + 4 = 10,   K = (6, 1) / 10 = (0.6, 0.1)
3. Correct: innovation 38 − 35 = 3,   x = (35, 5) + (0.6, 0.1)·3 = (36.8, 5.3)
K H = 0.600.10 (column K times row H),   I − K H = 0.40−0.11
P = 0.40−0.11 6112 = 2.40.40.41.9

The position variance fell from 6 to 2.4 as the GPS pinned it down. The velocity moved too, from 5 to 5.3, and its variance fell from 2 to 1.9. An off-diagonal of 0.4 remains: position and velocity errors are still linked. That one update is the whole Kalman filter, in numbers.

Example 2 · Velocity learned from position

Start at x = (0, 2), P = I, Δt = 1, Q = 0, R = 1. The GPS reads 3.

Show solutionHide solution
x⁻ = (2, 2),   P⁻ = F Fᵀ = 2111
S = 2 + 1 = 3,   K = (2, 1) / 3 = (0.667, 0.333)
innovation 3 − 2 = 1,   x = (2.667, 2.333)
P = 2/31/31/32/3

The robot was 1 m further than expected, so the filter decides it was faster than it thought: velocity up from 2 to 2.33 m/s. The velocity variance fell from 1 to 2/3 without any speed sensor.

Exercise Give the robot a better GPS. On the R line, change 4. to 1. Predict K and the new position variance by hand, then run to check. Edit the code and press Run.
slide_5.py Loading Python…
1
Output · Ctrl+Enter runs
Press Run to execute this slide's code. Ctrl+Enter works too.

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