Kalman Filter, Live
Lesson 3 · Rocket tracking

Rocket tracking: a 4-number Kalman filter in 2D

Slide 5 ran one cycle of the matrix filter by hand. Now we run the whole loop, many times, on a bigger problem. A rocket coasts from Earth toward Mars, and a noisy GPS reports where it is every second. Think of following a friend's taxi on a phone map: the dot jumps around a little, but you know the car moves smoothly, so you can guess where it really is. We track four numbers at once, x, y and the two velocities, with the five equations from the last slide. The only thing left to choose is how much to trust the model (Q) against the GPS (R).

Lesson 3 · slide 6Rocket tracking
s = (x, y, vₓ, vᵧ)    F = two copies of 1Δt01    H keeps (x, y)    Q vs R decides how smooth
Key ideas
  1. The rocket's state has four numbers: two positions and two velocities.
  2. F is two constant-velocity blocks, one for x and one for y. They are interleaved, because the state lists x, y first and vₓ, vᵧ after.
  3. H keeps the two positions, because GPS sees where the rocket is, not how fast it goes.
  4. Q says how much the motion model can be wrong (bigger Q, less trust in the model). In the lab Q = q·I, so one number q sets it. R says how noisy the GPS is (bigger R, less trust in the GPS). They are the two knobs to tune.
  5. The covariance ellipse grows on every predict and shrinks on every correct.

Four numbers for a rocket

The rocket moves in a plane. To predict where it will be next second, we need where it is and how fast it is going, in both directions. So the state is s = (x, y, vₓ, vᵧ) and P is a 4 by 4 matrix. (We call the state s here, so that x can mean the first position.)

The rocket coasts: no engine, so velocity stays the same from tick to tick, and each position grows by its velocity times Δt. That is the moving mass of slide 4, once for x and once for y:

F = 10Δt0010Δt00100001    H = 10000100

Row 1 of F says x + vₓΔt. Row 2 says y + vᵧΔt. Rows 3 and 4 keep the velocities. H has two rows, one for each GPS coordinate.

One tick with Δt = 1 s. The rocket is at (10, 4) moving (2, 1) per second. Each row of F meets the state s in turn: the new state is (12, 5, 2, 1).

Where are the "two copies" of 1Δt01? The state s is ordered (x, y, vₓ, vᵧ), so the copies are interleaved. Rows 1 and 3, read at columns 1 and 3, are the x copy: they link x with vₓ. Rows 2 and 4, at columns 2 and 4, are the y copy.

x copy: rows 1, 3 and columns 1, 3 of F  →  1Δt01 acting on (x, vₓ)

Q and R: the two knobs

R is the GPS noise. Distances here are in map units (one grid step on the Earth-to-Mars map), and so are all the errors below. Our GPS has σ = 3 map units in each direction, independent, so R = 9·I, a 2 by 2 matrix with 9 on the diagonal. This one you can usually read off a datasheet.

Q is harder. It says how wrong one tick of the model can be. Our rocket is not quite coasting: gravity bends its path, so its velocity drifts. In the lab Q = q·I, and q is the knob. A big q says "my model is poor", so the filter leans on the GPS and the estimate gets jumpy. A small q says "my model is good", so the filter smooths the GPS out. Too small, and it trusts a straight-line model so much that it misses the bend.

To compare settings we use the RMS error (root-mean-square error): square every error, average the squares, take the square root. It is the typical size of the error, and smaller is better.

The two pictures below run the same flight with the same GPS readings. Only q changes. They show the rocket's y position over time, the coordinate that gravity bends.

q = 5: the filter distrusts its model, so the estimate (solid line) chases the GPS dots and wobbles. Its RMS error is 81% of the GPS error.
q = 0.03: the filter trusts its model more. The estimate runs smoothly along the true path (dashed), and its band is narrower. Its RMS error drops to 45% of the GPS error.
RMS error on this flight (what the lab prints): GPS 3.79 (more than 3, because it is the 2D distance, with the x and y errors together)
q = 5: 3.08 (81%)   q = 1: 2.60 (69%)   q = 0.1: 1.94 (51%)   q = 0.03: 1.70 (45%)   q = 0: 5.40 (142%)

Lowering q helps, down to a point: q = 1 just misses the lab's goal of 65%, and q = 0.1 passes. q = 0 is the trap. The filter believes the straight-line model is perfect, so after a few seconds it stops listening to the GPS, and it sails straight past the bend. Its error ends up worse than the raw GPS.

The ellipse on the map

In 1D the uncertainty was a band. Here it is the 2σ ellipse of the position part of P, the top-left 2 by 2 block. After each predict it grows, because the rocket moved without looking. After each GPS fix it shrinks. After a few seconds the two balance, and the ellipse settles to a steady size.

The ellipse is round here because x and y have the same noise and do not interact. Give the GPS a worse east–west accuracy (a bigger first entry of R) and it stretches sideways.

R = 9009: round    R = 36009: east–west σ = 6, north–south σ = 3, so the ellipse gets wider than tall

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 · Predict and measure one tick

Δt = 1 s. The rocket's state is s = (10, 4, 2, 1): at (10, 4), moving 2 per second in x and 1 in y.

Show solutionHide solution
F s = (10 + 2, 4 + 1, 2, 1) = (12, 5, 2, 1)
H F s = (12, 5): the GPS should read about (12, 5)

A reading of (13, 4) would give the innovation (1, −1).

Example 2 · How the uncertainty grows

Start from P = I (4 by 4) and predict one tick with Δt = 1 and Q = 0.

Show solutionHide solution
F P Fᵀ = F Fᵀ = 2010020110100101

Each position variance doubled, from 1 to 2, because an unsure velocity added its own uncertainty. Each position became linked to its own velocity (the 1s), but x and y stay unlinked (the 0s). The map ellipse is the top-left block, 2·I: a circle with σ = √2.

Exercise The filter follows the GPS too closely. Lower q step by step (try 1, then 0.1, then 0.03) until the estimate's error is below 65% of the GPS error: q = 1 just misses, a smaller q passes. The lab prints both RMS errors (root-mean-square: the typical distance from the true path) and their ratio, and the readout under the picture shows them too. Then try q = 0 and see what goes wrong. Edit the code and press Run.
slide_6.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