Where Am I? — The Kalman Filter, EKF, and UKF

Chapter 1: Belief & the Bayes Filter

The single most important shift in this lesson is from storing a guess to storing a belief. A guess is one number. A belief is a whole probability distribution over where the robot might be — it encodes both the best estimate and the uncertainty around it. Stanford writes the belief over the state at time t as:

Read that slowly. x_t is the state at time t — for a wheeled robot, the pose (x, y, θ). z_1:t is every measurement we've gathered from step 1 up to now. u_1:t is every control command we've issued. So bel(x_t) is "the probability distribution over the state, given everything we've ever sensed and every move we've made." That conditional distribution is exactly the robot's internal knowledge of where it is.

Two beliefs: prediction and corrected

The Bayes filter juggles two distributions. After a control command but before seeing the new measurement, we have the prediction belief (Stanford draws it with a bar):

The bar means "before the latest measurement." It folds in the newest control u_t but not the newest measurement z_t. After we incorporate z_t, we get the corrected belief bel(x_t) (no bar). The whole filter is just a two-beat loop turning one into the other, forever:

The two equations

The prediction step uses the law of total probability — sum over every place the robot could have been, weighted by how likely the motion model says it would land at x_t:

Here p(x_t | u_t, x_t−1) is the state-transition model: "if I was at x_t−1 and commanded u_t, where do I end up?" The update step uses Bayes' rule — multiply the prediction by how likely the measurement would be at each candidate state, then renormalize:

Here p(z_t | x_t) is the measurement model: "if I were truly at x_t, how likely is this reading z_t?" and η (eta) is just a normalizer that forces the result back into a valid probability distribution (it's the denominator of Bayes' rule). That's it. Predict with the motion model; correct with the measurement model. Every filter in this lesson is a special case of these two lines.

Watch one full predict→update cycle on a discretized belief (a histogram over a hallway). Press Predict: the bar chart smears rightward and flattens (motion adds uncertainty). Press Update: a measurement sharpens it back toward a peak. This is the Bayes filter, made of bars.

The discrete Bayes filter, by hand

To make those two equations feel concrete (and to prove the histogram sim isn't smoke), let's run one cycle of the discrete Bayes filter — the version where the state space is a finite list of cells, so the integral becomes a sum. A robot lives in a 3-cell hallway, cells A, B, C. Its prior belief is bel = [0.5, 0.3, 0.2] (probably in A). We command "move right one cell," but the motion is noisy: 80% of the time it lands one cell right, 20% it stalls in place.

Predict (law of total probability — sum over where it came from). For each destination cell, add up every source cell's probability times the chance the motion took it there:

• P(A) = 0.5·0.2 (stalled in A) = 0.10
• P(B) = 0.5·0.8 (A→B) + 0.3·0.2 (stalled in B) = 0.40 + 0.06 = 0.46
• P(C) = 0.3·0.8 (B→C) + 0.2·1.0 (stuck at the right wall) = 0.24 + 0.20 = 0.44

So bel̅ = [0.10, 0.46, 0.44]. The belief shifted right and flattened — it used to peak hard at A (0.5); now it's spread across B and C. Moving lost knowledge, exactly as promised.

Update (Bayes' rule — multiply by the measurement likelihood, renormalize). The robot's sensor says "I see a door," and doors are at B (likelihood 0.6) but rare elsewhere (A: 0.1, C: 0.3). Multiply elementwise:

• A: 0.10·0.1 = 0.010
• B: 0.46·0.6 = 0.276
• C: 0.44·0.3 = 0.132

These don't sum to 1, so normalize by η = 1/(0.010+0.276+0.132) = 1/0.418 = 2.39: bel = [0.024, 0.660, 0.316]. The "door" measurement pulled the belief sharply toward B (0.66). Sensing gained knowledge — the belief re-sharpened. That's one full predict→update cycle, with nothing but sums and products. The continuous Kalman filter is this exact dance, but with Gaussians instead of bar charts.

The catch that motivates everything else

That hand calculation worked because there were only 3 cells. For a continuous robot pose, the belief is a function over all of ℝ³, and the prediction integral over "every place the robot could have been" has no closed form. You can't enumerate infinitely many cells. So the rest of the lesson is about a brilliant cheat: assume the belief is always a Gaussian. If it is, the two scary equations collapse into a handful of matrix multiplies that update just (μ, Σ). That's the Kalman filter. First, why Gaussians make the integral disappear — Chapter 2.

Chapter 2: Why the Gaussian? — The One Distribution That Survives

If we must pick one shape for the belief and stick with it forever, which shape is magic? The answer is the Gaussian (the bell curve, also called the normal distribution). It is magic for one specific reason that we'll make precise: when you push a Gaussian through the operations the Bayes filter needs — linear maps, sums, products — you get another Gaussian back. The shape is closed under exactly the operations we need. Nothing else is.

The 1-D Gaussian: just a mean and a variance

A one-dimensional Gaussian is fully described by two numbers. Its density is:

Don't be intimidated — only two symbols carry meaning. μ (mu) is the mean: the center of the bell, the best single guess. σ² (sigma-squared) is the variance: how wide the bell is, i.e. how uncertain we are. (σ itself is the standard deviation.) The whole leading fraction is just a constant that makes the area under the curve equal 1. We write "X is distributed as a Gaussian with mean μ and variance σ²" as X ~ N(μ, σ²). That is the entire belief: two numbers.

The multivariate Gaussian: a mean vector and a covariance matrix

A robot pose lives in more than one dimension (x, y, θ), so we need the multivariate Gaussian. Now the mean is a vector μ ∈ ℝⁿ (the center, one coordinate per state variable), and the spread is a covariance matrix Σ ∈ ℝ^n×n:

The covariance matrix Σ is the multivariate version of variance. Its diagonal entries are the variances of each coordinate (how unsure we are about x, about y, about θ). Its off-diagonal entries are covariances — how the errors in two coordinates are correlated. A non-zero x–y covariance means "if I'm wrong about x, I'm probably wrong about y in a correlated way." Geometrically, Σ is an uncertainty ellipse: the diagonal sets the width and height, the off-diagonal tilts it.

The three properties that make Gaussians survive the filter

Here is the payoff. Stanford lists three closure properties, and every one is load-bearing for the Kalman filter:

• 1. Affine map → still Gaussian. If X ~ N(μ, Σ) and Y = AX + b, then Y ~ N(Aμ + b, AΣA^⊤). Pushing a Gaussian through a linear function gives a Gaussian. This is the prediction step: the motion model is a linear map, so the predicted belief stays Gaussian.
• 2. Sum of independents → still Gaussian. If X₁ ~ N(μ₁, Σ₁) and X₂ ~ N(μ₂, Σ₂) are independent, their sum is N(μ₁+μ₂, Σ₁+Σ₂). This is how process noise gets added in: covariances simply add.
• 3. Product of Gaussians → still Gaussian. Multiply two Gaussian densities and (after renormalizing) you get a Gaussian. This is the update step: bel(x) ∝ p(z|x)·bel̅(x) is a product of two Gaussians, so the corrected belief is Gaussian.

The affine property, worked with numbers

Property 1 is the engine of the predict step, so let's see it move. Suppose a robot's position is X ~ N(μ = 2, σ² = 0.5). It moves under the map Y = AX + b with A = 1 (position carries over) and b = 3 (commanded +3 m). The property says Y ~ N(Aμ+b, Aσ²A) = N(1·2+3, 1·0.5·1) = N(5, 0.5). The mean shifted by the commanded move; the variance was unchanged (a pure shift adds no uncertainty by itself).

Now scale instead: Z = 2X (e.g. a units change, or velocity doubling a displacement). Then Z ~ N(2·2, 2·0.5·2) = N(4, 2.0). Notice the variance grew by A² = 4, not by A. Spread scales with the square of the linear factor — which is exactly the a²σ² in the 1-D predict and the FΣF^⊤ in the matrix predict. The transpose F^⊤ appears because, in multiple dimensions, you scale the covariance on both sides.

Play with a 2-D covariance ellipse. Drag the variance sliders to fatten it along x or y; drag the correlation slider to tilt it. The ellipse is the "1-sigma" contour — the region the robot believes it's probably inside. This same ellipse will breathe in the tracking showcase: growing on predicts, shrinking on updates.

Chapter 3: The 1-D Kalman Filter — Five Equations, By Hand

Let's collapse the Bayes filter onto a Gaussian belief in the simplest possible world: a robot moving along a single line, so the state is just a scalar position x. We assume (the three Kalman assumptions from the source):

• Linear motion: x_t = a·x_t−1 + b·u_t + ε_t, with process noise ε_t ~ N(0, σ_R²). For a robot rolling forward, a = 1 (position carries over) and b·u_t is the commanded displacement.
• Linear measurement: z_t = c·x_t + δ_t, with measurement noise δ_t ~ N(0, σ_Q²). For a sensor reading position directly, c = 1.
• Gaussian start: the initial belief is N(μ₀, σ₀²).

Under these three, the belief is Gaussian for all time, so we only ever track a mean μ and a variance σ². The Bayes filter's two steps become five scalar equations. (We'll use the convention σ_R² = process/motion noise, σ_Q² = measurement/sensor noise, matching Stanford's R and Q.)

Predict (project the belief forward through motion)

The mean rides the motion model forward. The variance grows: it's scaled by a² (here a=1, so unchanged) and then the process noise σ_R² is added (Gaussian property 2!). Moving makes us less sure. The bar denotes "predicted, pre-measurement."

Update (fold in the measurement)

The middle quantity K is the Kalman gain — the star of the next chapter. The term (z − c·μ̅) is the innovation: the surprise, how far the actual measurement is from what we predicted we'd measure. The new mean is the prediction nudged toward the measurement by K×surprise. The new variance shrinks by the factor (1 − K·c): sensing makes us more sure.

A full step, by hand, with actual numbers

Let's grind one complete cycle. Take c = 1 (sensor reads position directly) and a = 1.

Setup. Prior belief: μ = 2.0 m, σ² = 0.40 (we're moderately unsure). We command the robot to move +1.0 m, so b·u = 1.0. Process noise σ_R² = 0.20. Then the sensor reads z = 3.6 m with measurement noise σ_Q² = 0.10.

Look at what happened. We predicted 3.0; the sensor said 3.6. K = 0.857 means we trust the sensor a lot (because σ_Q² = 0.10 is much smaller than σ̅² = 0.60), so the fused estimate 3.514 lands close to the measurement — but not all the way; the prediction still pulls it back a little. And crucially the variance dropped from 0.60 to 0.086: after fusing, we are far more confident than from either source alone. That is the entire Kalman filter: predict (grow uncertainty), then update (blend toward the measurement and shrink uncertainty).

From scratch in Python

python
def kf_1d_step(mu, var, u, z, a=1.0, b=1.0, c=1.0, R=0.2, Q=0.1):
    # --- predict (motion) ---
    mu_bar  = a * mu + b * u          # project mean forward
    var_bar = a * a * var + R         # grow variance by process noise
    # --- update (measurement) ---
    K = (c * var_bar) / (c * c * var_bar + Q)   # Kalman gain (trust slider)
    mu_new  = mu_bar + K * (z - c * mu_bar)      # nudge toward the surprise
    var_new = (1.0 - K * c) * var_bar             # shrink variance
    return mu_new, var_new

# reproduce the worked example exactly:
mu, var = kf_1d_step(2.0, 0.4, u=1.0, z=3.6, R=0.2, Q=0.1)
print(round(mu,3), round(var,3))   # -> 3.514 0.086

A second cycle — watch the recursion compound

The filter is recursive: the output of one cycle is the input to the next. Take our result (μ = 3.514, σ² = 0.086) as the new prior. Command another +1.0 m move; the sensor now reads z = 4.7.

Two things to notice across cycles. First, the gain dropped from 0.857 to 0.741: as the estimate sharpens (smaller σ̅² entering the update), the filter trusts each new measurement a little less and its own prediction a little more. Second, the variance keeps trending down (0.086 → predict-to-0.286 → update-to-0.074), settling toward a steady-state value where the per-step variance growth from process noise exactly cancels the shrink from updating. The filter is learning, and you can watch it converge in the numbers.

Run that loop tick after tick — predict, update, predict, update — and you have a working filter. The idiomatic NumPy version just promotes the scalars to matrices; we'll do that in Chapter 5. First, the one quantity that controls everything: the gain K.

Chapter 4: The Gain K — A Trust Slider

Of the five equations, one number decides everything: the Kalman gain K. With c = 1 it has a beautifully readable form:

Read K as a fraction between 0 and 1 — a literal trust slider sliding between "trust the prediction" and "trust the measurement." It compares the two uncertainties and hands the bigger weight to whichever source is more confident (smaller variance).

The two extremes, worked out

Watch the gain settle, step by step

The gain isn't static — it evolves as the filter gains confidence. Here's a robot at rest (no motion, a=1, b=0) with steady process noise σ_R² = 0.04 and a sensor of fixed noise σ_Q² = 0.25, starting from a vague prior σ₀² = 4.0. Each tick: predict (var += 0.04), then compute K and shrink the var. Watch K fall toward a steady value as the estimate sharpens:

The first measurement gets a near-1 gain (0.942) and yanks the vague estimate into place. By tick 4 the gain has dropped to 0.36 and is converging toward a steady-state gain — the equilibrium where the shrink from updating exactly balances the growth from process noise. At steady state the filter no longer "learns" net confidence; it just maintains a fixed uncertainty, trusting each measurement a fixed fraction. This steady-state gain is what classical engineers precompute as the "α–β filter" constants. The Kalman filter derives them automatically from the noise ratio.

Why this is the optimal weight (intuition)

If you have two independent noisy estimates of the same quantity — one with variance σ₁², one with σ₂² — the minimum-variance way to combine them weights each inversely to its variance. Concretely the fused mean is (μ₁/σ₁² + μ₂/σ₂²) / (1/σ₁² + 1/σ₂²) and the fused variance is 1/(1/σ₁² + 1/σ₂²) — "inverse variances add." The Kalman gain is exactly that inverse-variance weighting, algebraically rearranged into the (z − μ̅) innovation form. The noisier a source, the smaller its slice of the blend. There's no cleverer linear combination; K is provably the best one, which is the precise sense in which the Kalman filter is "optimal."

Slide the two uncertainties below and watch K respond, and watch the fused Gaussian (teal) sit between the prediction (purple) and the measurement (orange) — always closer to whichever is sharper, and always taller (more certain) than either input.

Chapter 5: The Matrix Kalman Filter — Tracking in 2D

A real state is a vector, so μ becomes a vector and σ² becomes the covariance matrix Σ. The scalars a, b, c become matrices. Nothing conceptually changes — the same five equations, promoted to linear algebra. Here is the dictionary (Stanford's notation: F = motion matrix, B = control matrix, H = measurement matrix, R = process-noise covariance, Q = measurement-noise covariance):

The five matrix equations

Compare line-for-line to the scalar version: a²σ² became FΣF^⊤ (Gaussian property 1, the affine map on a covariance). The "+R" is still the added process noise (property 2). K is now a matrix, and the division became a matrix inverse. The innovation (z − Hμ̅) is now a vector of surprises. That's the whole algorithm.

Worked example: constant-velocity tracking

We'll track a target that moves on a line, but now the state holds both position and velocity: x = [p, v]^⊤. This is the classic "constant-velocity model." Over a timestep Δt = 1, position advances by velocity, velocity stays put. There's no control (B = 0), and the sensor measures position only (it can't see velocity directly — the filter must infer velocity). So:

Prior: μ = [0, 1]^⊤ (at position 0, believed moving at 1 unit/step), Σ = [[1, 0],[0, 1]]. Measurement: z = 1.2 (sensor sees position).

Predict the mean — μ̅ = Fμ:

Position moved from 0 to 1 (carried by velocity 1); velocity stayed at 1. Exactly the constant-velocity prediction.

Predict the covariance — Σ̅ = FΣF^⊤ + R. First FΣ = [[1,1],[0,1]]·[[1,0],[0,1]] = [[1,1],[0,1]]. Then (FΣ)F^⊤ = [[1,1],[0,1]]·[[1,0],[1,1]] = [[2,1],[1,1]]. Add R:

Notice the off-diagonal 1.00 appeared from nothing! Position and velocity are now correlated — the prediction step taught the filter that position-error and velocity-error move together. This coupling is what lets a position-only sensor correct the velocity estimate.

Innovation covariance S = HΣ̅H^⊤ + Q. With H = [1, 0], HΣ̅H^⊤ picks out the top-left entry 2.01, so S = 2.01 + 0.5 = 2.51.

Gain K = Σ̅H^⊤S⁻¹. Σ̅H^⊤ = first column of Σ̅ = [2.01, 1.00]^⊤. Divide by S = 2.51:

Two gains! K's top entry (0.801) corrects position; its bottom entry (0.398) corrects velocity — even though the sensor never measured velocity. The correlation we just grew is what carries the position-surprise into a velocity correction.

Update the mean. Innovation = z − Hμ̅ = 1.2 − 1 = 0.2. So μ = μ̅ + K·(0.2) = [1, 1]^⊤ + [0.160, 0.080]^⊤ = [1.160, 1.080]^⊤. The position was nudged toward the reading, and the velocity estimate edged up too — the filter inferred "you're a touch further along than I thought, so you're probably going a touch faster."

From scratch in NumPy

python
import numpy as np

def kf_step(mu, Sig, u, z, F, B, H, R, Q):
    # predict
    mu_bar  = F @ mu + B @ u
    Sig_bar = F @ Sig @ F.T + R
    # update
    S = H @ Sig_bar @ H.T + Q                 # innovation covariance
    K = Sig_bar @ H.T @ np.linalg.inv(S)      # Kalman gain (matrix)
    y = z - H @ mu_bar                         # innovation (surprise)
    mu_new  = mu_bar + K @ y
    I = np.eye(len(mu))
    Sig_new = (I - K @ H) @ Sig_bar
    return mu_new, Sig_new

# constant-velocity tracker reproducing the worked numbers:
F = np.array([[1.,1.],[0.,1.]]); B = np.zeros((2,1)); H = np.array([[1.,0.]])
R = np.eye(2)*0.01; Q = np.array([[0.5]])
mu = np.array([0.,1.]); Sig = np.eye(2)
mu, Sig = kf_step(mu, Sig, np.zeros(1), np.array([1.2]), F, B, H, R, Q)
print(mu.round(3))   # -> [1.16 1.08]

The "from scratch" and the "idiomatic" version are the same here — the matrix form is the clean form. We just promoted scalars to arrays and division to np.linalg.inv. Now let's watch it run continuously, tracking a moving target.

Chapter 6: 2D Tracking — The Ellipse That Follows

Let's put the matrix filter to work on a problem you can see: tracking a target that wanders across the plane. The state is now four numbers — position and velocity in both x and y: x = [p_x, p_y, v_x, v_y]^⊤. The constant-velocity model says each position advances by its velocity each step:

The sensor measures position only (a noisy ping of where the target is), so H picks out the first two coordinates: H = [[1,0,0,0],[0,1,0,0]]. The filter never directly sees velocity — it infers it from how position changes, exactly as in Chapter 5, now in 2D.

Try the two failure modes by hand. Crank Q way up (sensor is garbage): the filter stops trusting measurements, K shrinks, the ellipse grows, and the estimate coasts on the constant-velocity model — smooth but it can drift if the target turns. Crank R way up (model is untrusted): the filter trusts every measurement, K→1, the estimate becomes as jittery as the orange dots. The sweet spot is in between, and finding it is the art of filter tuning.

The four-dimensional belief, made concrete

The covariance Σ is now 4×4, and its structure tells a story. The top-left 2×2 block is the position uncertainty (the ellipse you see). The bottom-right 2×2 block is the velocity uncertainty (invisible, but real). The off-diagonal blocks are the position–velocity correlations — and after the F matrix mixes them each predict step, those correlations are what let a position-only sensor sharpen the hidden velocity estimate, exactly as in Chapter 5's worked example, now happening 14 times a second.

Trace one tick in your head: predict advances each position by its estimated velocity (the Δt entries in F) and inflates Σ by R; update takes the 2-D position innovation (z − Hμ̅), runs it through the 4×2 gain K, and corrects all four state components — including the two velocities the sensor never saw. The estimate's smoothness is the velocity estimate doing its job: a good velocity lets the predict step place the next estimate accurately before the noisy measurement even arrives.

Chapter 7: The Extended Kalman Filter — Bending the Line

The Kalman filter's whole magic rested on one word: linear. F, B, H were matrices, and a linear map of a Gaussian is a Gaussian. But a real robot's world is curved. A differential-drive robot's motion has cosθ and sinθ in it. A range sensor measures √((m_x−x)² + (m_y−y)²) — a square root, hopelessly nonlinear. Push a Gaussian through those and it warps into a banana shape that is no longer Gaussian. The clean machinery breaks.

The general nonlinear models replace the matrices with functions:

where g is the nonlinear motion model and h is the nonlinear measurement model. The Extended Kalman Filter (EKF)'s key idea is delightfully pragmatic: don't compute the warped banana exactly — approximate g and h by their best straight-line fit at the current estimate, then run the ordinary Kalman filter on that line.

The derivation, in one breath

Start from the Taylor expansion of the motion model about the previous mean μ_t−1:

The first term is a constant (g at the mean); the second is a linear function of x with slope-matrix G = ∇_xg. But that's precisely the affine form A·x + b that the Kalman filter already knows how to handle! So we just feed this linearization into the KF machinery: the mean rides the real curve (μ̅ = g(u, μ), the constant term), and the covariance rides the linear slope (Σ̅ = GΣG^⊤ + R, the affine-property formula with G in the F-slot). The measurement model gets the identical treatment, expanded about the predicted mean μ̅, giving H = ∇_xh. That's the entire EKF derivation: Taylor-expand, then run the KF on the tangent.

The EKF equations — the KF with g, h, and Jacobians

Define G = ∂g/∂x evaluated at the previous mean μ_t−1, and H = ∂h/∂x evaluated at the predicted mean μ̅_t. Then:

Spot the difference from Chapter 5. The mean is pushed through the full nonlinear function (μ̅ = g(...), not Fμ) — we use the real curve for the best estimate. But the covariance is propagated through the linearized Jacobian (GΣG^⊤) — we use the tangent line for the uncertainty. The innovation uses the real h: (z − h(μ̅)). That's the entire EKF: KF equations with g/h for the means and G/H for the covariances.

Worked example: the Jacobian of a range/bearing measurement

This is the heart of EKF localization (next chapter). A robot at pose (x, y, θ) measures a landmark at known map location (m_x, m_y). The sensor returns the range r (distance) and bearing φ (angle relative to the robot's heading). The measurement model h is (from the source):

That square root and arctangent are why we need the EKF. The Jacobian H = ∂h/∂(x,y,θ) is a 2×3 matrix. Let q = (m_x−x)² + (m_y−y)² (the squared distance, so r = √q). Working out the partial derivatives gives:

Let's plug in actual numbers. Robot at (x, y, θ) = (1, 2, 0). Landmark at (m_x, m_y) = (4, 6). Then:

• Δx = m_x−x = 4−1 = 3,   Δy = m_y−y = 6−2 = 4.
• q = 3² + 4² = 9 + 16 = 25,   so r = √25 = 5.
• Top row (range gradient): [ −3/5, −4/5, 0 ] = [−0.6, −0.8, 0].
• Bottom row (bearing gradient): [ 4/25, −3/25, −1 ] = [0.16, −0.12, −1].

Sanity-check the signs — they encode geometry. The range row says: move the robot +1 in x (the −0.6 entry) and the measured range decreases by 0.6 (you got closer to the landmark on its left). The bearing row's last entry is exactly −1: rotate the robot by +1 radian and the measured bearing drops by 1 radian (turning left makes a landmark appear more to your right). The Jacobian isn't abstract — each entry is "how much does this reading change if I nudge this state coordinate," and the numbers match physical intuition.

Confirming the Jacobian numerically (finite differences)

python
import numpy as np

def h(state, m):
    x, y, th = state
    dx, dy = m[0]-x, m[1]-y
    r   = np.hypot(dx, dy)
    phi = np.arctan2(dy, dx) - th
    return np.array([r, phi])

def H_analytic(state, m):
    x, y, th = state
    dx, dy = m[0]-x, m[1]-y
    q = dx*dx + dy*dy; r = np.sqrt(q)
    return np.array([[-dx/r, -dy/r, 0],
                     [ dy/q, -dx/q, -1]])

def H_numeric(state, m, eps=1e-6):       # finite-difference check
    J = np.zeros((2, 3))
    for i in range(3):
        s1 = np.array(state, float); s1[i] += eps
        s0 = np.array(state, float); s0[i] -= eps
        J[:, i] = (h(s1, m) - h(s0, m)) / (2*eps)
    return J

state, m = [1.,2.,0.], [4.,6.]
print(H_analytic(state, m).round(3))   # [[-0.6 -0.8  0.] [ 0.16 -0.12 -1.]]
print(np.allclose(H_analytic(state,m), H_numeric(state,m)))  # True

That allclose(...) == True is how you debug a hand-derived Jacobian in practice: wiggle each state coordinate by ε, watch how h changes, and confirm the slope matches your formula. A wrong Jacobian is the #1 cause of a diverging EKF.

Chapter 8: EKF Localization — Finding the Robot on a Known Map

Now the payoff for the whole robot-autonomy stack. Localization is the problem of determining the robot's pose relative to a given map. The map is a list of landmarks — point features with known global coordinates m_j = (m_j,x, m_j,y). The robot drives around (odometry → prediction) and, whenever it sees a landmark, measures its range and bearing (sensor → update). EKF localization fuses these into a continuously-corrected pose. This is hw4's Problem 1, and it's how a real TurtleBot knows where it is in a mapped building.

The motion model g (differential drive)

The control is u = [V, ω]^⊤ (forward velocity, turn rate). Over a timestep Δt with a zero-order hold, integrating the differential-drive dynamics gives the discrete motion model g (for ω not near zero):

When ω → 0 (driving nearly straight) this is numerically unstable (dividing by a tiny ω), so we use the straight-line limit: x' = x + V·cosθ·Δt, y' = y + V·sinθ·Δt, θ' = θ. The cos/sin make g nonlinear — hence EKF, not KF.

The motion Jacobian G, with numbers

The covariance prediction needs G = ∂g/∂(x,y,θ), a 3×3 matrix. Since x and y don't appear on the right-hand side of g except as the carried-over x, y (slope 1), the only nontrivial column is the θ one. Differentiating the straight-line form (small ω) gives the clean result:

Plug in numbers. Robot at θ = 0, commanded V = 0.5 m/s for Δt = 1 s. Then −V·sin0·1 = 0 and V·cos0·1 = 0.5, so:

Read the (1,3) entry, 0.5: if the heading θ is off by a little, the predicted y position is off by 0.5× as much (because at θ=0 the robot drives along x, so a heading error tips it sideways in y). That single off-diagonal is how a heading uncertainty leaks into a position uncertainty during prediction — and conversely how a position-correcting landmark sighting can tighten the heading estimate. The covariance step Σ̅ = GΣG^⊤ + R then grows and rotates the pose ellipse according to this G, and adds the process-noise budget R.

The measurement model h (range & bearing)

Exactly the h from Chapter 7: for the landmark we're looking at, h returns the predicted range and bearing, and H is the 2×3 Jacobian we computed. The full EKF-localization loop:

A concrete prediction–then–correction

Robot believes it's at μ = (1, 2, 0). It uses the Chapter 7 landmark at (4, 6), so it predicts it should see range r̂ = 5.0 and bearing φ̂ = atan2(4, 3) − 0 = 0.927 rad (about 53°). Now the real sensor returns z = (range 4.6, bearing 0.95). The innovation is the surprise: (4.6 − 5.0, 0.95 − 0.927) = (−0.4, +0.023). A negative range innovation means "I'm closer to the landmark than I thought," so the update will shift the robot's estimated position toward the landmark.

Let's actually compute the position correction. Take the predicted covariance Σ̅ = diag(0.3, 0.3, 0.1) (modest uncertainty), the Chapter-7 Jacobian H = [[−0.6, −0.8, 0], [0.16, −0.12, −1]], and measurement noise Q = diag(0.04, 0.01). We need S = HΣ̅H^⊤ + Q. The range row of HΣ̅ is [−0.6·0.3, −0.8·0.3, 0] = [−0.18, −0.24, 0]; dotting with the range row of H gives 0.6·0.18 + 0.8·0.24 = 0.108 + 0.192 = 0.30. So the range–range entry of S is 0.30 + 0.04 = 0.34. Focusing on the dominant range channel, its gain into x is roughly Σ̅_xx·H_r,x/S_rr = 0.3·(−0.6)/0.34 = −0.53.

The x-correction from the range innovation is then (−0.53)·(−0.4) = +0.21 m: the negative gain times the negative surprise yields a positive push in x — toward the landmark, which sits at larger x. Run the same arithmetic for y and you get a positive push toward y = 6 as well. The pose estimate slides from (1, 2) toward (4, 6) by the amount the geometry and the relative uncertainties justify, and Σ shrinks along the landmark's line of sight. The signs of H carried the surprise into a correction in exactly the right physical direction — which is the whole reason we computed that Jacobian so carefully in Chapter 7.

Multiple landmarks and data association

Real scans return several landmarks at once. Because the measurements are conditionally independent given the pose, the multi-measurement update is just a loop: apply the standard EKF correction once per landmark, as if no motion happened in between. But there's a subtlety the source dwells on: data association — which map landmark produced this reading? With unknown correspondences, you pick the landmark j that minimizes the Mahalanobis distance between the actual reading and the predicted one:

The Mahalanobis distance is "how many standard deviations of surprise is this association" — an innovation weighted by its covariance, so a big innovation through a very uncertain channel can still be a plausible match. A validation gate (d_j ≤ γ) rejects readings that don't match any landmark well, so a wandering pedestrian's leg doesn't corrupt your pose.

Chapter 9: The Unscented Kalman Filter — No Jacobians, Just Samples

The EKF's weakness is the linearization itself: it replaces a curve with a single tangent line at one point (the mean), then propagates the whole covariance through that one line. If the function bends within the spread of the belief, that's a lousy approximation. The Unscented Kalman Filter (UKF) attacks this with a different philosophy, captured in one slogan: it is easier to approximate a probability distribution than an arbitrary nonlinear function.

Picking the sigma points

For an n-dimensional belief N(μ, Σ) we use 2n+1 sigma points. The center one is the mean. The other 2n are placed symmetrically along the principal axes of the covariance, at a distance set by a spread parameter. For the 1-D case (n=1), the three sigma points are simply:

where λ is a tuning scalar (the points sit a few standard deviations out). The square root of the covariance is what spreads the points to match the belief's width. In higher dimensions, √Σ becomes a matrix square root (Cholesky factor), and the points spread along its columns.

Worked example: the unscented transform on y = x²

Let's push a Gaussian through the nonlinear function y = x², by hand. Belief: μ = 2, σ² = 1. Take λ = 0 for simplicity, so the spread distance is √((1+0)·1) = 1. The three sigma points and equal weights w = 1/3 each (with λ=0, all weights equal 1/(2(n+λ)) for the outer points and λ/(n+λ) for the center; with n=1, λ=0 this gives 0, 1/2, 1/2 — let's use that standard weighting):

Recover the mean — weighted average of the transformed points: μ_y = 0·4 + 0.5·9 + 0.5·1 = 5.0.

Recover the variance — weighted spread of transformed points around μ_y: σ_y² = 0.5·(9−5)² + 0.5·(1−5)² = 0.5·16 + 0.5·16 = 16.0.

Compare to the EKF's answer. The EKF would linearize y = x² at μ = 2: the derivative is 2x = 4, so it predicts μ_y ≈ h(2) = 4 and σ_y² ≈ 4²·1 = 16. The true mean of x² for x~N(2,1) is E[x²] = μ² + σ² = 4 + 1 = 5. The UKF got the mean exactly right (5.0) — it captured the curvature bias that the EKF missed (the EKF said 4, off by the whole σ²). This is the UKF's headline advantage: it captures effects to higher order than the EKF's single tangent line, for free, with no derivatives.

From transform to full filter step

The unscented transform we just did is the core; a full UKF step wraps the predict and update around it. To predict: take the sigma points of bel(x_t−1), push them through the motion model g, then recover μ̅ and Σ̅ from the transformed cloud (plus add process noise R). To update: take sigma points of the predicted belief, push them through the measurement model h to get a predicted measurement cloud, then compute the gain from the cross-covariance between the state cloud and the measurement cloud:

where S is the measurement cloud's covariance (plus Q) and Σ_xz is the cross-covariance between the state sigma points and their measurements. Compare to the EKF gain Σ̅H^⊤(HΣ̅H^⊤+Q)⁻¹: the term Σ̅H^⊤ (state-to-measurement coupling via the Jacobian) is replaced by the empirically measured cross-covariance Σ_xz. Same structure, but no derivative anywhere — the sigma points discovered the coupling by being pushed through the real h.

From scratch (1-D unscented transform)

python
import numpy as np

def unscented_1d(mu, var, f, lam=0.0):
    n = 1
    spread = np.sqrt((n + lam) * var)
    chi = np.array([mu, mu + spread, mu - spread])   # 2n+1 = 3 sigma points
    wm  = np.array([lam/(n+lam), 0.5/(n+lam), 0.5/(n+lam)])  # weights
    Y   = f(chi)                                     # push each through TRUE f
    mu_y  = np.sum(wm * Y)
    var_y = np.sum(wm * (Y - mu_y)**2)
    return mu_y, var_y

mu_y, var_y = unscented_1d(2.0, 1.0, lambda x: x**2)
print(round(mu_y,2), round(var_y,2))   # -> 5.0 16.0  (EKF would say 4.0, 16.0)

Watch the transform happen. Below, a Gaussian (left) has its three sigma points pushed through y = x² (the curve), landing as transformed points (right) that we average into the output Gaussian. Drag the input mean and width; the sigma points climb the curve and the output Gaussian reshapes — and the UKF's recovered mean tracks the true warped mean, not the tangent-line guess.

Chapter 10: Showcase — A Robot Localizes Itself, Live

Everything converges here. A robot drives a loop through a known map of landmarks (purple squares). It can't see its own pose — it only has noisy odometry (predict) and noisy range/bearing readings to landmarks (update). The teal robot is the EKF estimate; the faint grey robot is the unknown truth; the teal ellipse is the live pose covariance Σ, breathing as the filter predicts and corrects. Beams flash to the landmarks the robot is currently using to localize.

Push it until it breaks. Set measurement noise to the max and turn dropout on for a while: the ellipse grows huge, the estimate wanders off the truth on pure dead-reckoning — exactly the Chapter 0 problem, now visible as a swelling ellipse. Then drop a landmark back in: a single good reading collapses the ellipse and yanks the estimate home. That snap is the Kalman gain doing its job — a huge Σ̅ means a near-1 gain, so the fresh measurement dominates. The filter automatically becomes greedy for information exactly when it's most lost.

Chapter 11: Connections & Cheat-Sheet

You can now derive, by hand and in code, the entire Kalman family — and you understand exactly where each one lives on the autonomy stack: the filter is the Localization & SLAM module (Lesson 1's purple band), fusing the motion model (Lesson 2) with the sensors (Lesson 7) into the pose estimate every other module runs on.

The family, at a glance

Cheat-sheet

Go deeper on Engineermaxxing

These lessons unpack each piece in its own right — cross-link your understanding:

• The Bayes Filter — the recursive predict/update engine all of these specialize, derived from scratch with the hallway histogram.
• The Kalman Filter & Why a Kalman Filter Works — the linear-Gaussian filter in full, the gain as a trust slider, and the optimality story.
• The Extended Kalman Filter & The Unscented Kalman Filter — the nonlinear extensions, with deeper Jacobian and sigma-point derivations.
• Lesson 7: Sensors (LiDAR, IMU, ICP) — where the noisy measurements z and the noise covariance Q actually come from.
• Lesson 11: SLAM & Factor Graphs — what happens when the map itself is unknown and must be estimated jointly with the pose.
• Lesson 14: Particle Filters & MCL — the non-parametric sequel for when the Gaussian assumption breaks: global localization and the kidnapped robot.