The Kalman filter: how to estimate where a car is and how fast it goes from imprecise measurements

Combining two measurements and the Kalman gain, the filter as a running average, the state-space model, prediction and correction, tracking a car from GPS with an estimate of the unmeasured velocity, tuning the noises Q and R, optimality and nonlinear extensions.

14 minutes Matěj Žalmánek 2026-10-15

Series AI basics Part 41 of 52

Full series syllabus 52 of 83 topics published

What you will take away

How two imprecise measurements are combined and what the Kalman gain is.
How the filter alternates prediction from the model with correction from the measurement — and estimates even what nobody measures.
Why the setting of the noises Q and R matters so much and how the filter copes with a nonlinear world.

Useful to know first: Hidden Markov models: the Viterbi algorithm and the dishonest casino, Overview of probability distributions: Bernoulli, binomial, Poisson, uniform, normal and exponential

Apollo 11 flew to the Moon with an on-board computer that had less memory than today's graphing calculator. Its navigation nevertheless had to keep estimating, from imprecise measurements of stars, radar and inertial sensors, where the spacecraft was and how fast it was flying. It was helped by a method that Rudolf Kálmán published in 1960 and that Stanley Schmidt turned into practice for NASA. Today the Kalman filter is in every phone that combines GPS with an accelerometer, in every drone and in every car that keeps to its lane on its own. This part closes the module on probabilistic models.

It builds on the parts Hidden Markov models and Overview of probability distributions.

1. Two imprecise measurements are better than one

Let us start simply. We measure the distance to a lighthouse twice: once with a precise instrument — 100 m with an uncertainty (standard deviation) of 5 m — and once with a rough one — 110 m with an uncertainty of 10 m. What is the best estimate?

Theorem — combining two normal estimates

Let x ∈ ℝ be an unknown quantity, m₁, m₂ ∈ ℝ and σ₁², σ₂² > 0. Let the first estimate say x ~ N(m₁, σ₁²) (the prior distribution), and let the second estimate be an observation m₂ = x + ε, where the noise ε ~ N(0, σ₂²) is independent of x (for the normal distribution see the part Overview of probability distributions). Denote

K = σ₁²/σ₁² + σ₂² ∈ (0, 1).

Then the combined estimate — the conditional (posterior) distribution of x given the observation m₂ — is normal N(m, σ²) with

m = m₁ + K (m₂ − m₁), σ² = (1 − K) σ₁².

Equivalently 1/σ² = 1/σ₁² + 1/σ₂²: the precisions (reciprocal variances) add up.

Proof. By Bayes' theorem the posterior density is proportional to the product of the prior density and the likelihood, i.e. of the two normal densities. In the exponent, two quadratic functions of x are added:

−(x − m₁)²/2σ₁² − (x − m₂)²/2σ₂² = −1/2 (1/σ₁² + 1/σ₂²) x² + (m₁/σ₁² + m₂/σ₂²) x + const.

A density with the exponent −1/2 ax² + bx + const. (a > 0) is, after completing the square, normal with variance 1/a and mean b/a. Hence 1/σ² = 1/σ₁² + 1/σ₂², so

σ² = σ₁²σ₂²/σ₁² + σ₂² = (1 − K) σ₁², m = σ² (m₁/σ₁² + m₂/σ₂²) = (1 − K) m₁ + K m₂ = m₁ + K (m₂ − m₁). ∎

In plain words

Take the first estimate and move it towards the second — the more you trust the second, the further. The number K says how large a share of the difference to take over. The result is more precise than either measurement on its own; even a rough measurement adds information.

m = m₁ + K (m₂ − m₁), K = σ₁²/σ₁² + σ₂², σ² = (1 − K) σ₁²

m₁, σ₁²: the first estimate (blue in the figure) is the starting point — we move away from it; its variance in the numerator of K says: the less we trust the first estimate, the further from it we may move
m₂, σ₂²: the second estimate (amber); it enters the formula only through the difference m₂ − m₁ — if the two agreed, there would be nothing to correct. Its variance in the denominator of K makes the gain smaller: we trust a rough measurement little
K: the Kalman gain, a number between 0 and 1 — what share of the difference to take over. It is the share of the first estimate's uncertainty in the total uncertainty, so the more precise estimate “pulls” the result towards itself
m, σ²: the combined estimate (green); the factor 1 − K < 1 guarantees that the new uncertainty is smaller than the first one — every measurement, even a rough one, adds something

Move from the first estimate towards the second by a share given by which one you trust more.

Combining two distance measurements precise: 100 ± 5 mrough: 110 ± 10 mcombined: 102 ± 4.5 m

Here K = 25/25 + 100 = 0.2 (colours as in the formula above): we take over a fifth of the difference and get 102 m with an uncertainty of √20 ≈ 4.5 m — less than the 5 m of the precise instrument. This number K is the Kalman gain, and the whole filter is nothing more than this step, repeated over time.

Example — combining the measurements 100 ± 5 m and 110 ± 10 m step by step

Colours as in the breakdown of the formula above.

Variances: σ₁² = 5² = 25, σ₂² = 10² = 100.

Gain: K = 25/25 + 100 = 0.2.

Mean: m = 100 + 0.2 · (110 − 100) = 102 m — the same as the weighted average 0.8 · 100 + 0.2 · 110.

Variance: σ² = (1 − 0.2) · 25 = 20, σ = √20 ≈ 4.47 m.

Check via the precisions: 1/25 + 1/100 = 0.04 + 0.01 = 0.05 = 1/20. ✓

2. Measuring one quantity again and again

We measure the temperature in a room with a thermometer with an uncertainty of 2 °C, fifty times in a row, and after each measurement we combine the new reading with the estimate so far. After fifty measurements the estimate is 19.895 °C — exactly the arithmetic mean of all the measurements — and the uncertainty is 0.283 °C = 2/√50. The Kalman filter for a quantity that does not change is therefore an ordinary running average. It becomes interesting when the quantity changes over time.

Theorem — the filter for a constant quantity is a running average

Let σ² > 0 be the variance of the instrument and z₁, z₂, … ∈ ℝ measurements of a constant quantity. The filter starts with an estimate m₀ ∈ ℝ with variance v₀ > 0, and for k = 1, 2, … it combines the estimate so far N(m_k−1, v_k−1) with the measurement z_k by the previous theorem:

K_k = v_k−1/v_k−1 + σ², m_k = m_k−1 + K_k (z_k − m_k−1), v_k = (1 − K_k) v_k−1.

Then in the limit v₀ → ∞ (the initial estimate is so uncertain that the first measurement overrides it completely), for every k ≥ 1

K_k = 1/k, m_k = z₁ + … + z_k/k, v_k = σ²/k.

Proof by induction on k. For k = 1, K₁ = v₀/v₀ + σ² → 1, so m₁ = m₀ + K₁(z₁ − m₀) → z₁ and v₁ = v₀σ²/v₀ + σ² → σ². Let k ≥ 2 and let the statement hold for k − 1, i.e. v_k−1 = σ²/k − 1 and m_k−1 is the mean of z₁, …, z_k−1. Then

K_k = v_k−1/v_k−1 + σ² = 1/k − 1/1/k − 1 + 1 = 1/k, v_k = (1 − 1/k) · σ²/k − 1 = σ²/k,

m_k = m_k−1 + 1/k (z_k − m_k−1) = (k − 1) m_k−1 + z_k/k,

and that is the mean of all k measurements, because (k − 1) m_k−1 = z₁ + … + z_k−1. ∎

In plain words

In an average of ten numbers, the tenth measurement has a weight of one tenth — and that is exactly what the filter gives it too. The more we already know, the less a single new measurement moves us. The uncertainty falls as 1/√k: to halve the error you need four times as many measurements.

Example — the first three temperature measurements

Our filter starts with an estimate of 0 °C with variance 10⁶ (an uncertainty of 1,000 °C — “I know nothing”); the measurement variance is 2² = 4.

Each step is the combination of two estimates from the first section: the estimate so far plays the role of the first, the measurement that of the second (colours as in the breakdown of the combination formula above).

1st measurement 23.098 °C: K₁ = 10⁶/10⁶ + 4 ≈ 1, estimate 23.098 °C, variance 4.00.

2nd measurement 21.967 °C: K₂ = 4/4 + 4 = 1/2, estimate 23.098 + 1/2 · (21.967 − 23.098) = 22.533 °C, variance 2.00.

3rd measurement 16.630 °C: K₃ = 2/2 + 4 = 1/3, estimate 22.533 + 1/3 · (16.630 − 22.533) = 20.565 °C, variance 1.33 = 4/3.

Check: 23.098 + 21.967 + 16.630/3 = 20.565 °C — an ordinary average. After fifty measurements the variance is 4/50 = 0.08 and the uncertainty √0.08 ≈ 0.283 °C.

3. The state-space model

Definition — linear state-space model

Let n, m ≥ 1 be the dimensions of the state and of the measurement, and let

F ∈ ℝ^n×n be the state transition matrix,
H ∈ ℝ^m×n the measurement matrix,
Q ∈ ℝ^n×n a symmetric positive semidefinite matrix — the covariance of the process noise,
R ∈ ℝ^m×m a symmetric positive definite matrix — the covariance of the measurement noise,
x̂₀ ∈ ℝⁿ and a symmetric positive semidefinite P₀ ∈ ℝ^n×n describe the initial state x₀ ~ N(x̂₀, P₀).

A linear state-space model is a sequence of random vectors — hidden states x_k ∈ ℝⁿ and measurements z_k ∈ ℝ^m, k = 1, 2, … — for which

x_k = F x_k−1 + w_k, w_k ~ N(0, Q), z_k = H x_k + v_k, v_k ~ N(0, R),

where x₀, w₁, w₂, …, v₁, v₂, … are mutually independent. (The multivariate normal distribution N(μ, Σ) and the covariance matrix are defined below.) So the state evolves according to F with random bumps w_k, and the measurement captures it through H with noise v_k.

In plain words

It is the hidden Markov model from the previous part, except that the state is not “a fair or a loaded die” but a vector of numbers — say, position and velocity. Physics says roughly how the state evolves (F); random bumps and the model's inaccuracy are summed up by Q. The instrument sees only part of the state (H) and is off by R.

Our example: a car drives along a road at a roughly constant speed of 15 m/s, but the driver now and then brakes a little or speeds up — a random acceleration with a standard deviation of 0.5 m/s². The state is (position, velocity), the step is 1 s, so F = [[1, 1], [0, 1]]: the new position is the old one plus the velocity. GPS measures only the position, with an uncertainty of 8 m: H = [1, 0], R = 64.

Definition — covariance matrix and the multivariate normal distribution

Let n ≥ 1 and let X = (X₁, …, X_n)^T be a random vector whose components are random variables with finite variance. Its mean is the vector μ = E[X] = (E[X₁], …, E[X_n])^T ∈ ℝⁿ (for the mean and covariance see the part on expected value, variance and covariance). Its covariance matrix is Σ ∈ ℝ^n×n,

Σ = E[(X − μ)(X − μ)^T], i.e. Σ_ij = cov(X_i, X_j) = E[(X_i − μ_i)(X_j − μ_j)] for i, j = 1, …, n.

The diagonal holds the variances of the components (Σ_ii = var X_i), the off-diagonal entries the covariances. The matrix is symmetric (Σ = Σ^T) and positive semidefinite: for every a ∈ ℝⁿ, a^TΣa = var(a^TX) ≥ 0.

Let μ ∈ ℝⁿ and let Σ ∈ ℝ^n×n be a symmetric positive definite matrix. A random vector X has the multivariate normal distribution, written X ~ N(μ, Σ), if and only if it has the density

f(x) = 1/√(2π)ⁿ det Σ exp(−1/2 (x − μ)^T Σ⁻¹ (x − μ)), x ∈ ℝⁿ.

Then E[X] = μ and the covariance matrix of X is Σ. This part uses two properties:

If X ~ N(μ, Σ), A ∈ ℝ^p×n and c ∈ ℝ^p, then AX + c ~ N(Aμ + c, A Σ A^T) (if AΣA^T is singular, it is a so-called degenerate normal distribution without a density).
If X ~ N(μ₁, Σ₁) and Y ~ N(μ₂, Σ₂) are independent vectors in ℝⁿ, then X + Y ~ N(μ₁ + μ₂, Σ₁ + Σ₂).

In plain words

A covariance matrix is “uncertainty for several numbers at once”. The diagonal says how unsure we are about the position and how unsure about the velocity. Off the diagonal is whether the errors go together: a positive covariance of position and velocity means “if we placed the car too far ahead, we probably also gave it too high a speed”.

Example — where the matrix Q comes from

During a step of length Δt, a constant random acceleration a ~ N(0, q²) acts on the car. From the formulas for uniformly accelerated motion:

p_k = p_k−1 + Δt · v_k−1 + ½ Δt² a, v_k = v_k−1 + Δt · a.

Without noise this is F = [[1, Δt], [0, 1]]. The noise is w = g a with the vector g = (½ Δt², Δt), and by the rule AΣA^T above

Q = q² g g^T = q² [[Δt⁴/4, Δt³/2], [Δt³/2, Δt²]].

For Δt = 1 s and q = 0.5 m/s²: Q = 0.25 · [[0.25, 0.5], [0.5, 1]] = [[0.0625, 0.125], [0.125, 0.25]]. So in one second the random acceleration adds an uncertainty of √0.0625 = 0.25 m in position and 0.5 m/s in velocity — and both errors have the same sign, which is why the covariance is positive. This is exactly the matrix in the code below.

4. Prediction and correction

Algorithm — the Kalman filter

Input: the matrices F, H, Q, R of a linear state-space model (definition above), an initial estimate x̂₀ ∈ ℝⁿ with covariance P₀ ∈ ℝ^n×n (symmetric, positive semidefinite) and measurements z₁, …, z_T ∈ ℝ^m.

Output: for every k = 1, …, T an estimate x̂_k ∈ ℝⁿ and its covariance P_k ∈ ℝ^n×n — the mean and covariance of the normal distribution with which the filter describes the state x_k after the measurements z₁, …, z_k. I is the n × n identity matrix.

Steps: for k = 1, 2, …, T in turn:

Prediction: x̂_k⁻ = F x̂_k−1, P_k⁻ = F P_k−1 F^T + Q.
Innovation and its covariance: y_k = z_k − H x̂_k⁻, S_k = H P_k⁻ H^T + R (it is positive definite because R is, and therefore invertible).
Gain: K_k = P_k⁻ H^T S_k⁻¹ ∈ ℝ^n×m.
Correction: x̂_k = x̂_k⁻ + K_k y_k, P_k = (I − K_k H) P_k⁻.

The algorithm stops after processing the last measurement z_T (when running online, it continues with every new measurement). In the breakdowns below we drop the index k: x̂, P on the right are the values from the previous step, those on the left the new ones.

In plain words

Prediction: “according to physics, the car should now be here” — and because physics is not perfect, the uncertainty grows a little. Correction: a measurement arrives; the difference between the measurement and the prediction (the innovation) is taken over only partly, by K, depending on which we trust more. It is the combination of two measurements from the first section — one “measurement” is the prediction from last time, the other is GPS. On top of that, the covariance P remembers how the position error and the velocity error are related, and thanks to that the filter also corrects the velocity, which nobody measures.

The colours in the breakdowns below match the figures: prediction (formerly “first estimate”) in blue, measurement in amber, result in green, gain in purple.

x̂⁻ = F x̂, P⁻ = F P F^T + Q

x̂⁻, P⁻: the prediction for this step, before the measurement arrives — it plays the role of the “first estimate” from combining two measurements
F … F^T: the model moves the estimate forward; the uncertainty is carried over by the rule AΣA^T (for the car: an uncertain velocity turns into uncertainty in position within a second). FP is not enough, because covariance is a quadratic quantity — it is multiplied from the left and from the right
Q: the model is not perfect (the driver brakes a little), so uncertainty is added in every step — without Q, P would drop to zero and the filter would stop listening to the measurements

Prediction: where physics says the state will get to, and how much the uncertainty grows on the way.

K = P⁻ H^T (H P⁻ H^T + R)⁻¹

H P⁻ H^T: the uncertainty of the prediction translated into the instrument's terms (for the car, just the variance of the position) — it corresponds to σ₁²
R: the measurement variance, corresponding to σ₂²; the sum in the brackets is the innovation variance S — how far the measurement and the prediction may differ purely by chance
P⁻ H^T: the covariance of the whole state with the measured quantity; thanks to it, even the velocity, which nobody measures, gets a non-zero gain — the position error and the velocity error are related
K: the Kalman gain, one number for each component of the state; an inverse instead of a division only because the measurement can be a vector

In one dimension with H = 1, P⁻H^T = HP⁻H^T = σ₁² and the formula is exactly K = σ₁²/σ₁² + σ₂² from the first section.

x̂ = x̂⁻ + K (z − H x̂⁻), P = (I − K H) P⁻

x̂⁻, P⁻: the prediction — the starting point we move away from
z: the new measurement (the amber GPS points in the figure); the only place where data enter the estimate
H: converts the predicted state into what the instrument should show; the measurement and the state cannot be subtracted directly, because GPS sees only the position, not the velocity
K: what share of the innovation z − Hx̂⁻ to take over into each component of the state
x̂, P: the corrected estimate (green in the figure); the factor I − KH reduces the uncertainty — the more, the larger the share of the measurement we took over

Correction: the same “move towards the measurement by the share K”, only for vectors.

import numpy as np

F = np.array([[1, 1], [0, 1]])                    # position += velocity
H = np.array([[1, 0]])                            # we measure only the position
Q = 0.5**2 * np.array([[1/4, 1/2], [1/2, 1]])     # random acceleration
R = np.array([[8**2]])                            # GPS ± 8 m

def kalman(mereni, x, P):
    for z in mereni:
        x, P = F @ x, F @ P @ F.T + Q             # prediction
        K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
        x = x + K @ (z - H @ x)                   # correction by the innovation
        P = (np.eye(2) - K @ H) @ P
        yield x, P

Example — the first two steps of the filter on the car data

Start. The first GPS measurement is z₁ = 8.44 m. The filter starts with position 8.44 m, velocity 0 and a large uncertainty P₀ = [[100, 0], [0, 100]] (10 m and 10 m/s). The correction with the same measurement does not change the position (the innovation is 0), it only reduces its variance (colours as in the breakdowns above): K = 100/100 + 64 = 0.610, P₁₁ = (1 − 0.610) · 100 = 39.02.

Prediction for second 2. x̂⁻ = (8.44 + 0, 0) = (8.44, 0.00). The covariance FPF^T adds the velocity variance to the position variance (position += velocity), and Q is added on top:

P⁻ = [[39.02 + 100 + 0.0625, 100 + 0.125], [100 + 0.125, 100 + 0.25]] = [[139.09, 100.13], [100.13, 100.25]].

Correction. Measurement z₂ = 23.23 m, innovation y = 23.23 − 8.44 = 14.78 m, its variance S = 139.09 + 64 = 203.09.

K = (139.09, 100.13)/203.09 = (0.685, 0.493),

x̂ = (8.44 + 0.685 · 14.78, 0 + 0.493 · 14.78) = (18.57 m, 7.29 m/s).

After two measurements the filter “knows” that the car is moving, even though nobody measured the velocity: it estimated 7.3 m/s (true value 13.9 m/s). The gain for the velocity is non-zero only thanks to the off-diagonal covariance 100.13. Why only half? The measurements have 8 m of noise, so the filter does not yet take the 15 m difference fully seriously — over the next seconds the velocity settles in.

5. The filter tracks a car

A minute of driving, one GPS measurement per second. In that time the car covers just under a kilometre, so the noise would get lost in a plot of the position. We therefore plot how far the car is ahead of or behind a drive at exactly 15 m/s:

Tracking a car: position deviation at the top, velocity at the bottom true value GPS measurement filter estimate ± 2σ

The GPS measurements deviate from the true position by 8.3 m on average (from the tenth second, once the filter has warmed up). The filter's estimate by only 4.9 m — 41 % less. And the velocity, which no sensor measures, it estimates with an error of 0.8 m/s: the filter infers it from how the position changes. At the start it does not know the velocity (it starts from zero), but within a few seconds it catches on. The ±2σ band is the filter's honest uncertainty — the true position lies inside it in 95 % of the steps, exactly as it should.

Definition — average error (RMSE) and the ±2σ band

Let 𝒦 be a finite non-empty set of steps, N = |𝒦|, and for every k ∈ 𝒦 let p_k ∈ ℝ be the true position and p̂_k ∈ ℝ its estimate (the first component of the filter's estimate x̂_k, or the measurement itself). The root mean square error (RMSE) is the number

RMSE = √1/N ∑_{k ∈ 𝒦} (p̂_k − p_k)².

The “average error” in this part is the RMSE over the steps 𝒦 = {10, …, 59}, so N = 50. Squares penalize large errors more than small ones, and the square root brings the result back to metres. For GPS alone, the RMSE should come out around σ = 8 m — and it came out at 8.3 m. For the velocity the RMSE is defined the same way with the second component of the state.

Let P_k ∈ ℝ^2×2 be the covariance of the filter's estimate at step k and P_k,11 its top-left entry (the variance of the position). The ±2σ band at step k is the interval

[p̂_k − 2√P_k,11, p̂_k + 2√P_k,11].

For X ~ N(μ, σ²), P(|X − μ| ≤ 2σ) ≈ 0.9545; if the filter has computed its uncertainty P_k correctly, the true value must fall inside the band in about 95 % of the steps.

After a few steps the Kalman gain settles at K ≈ (0.30, 0.05): from each innovation the filter takes 30 % into the position and 5 % (0.05 m/s per metre of innovation) into the velocity. Over 50 simulated drives, the filter's error is on average 55 % of the error of GPS alone.

Theorem — the steady-state gain

The formulas for P⁻, K and P contain no measurements z — the covariance and the gain can be computed in advance. Combining prediction and correction gives a recurrence (the discrete Riccati equation)

P⁻_k+1 = F (P⁻_k − P⁻_kH^T (HP⁻_kH^T + R)⁻¹ HP⁻_k) F^T + Q.

Let F ∈ ℝ^n×n, H ∈ ℝ^m×n, a positive definite R and Q = GG^T for some G ∈ ℝ^n×r be fixed (independent of k), and let

the pair (F, H) be observable: the matrix formed by stacking H, HF, …, HFⁿ⁻¹ one below another has rank n (the whole state can be determined from a series of measurements);
the pair (F, G) be controllable: the matrix formed by placing G, FG, …, Fⁿ⁻¹G side by side has rank n (over time the noise reaches all components of the state).

Then for every symmetric positive semidefinite initial matrix the sequence P⁻_k converges to a matrix P_∞ that does not depend on the initial matrix and is the only symmetric positive semidefinite fixed point of the recurrence; the gain K_k converges to the steady-state value K_∞ = P_∞H^T (HP_∞H^T + R)⁻¹. For the car both conditions hold: [H; HF] = [[1, 0], [1, 1]] and [g, Fg] = [[0.5, 1.5], [1, 1]] both have rank 2.

For the car: after 60 steps K = (0.297, 0.052) and P = [[19.04, 3.35], [3.35, 1.29]], so the filter trusts itself to within √19.04 ≈ 4.4 m in position and √1.29 ≈ 1.1 m/s in velocity.

In plain words

At the start the filter knows almost nothing and gulps down every measurement (gain 0.61, then 0.68). As the estimate becomes more precise, the gain falls — until it settles where the uncertainty added per step by the noise Q is as large as the uncertainty the measurement takes away. From then on nothing changes, which is why simple devices often compute the gain once in advance and the filter then just multiplies by fixed numbers.

6. Whom to trust: Q and R

The filter needs to know Q and R. The GPS manufacturer usually states its accuracy, but the process noise is harder to estimate. What if we give the filter different values from those in the data?

Process noise the filter believes in (m/s²)	0.01	0.1	0.5 (true)	2	10
position error (m)	9.29	5.88	4.86	5.67	7.08
velocity error (m/s)	1.19	0.86	0.79	1.53	5.19

When the filter believes that the car is driving at an almost perfectly constant speed (small Q), it sticks to its model and reacts late to real acceleration — the position error is even larger than with GPS alone. When, on the other hand, it expects wild changes (large Q), it does not trust the model and copies the noisy measurements. It does best with the true value of 0.5. The same holds for R: with a GPS uncertainty of 2, 8 and 30 m, the position error comes out at 5.66, 4.86 and 5.39 m.

7. Why exactly like this

Theorem — optimality of the Kalman filter

Let x_k, z_k satisfy the linear state-space model (definition above) with initial state x₀ ~ N(x̂₀, P₀), and let x̂_k, P_k be the outputs of the Kalman filter run with the same F, H, Q, R, x̂₀, P₀. Then for every k ≥ 1:

the conditional (posterior) distribution of the state x_k given the measurements z₁, …, z_k is exactly N(x̂_k, P_k);
in particular x̂_k = E[x_k | z₁, …, z_k], and of all estimates g(z₁, …, z_k) it has the smallest mean squared error E[‖x_k − g(z₁, …, z_k)‖²].

If we drop the normality assumption (x₀, w_k, v_k are only uncorrelated, with the same means and covariances), x̂_k is the best in the sense of mean squared error among the estimates that are an affine (linear plus a constant) function of z₁, …, z_k.

In plain words

The Kalman filter is Bayes' theorem applied again and again. The prediction is the prior distribution, the measurement is the likelihood, the correction is the posterior distribution. The normal distribution has the rare property that a product of normals is again normal — so it is enough to remember the mean and the covariance instead of a whole function. It is the forward algorithm from hidden Markov models for a continuous state.

p(x_k | z_1:k) ∝ p(z_k | x_k) · ∫ p(x_k | x_k−1) p(x_k−1 | z_1:k−1) dx_k−1

∫ p(x_k | x_k−1) p(x_k−1 | z_1:k−1) dx_k−1: the prediction (prior distribution): we do not know the previous state exactly, so we average the transition over all possible previous states, weighted by what we believed last time. The integral is the continuous counterpart of the sum ∑_i in the HMM forward algorithm. For a linear model and a normal distribution it comes out as N(Fx̂, FPF^T + Q)
p(z_k | x_k): the likelihood of the new measurement — how well each place would explain what the GPS showed; from the model it is N(z_k; Hx_k, R)
p(x_k | z_1:k): the posterior distribution after all the measurements; “∝” because we do not need the normalizing constant p(z_k | z_1:k−1) — the product of the two normal densities is worked out by the proof of the theorem at the start of this part, and it yields exactly the correction step

Bayes' theorem over time: prediction times likelihood gives a new estimate, which is the prediction for the next step.

8. When the world is not linear

The extended Kalman filter (EKF) linearizes a nonlinear model in every step using derivatives — this is how the Apollo navigation did it too.
The unscented Kalman filter (UKF), instead of derivatives, pushes a few carefully chosen points through the model and computes the new mean and covariance from them.
The particle filter gives up the normal distribution entirely: it represents the estimate by thousands of samples (“particles”) that move according to the model and are reweighted according to the measurements. It can handle an estimate with several peaks, too, such as “the robot is either in the kitchen or in the living room”.
Where it is used: fusing GPS, accelerometer and gyroscope in phones, navigation of aircraft, drones and rockets, tracking objects by radar, localizing autonomous cars, and even smoothing economic time series.

Definition — the extended Kalman filter (EKF)

Let f : ℝⁿ → ℝⁿ and h : ℝⁿ → ℝ^m be differentiable functions (for derivatives see the part on derivatives, the gradient and the chain rule), Q, R as in the linear model, and let the model be nonlinear:

x_k = f(x_k−1) + w_k, z_k = h(x_k) + v_k, w_k ~ N(0, Q), v_k ~ N(0, R), all noises independent.

The Jacobian matrix of the function f at a point a ∈ ℝⁿ is the matrix of partial derivatives J_f(a) ∈ ℝ^n×n, (J_f(a))_ij = ∂f_i/∂x_j(a); similarly J_h(a) ∈ ℝ^m×n. The extended Kalman filter has the same input and output as the Kalman filter (with f, h in place of F, H) and for k = 1, 2, …, T performs:

Prediction: F_k = J_f(x̂_k−1), x̂_k⁻ = f(x̂_k−1), P_k⁻ = F_k P_k−1 F_k^T + Q.
Gain: H_k = J_h(x̂_k⁻), K_k = P_k⁻ H_k^T (H_k P_k⁻ H_k^T + R)⁻¹.
Correction: x̂_k = x̂_k⁻ + K_k (z_k − h(x̂_k⁻)), P_k = (I − K_k H_k) P_k⁻.

So the EKF computes the mean with the nonlinear functions and the covariance with their linearization at the current estimate.

Example: a radar measures the distance h(p_x, p_y) = √p_x² + p_y²; then H_k = (p_x/d, p_y/d) with d = √p_x² + p_y² — a unit vector pointing towards the target. For a target at (300, 400) m, d = 500 m and H_k = (0.6, 0.8). The result is no longer the exact posterior distribution, only an approximation of it; the more curved the model is within the range of the uncertainty, the worse the approximation.

9. Summary

Two independent normal measurements are combined by a weighted average; the precisions add up, and the weight K is the Kalman gain.
The Kalman filter repeats a prediction from the model and a correction from the measurement; for a constant quantity it is a running average.
For the car it reduced the position error from 8.3 m (GPS) to 4.9 m and also estimated the unmeasured velocity with an error of 0.8 m/s; the ±2σ band covered the true value in 95 % of the steps.
The result depends on Q and R: too much trust in the model reacts late, too little copies the noise.
For a linear model with normal noise, the filter is an exact Bayesian update; the nonlinear world is handled by the EKF, the UKF and particle filters.

This part closes the module on probabilistic models. Want to get a feel for it? Have an AI agent write a Kalman filter for the accelerometer data from your own phone, and see how well it estimates how far you moved the phone.

Do you see it differently? Write to support@coden-app.com.

The Kalman filter: how to estimate where a car is and how fast it goes from imprecise measurements

1. Two imprecise measurements are better than one

2. Measuring one quantity again and again

3. The state-space model

4. Prediction and correction

5. The filter tracks a car

6. Whom to trust: Q and R

7. Why exactly like this

8. When the world is not linear

9. Summary

An Agent on your computer, driven from your phone