Extended Kalman Filter (EKF)

0.0 Warm-up: From a Ruler to the Distance Across Space (Pythagoras)

Before writing down any GPS equation, let us nail the most basic thing in math — measuring distance. Just look at the picture:

• 1-D (a line): on a ruler, the distance from A to B is one coordinate minus the other. To stop caring which is bigger (kill the minus sign), square it: distance² = (x₂ - x₁)²
• 2-D (a tabletop): the distance between two points on a table is the hypotenuse of a right triangle. Pythagoras: hypotenuse² = the sum of the two legs²: distance² = (x₂ - x₁)² + (y₂ - y₁)²
• 3-D (space): up in the sky, add the Z axis and you are simply drawing the diagonal of a box. Notice how the 2-D floor distance plus the Z gap forms yet another right triangle. The rule stays brutally simple: distance² = (2-D floor diagonal)² + (Δz)² = (x₂ - x₁)² + (y₂ - y₁)² + (z₂ - z₁)²

0.1 Prerequisite: Compass-and-Ruler Construction at Planetary Scale (How GPS Works)

Ever wondered how GPS knows where you are? At heart it is the middle-school exercise of drawing circles and finding where they cross.

A satellite sends a signal to your phone; the speed of light c times the time gap (t_i - t) is the distance (the radius). Four satellites give exactly four equations — enough to solve for your receiver's 3-D position (x, y, z) plus its clock offset t:

• 👈 t₁, t₂, t₃, t₄: the instants the satellites transmitted. Satellites carry atomic clocks, so these four times are perfectly synced and accurate.
• 👈 t: your receiver's unknown clock time. A receiver only has a cheap quartz clock, so its time is off. That is exactly why we need a 4th satellite — to solve for this unknown t (the receiver clock offset) together with your 3-D position (x, y, z).

Note: centimetre-level RTK (carrier-phase differential) gets its own chapter. Here we treat GPS as a black box that hands back noisy $(x, y)$ coordinates.

0.2 Prerequisite: Slicing Time (Odometry Integration)

What do you do when GPS is gone? You lean on the wheel encoder (linear speed v) and the IMU (yaw rate ω), adding up every tiny move. That is odometry.

Slice time into tiny pieces dt (say 0.1 s). Add up those tiny coordinate changes step by step along the path (that is a Riemann sum) — and you have your dead-reckoned trajectory.

1. Error and Variance: First, Measure How Wrong It Is

In an EKF we stop believing in exact numbers. Every sensor is noisy, so we describe them with statistics.

What Are Error and Variance?

Say the wheel encoder's true speed is 10 m/s. We read it 3 times in a row and get three slightly-off numbers. Each gap is an error:

• Reading 1: 10.2 ➡ error = +0.2
• Reading 2: 9.9 ➡ error = -0.1
• Reading 3: 9.9 ➡ error = -0.1

Careful! If we just average the errors: (+0.2 - 0.1 - 0.1) / 3 = 0! The signs cancel perfectly, handing us the fatal illusion of a flawless sensor — when in fact the data was jumping around the whole time.

To stop the signs from cancelling, mathematicians found a beautiful trick: square every error (a negative squared turns positive), add them up, divide by the number of readings. That is the strict definition of variance (σ²).

In our example: σ² = (0.2² + (-0.1)² + (-0.1)²) / 3 = (0.04 + 0.01 + 0.01) / 3 = 0.02. The real jitter is now pinned down exactly.

3 Readings Tell You Nothing — So Take 10,000

Jack the car up, let the wheels spin free, lock the true speed at 10 m/s. Hook a computer to the encoder and read forever — one dot on the number line per reading. Where do the dots pile up?

Nobody arranged this shape. We just let it read, and the dots piled up by themselves: densest in the middle, thinner toward both ends, roughly symmetric.

Watch that σ² at the end — it settles near 0.02, exactly the number we worked out by hand from 3 readings. This piled-up curve has a name: the normal distribution, a.k.a. the bell curve.

And variance σ² is simply how fat this bell is — fatness is the only degree of freedom it has:

The Strange Part: Measure Something Else, Same Bell

What you measure	What tiny causes add up behind it	Shape
One wheel-encoder reading	gear backlash + road grit + tyre flex + timing jitter + …	bell
One IMU angle reading	temperature drift + circuit noise + vibration + …	bell
The heights of one school class	hundreds of genes + nutrition + sleep + exercise + …	bell
The diameters of one batch of screws	tool wear + material hardness + machine vibration + …	bell

Look at the second column — what they share is this: the result is a big pile of tiny causes ADDED together. That is where the bell comes from.

Why? Just Count Them

Cut it down to only 5 causes, each nudging the reading by +0.02 or −0.02. That is 2×2×2×2×2 = 32 combinations. Count them one by one:

Only 5 causes and the bell is already showing. A real wheel encoder has hundreds or thousands of them; the bars pack so tight you cannot see the seams, and out comes the smooth curve from the top of this section.

Height works the same way: to reach 2 m, hundreds of genes must all vote tall — far too few paths do that, so very tall and very short people are rare and everyone else piles into the middle.

This law is the foundation for everything that follows: nature has already fixed the shape, so to describe an uncertain quantity we owe only two things — where the centre is, and how fat the bell is (the variance).

Those two are all the EKF ever moves around. Once the state becomes three numbers x, y, θ, the centre becomes three numbers and the fatness has to grow into a table — that is the P matrix we will bring out later.

2. Three Steps with Your Eyes Shut: How the Error Snowballs

One step, and only two things happen: the old error gets carried forward — the heading is already tilted, so θ's error leaks into x and y; and this step's readings are each wrong by one more helping — the encoder's distance and the IMU's angle.

First: How the Error Gets Carried

Let the car drive due east (θ = 0), 1 metre per step. Now the IMU is off by a hair and the heading is tilted by δθ — just look where the step lands:

Both are on the books. But they are nowhere near the same size — plug in real numbers and look:

heading off by δθ	y gains sin(δθ)	x loses 1−cos(δθ)	ratio
0.1 rad (5.7°)	0.0998 m ≈ 10 cm	0.0050 m = half a cm	20×
0.01 rad (0.57°)	0.0100 m = 1 cm	0.00005 m = 0.05 mm	200×

Read the last column: shrink δθ by 10× and the sideways part shrinks 10×, but the along-track part shrinks 100×. Because sin(δθ) ≈ δθ while 1−cos(δθ) ≈ δθ²/2 — one is the error itself, the other is the error squared. We are not going to bookkeep 0.05 mm.

And that is how the three carry-forward rules fall out (δ reads «error of»):

There are only two sources of error — the wheel encoder measuring «how far», and the IMU measuring «how much I turned». Look:

Sensors are never accurate — every reading is wrong by one helping, and how wrong this time has nothing to do with last time — so every step adds another Q to the books. At the start the books are all zeros. Let us compute:

Step 1: the old books are all zero, so nothing to carry; the encoder and the IMU are each wrong by one helping this time:

• Var(x) = 0.01 (encoder)　Var(y) = 0　Var(θ) = 0.01 (IMU)
• ← sideways is 0: no sensor measures it, and θ only just went wrong — no time yet to drag y off course.

Step 2: the old books are no longer zero, and θ's error starts leaking into y. One at a time.

x and θ are easy — each just books one more helping from this reading: Var(x) = 0.01 + 0.01 = 0.02 (encoder), Var(θ) = 0.02 (IMU).

y is the awkward one, because new δy = δy + δθ — two errors added together. Variance is «the average of the squared error», so square it, using the plain old (a+b)² expansion:

The first and last terms we know. But that middle term, average(δy·δθ), is brand new — it is nobody's variance. So what is it?

That New Thing Is Called Covariance

average(δy·δθ) is not anyone's variance — it measures whether two errors move together. Why would y and θ move together? Because they are joined at the hip. Look:

Suppose the heading should have been due east (the ideal path), but the sensor is slightly off and the nose points a little north. As the car drives forward, look at the picture above:

X falls short (a negative error) while Y gains a stretch out of nowhere (a positive error).

In other words, the moment θ is wrong, the errors in X and Y become linked. How do mathematicians measure that link? Simple: multiply each X error by its Y error, then average:

In this picture: X is negative, Y is positive. Negative times positive is negative. So the covariance comes out negative — the maths says precisely: «whenever X runs short, Y must run long».

About that yellow covariance ellipse: why is a scatter sometimes a circle and sometimes an ellipse? Let us just compute two hardcore examples:

• Case 1: X and Y errors are independent (scatter is a circle)
  Four readings. X errors are [+1, -1, -1, +1], Y errors are [+1, +1, -1, -1].
  Each jumps around on its own, no pattern. Plug into the formula:
  Cov = ( (1×1) + (-1×1) + (-1×-1) + (1×-1) ) / 4 = (1 - 1 + 1 - 1) / 4 = 0
  👈 Meaning: covariance = 0, no link at all. The dots spread evenly in every direction, and the error boundary is a circle.
• Case 2: X and Y are strongly linked (scatter becomes a tilted ellipse)
  Back to the picture above. Because the nose is tilted, whenever X falls short (negative), Y must gain (positive).
  X errors are [-1, -2, 1, 2], Y errors are [2, 4, -2, -4] (strictly anti-correlated).
  Cov = ( (-1×2) + (-2×4) + (1×-2) + (2×-4) ) / 4 = (-2 - 8 - 2 - 8) / 4 = -5
  👈 Meaning: covariance = -5. That big negative number says most of the dots sit in the second and fourth quadrants (upper-left and lower-right). So in 2-D the circle gets stretched into a tilted ellipse (the yellow shape in the figure).

Back to step 2. At this instant y and θ have no relationship yet, so Cov(y,θ) = 0. Plug it in:

But by the end of this step things have changed — a helping of δθ is now baked into δy, and the two are welded from here on:

← the covariance grew out of nothing. Remember this number; next step it bites.

Step 3: now that 2 × Cov(y,θ) really bites — because Cov(y,θ) is no longer 0:

The link keeps thickening too: Cov(y,θ) = 0.01 + 0.02 = 0.03. Meanwhile honest old x plods along: Var(x) = 0.03.

Draw the Three Steps of Error on the Path

Variance is in «metres squared», which you cannot draw on the ground. Take the square root to get back to metres (section 1: variance is the square of «how far off on average») — and that root is exactly the length of the error bar. Root all three steps:

(0.14 × 0.14 = 0.0196 ≈ 0.02, 0.17 × 0.17 = 0.0289 ≈ 0.03, 0.22 × 0.22 = 0.0484 ≈ 0.05 — multiply back and it checks out.)

Now draw those lengths straight onto the path:

(green bar = along-track error ±√Var(x), orange bar = sideways error ±√Var(y), both drawn along the heading; ellipses and bars are plotted exactly from the hand-computed numbers)

Read these three lines against the picture and everything lines up:

• 👈 Along-track (x): variance 0.01→0.02→0.03, radius 0.10→0.14→0.17. Grows honestly and linearly — it only eats the wheel encoder's helping, nothing to do with θ.
• 👈 Sideways (y): variance 0→0.01→0.05, radius 0→0.10→0.22. Starts at zero and accelerates — it has no sensor of its own; every scrap of its uncertainty leaked in from IMU yaw.
• 👈 The y–θ link: 0→0.01→0.03, grown from nothing and welding tighter. It is never drawn on the map, yet it is the engine behind the sideways blow-up: every step y must swallow another 2 × Cov(y,θ).

Why is the ellipse tilted? Because the error bars are drawn along the heading — the road bends, the nose turns, the ellipse turns with it. Book it in another frame and the shape does not change one bit:

Same blob of error, two ways of writing it down.

3. Six Numbers No Longer Fit: Enter the P Matrix

Roll call: we have been tracking 6 numbers all along

Look back at those three steps — this is what each one had to update:

• 👈 Three «how spread am I» (variances): Var(x), Var(y), Var(θ)
• 👈 Three «how are we tied together» (covariances): Cov(x,y), Cov(x,θ), Cov(y,θ)

Six numbers in total — none optional, and no more needed. But line them up as a flat list and you hit a snag immediately.

Hold on: that annoying «2×» is what forces a square matrix

The hand computation works, but does something feel off? Stare at this term:

Where does that 2 come from? Look back at the expansion: the middle terms are δy·δθ and δθ·δy — the same number occupying two slots, so it gets counted twice.

Here is the snag: store the 6 numbers as a flat list and Cov(y,θ) gets exactly one slot, so you have to remember by hand «multiply this one by 2». With more states, that special case will bite you.

Six numbers, spread across 3×3 = 9 cells: variances on the diagonal, links symmetrically off it. That is the P matrix of the EKF — the 3 extra cells are not waste, they are redundancy bought to kill a special case:

The symmetry is not decoration: «y links θ» and «θ links y» are the same number, it must be counted twice, therefore it must be seated twice. The square matrix is the consequence of that need.

Induction: those 6 rules are really one rule

Lay out the rules we used while hand-computing — one per cell:

The six look completely different — some carry a 2×, one has a single term. But look at how they were produced and they are all the same move:

Check two of them at random (a recipe is just one of the three carry-forward rules):

Word for word what the table says. Which means: those 6 rules are not 6 facts to memorise — they are one move run 6 times.

And what is «the recipe for new δi»? It is simply row i of the carry-forward rules.

Aside: why those three rows of coefficients are called a «Jacobian»

Remember how we obtained those coefficients — «θ is off by a hair, how much does the output follow?» That is a slope.

Why must it be a slope? Because the motion equations contain sin and cos, which are curved; a curve has no single fixed conversion factor. But as long as the error is small, the slope of the tangent at the current point works as the conversion ratio:

That 1 and that 0 in the table came from exactly this:

Arrange every «which input → which output» slope into a square table and mathematics calls it the Jacobian matrix — in plain words, a full table of error conversion rates. It changes with the current θ and d, so it has to be recomputed every step.

Stack those three rows into a square and you have our F:

Write that move out for all 9 cells at once and the compact notation is exactly F P Fᵀ; add this step's Q and the 6 rules are bound into a single line:

Why multiply by F on the left and Fᵀ again on the right?

Because every cell of P holds a product of two errors — e.g. Cov(y,θ) = avg( δy × δθ ). And a carry-forward rule can only carry one error at a time. Two errors means carrying twice:

It checks out: this cell uses exactly row 2 of F (which owns new δy) and row 3 of F (new δθ), and expanding gives the Cov(y,θ) + Var(θ) we computed by hand.

So why write Fᵀ on the right? Because matrix multiplication only eats one way: row × column:

Hands on: redo step 3 strictly by the matrix rules

Matrix multiplication has exactly one rule: the cell at row i, column j = row i of the left × column j of the right (multiply matching positions, then add them all). We need three ingredients:

Punch 1: F × P　(carry the first error)

Row 2 of F is 0 1 1, which reads «new δy = old δy + old δθ». By the rule, row 2 of the result is:

Row 1's 1 0 0 and row 3's 0 0 1 are easier still — they just copy the matching row of P. Stack the three:

Punch 2: × Fᵀ　(the second error must be carried too)

Column 2 of Fᵀ is just row 2 of F stood upright: (0, 1, 1). Compute row 2, column 2 — the Var(new y) cell:

Sweep all nine cells the same way:

Punch 3: + Q　(the two fresh helpings from this step's readings)

Check the answers

cell	hand-computed in section 2	what the matrix just produced
Var(x)	0.03	0.03	✓
Var(y)	0.05	0.05	✓
Var(θ)	0.03	0.03	✓
Cov(y,θ)	0.03	0.03	✓

That is the beauty of matrices

	the 6 hand-written rules	this one-line formula
how many rules to write	6, every one different	1 line
that 2×	you must remember it	automatic
state grows from 3 to 10	re-derive 55 rules	not a character changes
switch to a drone or a robot arm	start over from scratch	only the numbers in F and Q

The matrix does not do one penny of extra arithmetic for you — it merely turns «a special case you must remember» into «something the cells do by themselves».

4. The Whole Secret of Fusion: Weighted Average and the Gain K

P has already worked out «how unsure I am». Now GPS arrives, carrying a variance of its own. Who do we believe? And by how much? Let us master it on a single number x first — everything later is just arming it for multiple dimensions.

Two claims, one position

Drive in a straight line for a while and watch only the east-west coordinate x. At one instant we hold two claims:

The lazy answer is to split the difference at 12. But GPS is clearly the more confident one right now, so the answer ought to lean toward GPS. Lean by how much? We do not guess — we try every possible blend and see which one comes out with the smallest variance. Smallest variance = most trustworthy; that was the rule we set in section 1.

The optimal blend, found by trying

Let the blend be: listen to dead reckoning w of the way, to GPS 1−w. The result is w × 10 + (1−w) × 14. What is its variance? Two rules, both flowing from «variance is the square of the error»:

Rule 1 · scaling	multiply an error by w and its variance is multiplied by w²　— halve the error and the square shrinks to a quarter
Rule 2 · adding	stack two unrelated errors and the variances simply add　— the dead-reckoning error and the GPS error sometimes agree and sometimes oppose, so they partly cancel; that is why σ does not add

Pure arithmetic. List every blend:

Verify the w = 0.2 cell: 0.2²×4 + 0.8²×1 = 0.16 + 0.64 = 0.8 ✓

Plot the variance of every blend and you get a bowl-shaped parabola. The famous «optimal estimate» involves no magic whatsoever — it is the bottom of the bowl. Three things hide in that little table:

(1) The bottom sits at w=0.2	listen to dead reckoning 20%, GPS 80%. Not all-in on GPS — the w=0 cell reads 1.00, so mixing in a little dead reckoning is actually better.
② 0.80 < 1.00	the blend is more accurate than the better witness alone. Fusion never loses — that is the entire reason sensor fusion exists.
③ 0.2 = 1 ÷ (4+1)	you never have to search for the best weight; there is a formula: each vote is inversely proportional to its own variance. Variance is your rap sheet — the bigger it is, the less say you get.

Rewrite it, and K is born

The blend is 0.2×10 + 0.8×14 = 13.2. The very same expression can be rewritten as «start from dead reckoning, then step toward GPS»:

That K is the Kalman gain. It reads in plain speech as «how far to step toward GPS». The numerator is my own uncertainty — the less sure I am, the harder I step:

prediction variance	K	behaviour
huge (totally unsure)	→ 1	big step toward GPS
tiny (very sure)	→ 0	GPS only gets to nudge
equally bad on both sides	= 0.5	listen half to each

The variance after fusing also has a one-shot formula (and it matches the bottom of the bowl, 0.8):

Draw each claim as a hill of possibility: the peak is the number it reports, the wider the hill the less sure it is. Blue is the fused result — it always stands between the two hills, leaning toward the thinner one, and it is thinner than either. Make dead reckoning terrible and watch K race toward 1; make both equal and K parks at 0.5.

Deep dive: why does the bottom of the bowl land exactly on K? Two hills multiplied

K is no accident of enumeration; it is what you get when two hills are multiplied. «Satisfy both claims at once» (be near the prediction AND near GPS) is mathematically the product of two bells — only places that are high on both survive.

A bell hill is exp(−deviation² / 2σ²): the further from the peak the lower, and it drops off by the square, so very fast. Multiply two of them and the exponents add:

− (x−10)² / (2×4) 　−　 (x−14)² / (2×1)

Two downward parabolas added give another parabola → the product is still a bell.

This is the root of why Kalman can run forever: Gaussian × Gaussian = Gaussian. The shape is closed, so each step only has to carry two numbers «peak and width» forward and never the whole curve. And that is exactly why Kalman demands Gaussian noise — change the shape and the product stops closing.

Where does the new peak land? Adding two parabolas puts the vertex at the average weighted by sharpness, where sharpness = 1/variance (the sharper, the more certain):

new peak = ( 10×(1/4) + 14×(1/1) ) ÷ ( 1/4 + 1/1 ) = 16.5 ÷ 1.25 = 13.2　← word for word the K-weighted answer above
new width = 1/σ′² = 1/4 + 1/1 = 1.25 　→　 σ′² = 0.8　← exactly (1−K)×4

«Weighted by 1/variance» in plain words is the bottom cell of that bowl. The bottom was never fitted — it is the peak of two Gaussian beliefs multiplied, not a hair more or less.

And that is the Kalman filter

Give the two claims their formal names:

From one number to three: how those 5 lines grew

First be clear about what GPS actually hands you — 2 coordinates plus the variance of each; about θ it says nothing at all. Our state has 3 numbers. The mismatch is the root of every complication:

(1) How to measure R: same trick as the wheel encoder

In section 1 we measured the encoder by jacking the car up and taking 10,000 readings. Copy that for GPS: park the car on a surveyed point, do not move, and let it report 3,000 times. Watch where the dots scatter.

Those two side histograms are the same bell from section 1. Measure this blob with section 2's rules (average of the squared error = variance; average of the product of two errors = covariance): Var(x) ≈ 0.05, Var(y) ≈ 0.05, Cov(x,y) ≈ 0 — and R is measured:

Both variances are now measured, not invented: Q (encoder + IMU) in section 1, R (GPS) just now. An EKF is not tuned, it is calibrated. And note — there is no θ dimension anywhere in this cloud; you cannot measure it, because GPS never reported it.

(2) Forget matrices first: fuse one number at a time

Ingredients: prediction [3.00, 0.20, 0.05], P from the books after step 3 [[0.03,0,0],[0,0.05,0.03],[0,0.03,0.03]], GPS reports [3.20, 0.10]. The one-dimensional formula from the last section is enough:

(3) Stack them: why K must be 3 rows by 2 columns

Get clear on K's job first — it turns «the GPS disagreement» into «a correction to the state»: correction = K × disagreement. How many numbers sit at each end is already fixed by reality:

numbers in	however many numbers GPS reports, that many disagreements → 2　⇒　K has 2 columns
numbers out	however many items the state has, that many to correct → 3　⇒　K has 3 rows

That is exactly how matrix multiplication counts: rows = how many numbers come out, columns = how many go in. So K can only be 3×2 — one cell more or less and the multiplication does not connect. Laid out:

3×2 = 6 pairings, and every pairing needs its own answer to «how much» — none can be skipped. Take row 3, column 2, the 0.3: the north-south disagreement hands 0.3 of itself to θ — GPS never measured θ, and this single cell is how θ gets corrected anyway.

The shape follows the two ends; the rule never changes:

If…	K becomes	why
GPS is replaced by an integrated unit that also reports heading (3 numbers)	3 rows × 3 cols	3 numbers come in
speed is added to the state (4 items)	4 rows × 2 cols	4 numbers come out
only the east-west GPS coordinate is used (1 number)	3 rows × 1 col	only 1 number comes in

All three cells boil down to one sentence: K at row i, column j = «the link between state item i and GPS reading j» ÷ «the total variance along GPS reading j».

Follow that sentence into P: the numerator wants «the link of every item with x and y» (pick columns → 3×2); the denominator wants «the x, y block itself + R» (pick rows and columns → 2×2). Both are the same move: keep only what GPS can see and throw away θ's row and column.

(4) Write that move down as a table, and it is H

H answers exactly one question: if the car really were in this state, what should GPS report?

what goes in each cell	move this state item by 1 and see how much the number GPS should report follows (that is a slope)
in our problem	move x by 1 m → the reading moves 1; move y → reading-x moves 0; turn θ → the reading moves 0 (spin on the spot, the position never moved)　⇒　H = [[1,0,0],[0,1,0]]
where it is used	H x translates the position ｜ H P Hᵀ translates the uncertainty (so it can be compared with R) ｜ P Hᵀ translates backwards, spreading the disagreement onto the state
why it never changes with time	«reading = x» is a straight line, so the slope is the same everywhere → a constant. The cos(θ) that F owns is curved, its slope moves with θ → recompute every step. That is the E in EKF: patch curved bits with a tangent. Here only F needs it.

Check it: P Hᵀ = the first two columns of P = [[0.03,0],[0,0.05],[0,0.03]] ✓ exactly the numerator (the 0.03 in the θ row is the channel it rides along); H P Hᵀ + R = [[0.03,0],[0,0.05]] + R = [[0.08,0],[0,0.10]] ✓ exactly the denominator.

(5) Look back: the 1-D K = P/(P+R) was an abbreviation

We kept reading it as «my variance ÷ total variance». That reading happens to work in one dimension and breaks the moment you add another. The real template is:

In one dimension the measured quantity is me, so both halves collapse onto P — which is the only reason it looks like P/(P+R):

	1-D	multi-D
what is measured	me, x, itself	the 2 numbers GPS reports
numerator = the link	Cov(x,x) = P	P Hᵀ　(3×2)
denominator = total variance of the measured	P + R	H P Hᵀ + R　(2×2)

So why does the numerator carry only one Hᵀ while the denominator carries an H on each side? Use the old rule from deriving F P Fᵀ in section 3: a cell holds two errors; whichever error needs translating to the GPS side gets one multiplication on that side.

what we want	the two errors in this cell	which needs translating	written as
denominator (variance of the reading itself)	reading error × reading error	both of them	H P Hᵀ
numerator (link between state and reading)	state error × reading error	only the right one (the left is already state)	P Hᵀ

Run our numbers through it and the 1-D flavour comes right back:

(6) Turn the division into an inverse, and K falls out

The denominator is now a 2×2 table, so you cannot simply divide. In 1-D, a ÷ b = a × (1/b), and 1/b is defined as «the number that multiplies up to 1». The matrix version copies that definition: the matrix that multiplies up to the identity I, written S⁻¹. For a diagonal matrix just invert each entry: S⁻¹ = [[12.5,0],[0,10]] (check: 0.08×12.5=1 ✓).

While we are here, check the sizes — they land exactly on the 3×2 counted out in (3): P Hᵀ = (3×3)(3×2) = 3×2; H P Hᵀ + R = (2×3)(3×3)(3×2) + (2×2) = 2×2, still 2×2 after inversion; multiply them, (3×2)(2×2) = 3×2 ✓

(7) Step across, then shrink P

The disagreement is written ν = z − H x (ν is the Greek letter nu — the symbol for «disagreement», not a velocity v; its unit is metres), so the correction is x ← x + K ν. For the variance shrink, the 1-D (1−K) becomes (I − K H) (K is 3×2, H is 2×3, so the product is 3×3, the same size as I):

	before	after	change
x	3.00	3.075	Var 0.03 → 0.01875（−37.5%）
y	0.20	0.15	Var 0.05 → 0.025（−50%）
θ	0.05	0.02	Var 0.03 → 0.021（−30%）

1-D → multi-D, word for word

1-D	multi-D	what changed
z − x	z − H x	first translate into what GPS should report
P + R	H P Hᵀ + R	P must be translated across too, before it can be added to R
P ÷ (P+R)	P Hᵀ (H P Hᵀ + R)⁻¹	division → inverse; Hᵀ spreads the disagreement back onto the state
(1 − K) P	(I − K H) P	1 becomes I, K becomes K H

Not one line of it is new knowledge — it is all «fuse one number at a time» plus «keep only what GPS can see». That is how the 5 textbook lines grew:

• [ PREDICT ] — dead reckoning, eyes shut
• 1. x_new = f(x, u) // accumulate the coordinates
• 2. P_new = F P Fᵀ + Q // old books carried and amplified, plus this step's fresh error
• [ UPDATE ] — correcting with eyes open
• 3. K = P Hᵀ (H P Hᵀ + R)⁻¹ // this is P/(P+R); multi-D just needs an inverse
• 4. x = x_new + K (z − H x_new) // step K of the way toward GPS
• 5. P = (I − K H) P_new // this is (1−K)×P; the variance shrinks

Symbol table: who is who, and where they come from

The GPS variance is R — it appears exactly once in those 5 lines, inside the bracket on line 3. Here is every symbol:

symbol	what it is	where it comes from	size
x	state: where the car is, where it points　(speed is not in here)	accumulated step by step from encoder + IMU	3 numbers
P	how unsure I am (links included)	rolled forward step by step in sections 2 and 3	3×3
Q	the helping encoder + IMU each get wrong this step	measured with the section-1 method, wheels jacked up	3×3
F	slope table: state → next state	slopes of the motion equations, recomputed every step	3×3
z	the coordinates GPS just reported	handed over by GPS	2 numbers
R	the GPS variance (how unsure it is)	measured by parking and logging thousands of fixes	2×2
H	slope table: state → GPS reading	slopes of the measurement equation; GPS is linear → constant	2×3
ν	the disagreement between GPS and me	computed on the spot: z − H x	2 numbers
K	how far to step toward GPS	computed on the spot from P and R	3×2

There is no speed in this table — the v read off the encoder and the w read off the IMU are inputs, used once each step and thrown away. They occupy no cell of P and take no part in K.

Only two things in the whole table are yours to declare in advance: Q (how bad the encoder + IMU are) and R (how bad GPS is). And neither is guessed — both were measured off the sensors. Every other symbol is computed.