The Extended Kalman Filter — Fusing Nonlinear Sensors

Chapter 1: A Two-Minute Recap — the Linear Kalman Filter

Before we extend the Kalman filter, let's make sure the thing we're extending is fresh. This is a deliberate, brisk recap — the full treatment is in Lesson 5: The Kalman Filter, and if any of this feels shaky, go back there first. Everything in this lesson is built on top of these five equations, so they must feel like home.

The Kalman filter tracks a state — the quantities you care about, like position and velocity — written as a vector x. It never claims to know x exactly; it carries a best estimate (written x̂, "x-hat") plus a covariance P, which is its honest uncertainty — a measure of how fuzzy the estimate is. A small P means "I'm confident"; a large P means "I'm guessing."

It runs a relentless two-step loop, forever:

The five equations (linear version)

In the linear world, the motion model is a matrix F (how the state evolves) plus a control input u through a matrix B, and the measurement model is a matrix H (how the state maps to what the sensor sees). The predict step:

The little minus superscript means "before the measurement" (the prediction). The update step then computes the famous Kalman gain K — the trust dial that decides how much to believe the new measurement versus the prediction — and uses it to correct both the estimate and the covariance:

Where z is the actual measurement, R is the measurement-noise covariance (how noisy the sensor is), and Q is the process-noise covariance (how much the motion model is trusted). The quantity (z − H x̂⁻) is the star of the whole show: it's the innovation — the surprise — the gap between what the sensor said and what we predicted it would say. The gain K scales that surprise into a correction.

Why H appears twice, and what it really does

Notice H shows up in two roles, and separating them is the key to the whole extension. First, in H x̂⁻ (inside the innovation), H is being used to predict the measurement — "if my state is x̂⁻, what would the sensor read?" Second, in P H^T and H P H^T (inside the gain), H is being used as a slope — "how sensitive is the measurement to a small change in the state?" — to propagate uncertainty.

In the linear world these are the same matrix, because for a straight line the function is its slope. But for a curve, predicting the measurement and finding the slope are different operations: you predict by plugging your state into the real curved function h(x), and you find the slope by differentiating h. The EKF keeps these two jobs separate — use the true function h for the prediction, use its Jacobian H for the slope. Hold that distinction; it is the hinge of Chapters 4 and 5.

Chapter 2: The Problem — Real Robots Are Nonlinear

We've claimed the world is curved. Now let's make the claim concrete by writing down the two functions a real robot actually uses, and watching the linearity assumption shatter in each. There are exactly two places nonlinearity sneaks into a filter: the motion model (how the state evolves) and the measurement model (how the state maps to what a sensor reads). Both are curved for an ordinary wheeled robot.

The nonlinear motion model — the unicycle

Our robot's state is its pose: position and heading, x = [x, y, θ], where θ (theta) is the direction it's facing. Each tick the robot is commanded to drive forward a distance v and turn by an angle ω (omega). How does the pose change? Drive forward in the direction you're facing — so your x and y change by the forward distance projected onto each axis, which means cosine and sine of your heading:

This three-line rule is the unicycle model (also called the differential-drive model) — the standard motion model for wheeled robots, and the reason your robot vacuum can be told "go forward and turn." Call this whole update the function f(x, u): it takes the old pose x and the command u = [v, ω] and returns the new pose. And it is hopelessly nonlinear, because of those cos(θ) and sin(θ) terms. Your new x depends on the cosine of your heading, not on the heading times some matrix. There is no matrix F that produces a cosine.

The nonlinear measurement model — range and bearing

Now the sensor. The robot measures the range r (straight-line distance) and bearing φ (relative direction) to a landmark at a known location (x_L, y_L). Let dx = x_L − x and dy = y_L − y be the offsets from the robot to the landmark. Then geometry — the Pythagorean theorem and a bit of trig — gives:

Call this pair the measurement function h(x): it takes your pose and returns what the sensor should read. (Here atan2 is the two-argument arctangent — it returns the angle of the vector (dx, dy), handling all four quadrants correctly, where a plain arctangent would get confused.) And again, hopelessly nonlinear: the range is a square root of a sum of squares, and the bearing is an arctangent. Neither can be written as a matrix H times your pose. A square root is not a matrix; an arctangent is not a matrix.

What goes wrong if you ignore the curvature

Suppose you stubbornly pretend the system is linear — you pick some fixed matrices F and H and run the ordinary Kalman filter. Two things break, and they compound:

• The prediction lands in the wrong place. Moving the estimate forward with a matrix instead of the real cos/sin update puts the predicted pose somewhere the robot could never actually be. Small per-step, but it accumulates.
• The innovation is computed against a wrong expected measurement. If you predict the range with a matrix instead of the true square root, the "surprise" (z − expected) is garbage, so the correction K applies is garbage — it pulls the estimate toward a phantom.

And here's the vicious part: once the estimate is wrong, the next prediction starts from a wrong place, so the next linearization (if you were doing one) is anchored at the wrong point, so the next correction is even more wrong. Errors feed errors. This is divergence — the filter's estimate runs away from the truth and never comes back — and it is the central failure mode of the EKF, the subject of Chapter 9. The curvature is not a nuisance to be papered over; respecting it is the difference between a filter that tracks and a filter that explodes.

Below, drive a unicycle robot and watch its true curved path against the path a naive linear motion model predicts. The two agree for an instant after each correction, then peel apart as the heading changes — exactly the curvature the EKF must respect.

Set the turn rate to zero and the dashed and solid lines lie on top of each other — a pure straight drive really is linear, and the ordinary Kalman filter would handle it fine. Crank the turn rate up and the dashed line shoots off tangent while the truth curves away. The gap you see is the linearization error, and it is exactly what the EKF must keep small by re-linearizing at every step around the latest estimate.

Chapter 3: Linearization & the Jacobian — the Local Straight Line

We have a curved function and a filter that only eats straight lines. The fix, in one word, is linearization: replace the curve with the straight line that touches it at the point where we currently are — the tangent line. Over a small neighborhood the tangent line and the curve are nearly the same, so the filter, which only ever makes small corrections, never notices the difference. The art of the EKF is choosing where to draw that tangent (answer: at the current estimate) and knowing when the lie gets too big (Chapter 9).

One input first — the derivative is the slope

Start with the simplest curve, one input and one output, so we can see the idea naked. Take the range to a landmark fixed at the origin while we move along a line at y = 3 — the very example from Chapter 0:

The tangent line at a point x₀ needs two things: the value there, r(x₀), and the slope there. The slope of a function is its derivative, written dr/dx — literally "how much r changes per unit change in x." For the square root, the chain rule gives:

Let's compute the slope at x₀ = 3 with real numbers. The value is r(3) = √(9+9) = √18 ≈ 4.243. The slope is:

So near x = 3, the tangent line is r ≈ 4.243 + 0.707 · (x − 3). Test it: at x = 3.1, the line predicts 4.243 + 0.707×0.1 = 4.314. The true value is √(3.1²+9) = √18.61 = 4.314. Identical to three decimals — the tangent is a superb local stand-in. Now test it far away, at x = 8: the line predicts 4.243 + 0.707×5 = 7.78, but the truth is √(64+9) = √73 = 8.54. Off by 0.76 — the lie grew because we left the neighborhood. Linearization is excellent nearby and lousy far away. Remember that; it is the whole risk profile of the EKF.

Many inputs — the Jacobian is the matrix of all the slopes

Our real functions have several inputs (a pose has x, y, θ) and several outputs (a measurement has range and bearing). The slope is no longer a single number — we need the slope of each output with respect to each input. Collect them all into a grid, and that grid is the Jacobian. It is nothing more exotic than "a matrix where every entry is one partial slope."

Here is the Jacobian as a literal recipe. For a measurement function h(x) with outputs (r, φ) and inputs (x, y, θ), the measurement Jacobian H is:

Two rows (range, bearing), three columns (x, y, θ). Each entry asks one tiny question: "if I nudge this one input a hair, how much does this one output move?" Fill in all six and you have the local linear stand-in for the entire curved sensor. We will compute every one of these six entries with real numbers in Chapter 5 — it is just calculus you can do on paper.

The motion Jacobian, conceptually

The same idea applies to the motion model f(x, u). Its Jacobian with respect to the state (call it F, because it plays the role the matrix F played in the linear filter) asks: "if my pose were a little different, how would the predicted next pose differ?" For the unicycle, the only curved entries come from differentiating the cos and sin terms with respect to the heading θ:

(Because the derivative of cos is −sin, and the derivative of sin is cos.) Everything else in the unicycle motion Jacobian is just 1s and 0s — a small step in x moves the next x one-for-one, and so on. So the motion Jacobian F is mostly the identity matrix with two trig entries from the heading. Concrete numbers for this come in Chapter 4.

Below, see linearization in action on the range function. The blue curve is the truth; the orange tangent line is the local linear approximation at your chosen anchor point. Move the anchor and watch the tangent re-attach — and watch the error (the vertical gap between line and curve) stay tiny near the anchor and balloon as you look further away.

Chapter 4: The EKF Predict Step — Move With f, Grow With F

Now we assemble the machine. The EKF, like the linear filter, runs predict then update. This chapter does predict; the next does update. The rule for converting the linear step to the extended step is mechanical and you'll never forget it: use the true nonlinear function to move the mean, use its Jacobian to move the covariance. That one sentence is the entire extension of the predict step.

Moving the estimate: the true function f

To predict where the robot will be, we do not use a matrix. We push the current best estimate through the actual nonlinear motion function f — the real cos/sin unicycle update from Chapter 2:

This is the first change from the linear filter, and it's the easy one: instead of F x̂ + B u, we just evaluate the honest function. There's no approximation in moving the mean — we use the real physics. If the robot is at (2, 1) facing 30° and commanded to drive 1 m forward, we compute the new x as 2 + 1·cos(30°) = 2 + 0.866 = 2.866, the new y as 1 + 1·sin(30°) = 1.5, exactly. No matrix, no linearization — the mean moves along the true curve.

Growing the uncertainty: the Jacobian F

The covariance is where linearization enters, because there is no exact way to push an uncertainty cloud through a curved function and keep it a clean ellipse. So we approximate: we transport the covariance through the local linear version of f — its Jacobian, evaluated at the current estimate. Call that Jacobian F (it's a matrix; it replaces the linear filter's matrix F):

Identical in form to the linear filter's covariance prediction — same F P F^T + Q — but now F is the Jacobian of the motion model, recomputed fresh at the current estimate every single tick, not a fixed matrix. That re-computation is the heart of "extended": the filter re-linearizes at wherever it currently thinks it is, so the tangent plane is always drawn under its feet.

The unicycle motion Jacobian, with real numbers

Let's compute F concretely so it's not abstract. The motion model maps pose (x, y, θ) to next pose (x', y', θ'). Differentiate each output by each input. Most entries are trivial: x' = x + v·cosθ, so ∂x'/∂x = 1 (a step in x moves x' one-for-one), ∂x'/∂y = 0 (y doesn't appear), and the only curved entry is ∂x'/∂θ = −v·sinθ. Do the same for y' and θ' and you get:

Now plug in numbers. Say the robot is heading θ = 30° and the commanded forward distance is v = 1 m. Then sin(30°) = 0.5 and cos(30°) = 0.866, so:

Read the meaning straight off the matrix. The −0.5 in the top-right says: "if my heading were a little larger (turned more toward north), my next x would be a little smaller" — which is true, because turning away from east means you cover less easting on a forward step. The 0.866 below it says the next y grows as heading increases. Those two off-diagonal terms are precisely where the curvature lives; everything else is the identity, encoding "position carries forward one-for-one." This F is what we sandwich the covariance in: P⁻ = F P F^T + Q.

What the covariance does — the ellipse grows and tilts

Geometrically, the covariance P is an uncertainty ellipse drawn around the estimate — the region the robot probably is in. The predict step does two things to it. First, Q (process noise) inflates it: prediction always adds doubt, because the motion model isn't perfect. Second, F rotates and stretches it: as the robot turns, the directions of greatest uncertainty rotate with it, and the trig entries in F are what carry that rotation into the ellipse. A robot that has driven a long way without a measurement has a big, possibly stretched ellipse — it's confident about some directions and vague about others.

Below, drive the robot and watch its uncertainty ellipse grow with every predicted step (no measurements yet). Notice how the ellipse stretches along the direction of travel — dead-reckoning is more uncertain about how far you've gone than about which way is sideways.

Chapter 5: The EKF Update Step — One Full Correction by Hand

This is the chapter where everything becomes real. We'll take a predicted pose with a measurement coming in, derive the measurement Jacobian H from scratch with actual numbers, sanity-check it with a finite difference (the single most useful debugging trick in all of filtering), and then run one complete EKF update by hand. By the end you could implement the filter from this page alone.

The update rule — the same mechanical swap

The conversion rule is identical to predict: predict the measurement with the true function h, propagate the uncertainty with its Jacobian H. So the innovation uses the honest measurement function, and the gain uses the Jacobian:

Compare to Lesson 5: the only differences are h(x̂⁻) in place of H x̂⁻ (use the curve to predict the measurement), and H = the Jacobian of h everywhere it propagates uncertainty. The trust-dial intuition for K, the shrinking of P, the surprise that is the innovation — all untouched. So the entire job is: compute h, and compute H. Let's do both with numbers.

The measurement Jacobian, derived term by term

Recall the range-bearing measurement, with offsets dx = x_L − x and dy = y_L − y to a landmark at (x_L, y_L):

We need the 2×3 Jacobian H — the slope of each output (r, φ) with respect to each input (x, y, θ). Let's derive all six entries. (Note dx and dy depend on x and y with a minus sign: ∂dx/∂x = −1, ∂dy/∂y = −1.)

Range slopes. Differentiate r = √(dx²+dy²). By the chain rule, ∂r/∂dx = dx/r, and since ∂dx/∂x = −1:

(Range doesn't depend on your heading — how far away the landmark is has nothing to do with which way you're facing, so that entry is exactly 0.) Geometrically −dx/r and −dy/r are the components of the unit vector pointing from the landmark back to you — "move toward the landmark and the range shrinks fastest along the line connecting you."

Bearing slopes. Differentiate φ = atan2(dy, dx) − θ. The derivative of atan2 gives ∂φ/∂dx = −dy/(dx²+dy²) = −dy/r² and ∂φ/∂dy = dx/r². Again applying ∂dx/∂x = −1 and ∂dy/∂y = −1:

(The −1 is because the bearing is measured relative to your heading: turn your body 1 radian and the relative bearing to a fixed landmark drops by exactly 1 radian. That entry is the cleanest in the whole matrix.) Assembling:

Plug in real numbers

Let the robot's predicted pose be x̂⁻ = (2, 1, 0.3 rad) and the landmark be at (5, 5). Then:

The expected measurement (used in the innovation) is h(x̂⁻): range = 5 m, and bearing = atan2(4, 3) − 0.3 = 0.927 − 0.3 = 0.627 rad. Now fill in the Jacobian entries:

The finite-difference sanity check — do not skip this in real life

A wrong Jacobian is the number-one EKF bug (Chapter 9). The cheapest, most reliable defense costs three lines of code: a finite-difference check. The idea: a derivative is just "rise over run for a tiny run." So to check ∂r/∂x, nudge x by a tiny ε (epsilon, say 0.001), recompute the true range, and see how much it moved per unit nudge. If that empirical slope matches your analytic Jacobian entry, your calculus was right. Let's check ∂r/∂x = −0.600 by hand.

At x = 2: dx = 3, r = √(9+16) = 5.000000. Nudge x to 2.001: now dx = 5 − 2.001 = 2.999, dy = 4, so r = √(2.999² + 16) = √(8.994001 + 16) = √24.994001 = 4.999400. The empirical slope is:

It matches −0.600 to three decimals. The analytic Jacobian is correct. Run this check on every Jacobian entry you ever derive — nudge each input, measure the output's response, compare to your formula. If even one entry disagrees, you found a sign error or a dropped chain-rule factor before it crashed your filter in the field. This is not optional hygiene; it is the single discipline that separates working EKFs from diverging ones.

One complete update, by hand

Now run the whole update with our numbers. To keep the arithmetic on paper we'll do the bearing channel only (a 1-D measurement), which shows every step without a matrix inverse. Say the predicted heading is uncertain: the relevant state is θ with predicted value θ̂⁻ = 0.3 rad and prediction variance P = 0.04 rad² (so a standard deviation of 0.2 rad). The bearing measurement has noise variance R = 0.01 rad² (std 0.1 rad). The relevant Jacobian entry is ∂φ/∂θ = −1.

The robot's sensor returns an actual bearing measurement z = 0.50 rad. Step through the update:

1. Predicted measurement: h(θ̂⁻) = atan2(4,3) − 0.3 = 0.927 − 0.3 = 0.627 rad. (What we expected the bearing to read.)
2. Innovation: y = z − h = 0.500 − 0.627 = −0.127 rad. (The surprise: the sensor saw a smaller bearing than predicted.)
3. Innovation variance: S = H P H^T + R = (−1)(0.04)(−1) + 0.01 = 0.04 + 0.01 = 0.05.
4. Kalman gain: K = P H^T S⁻¹ = (0.04)(−1)/(0.05) = −0.8. (The trust dial: 0.8 of the way toward the measurement, sign carried by H.)
5. Corrected heading: θ̂ = θ̂⁻ + K·y = 0.3 + (−0.8)(−0.127) = 0.3 + 0.102 = 0.402 rad. (The estimate moved toward the measurement.)
6. Corrected variance: P = (1 − K H) P = (1 − (−0.8)(−1))(0.04) = (1 − 0.8)(0.04) = 0.2 × 0.04 = 0.008. (Down from 0.04 — the measurement shrank our uncertainty fivefold.)

Read what happened: we expected a bearing of 0.627, the sensor said 0.500, the −0.127 surprise got scaled by the gain into a +0.102 correction of the heading, and our uncertainty fell from 0.04 to 0.008. The estimate is now both more accurate and more confident. That is one EKF update — mechanically identical to a linear Kalman update, except we used the true atan2 function to predict the measurement and the Jacobian entry −1 to propagate the uncertainty. Stack predict-update-predict-update and you have a robot localizer.

Below, place a landmark and a robot, and run a single update step at a time. Watch the expected measurement, the innovation, the gain, and the ellipse shrink — the exact arithmetic above, live.

Chapter 6: Showcase — An EKF Localizing a Robot From Landmarks

Time for the payoff. Below is a full EKF localizer running live: a robot drives a path it doesn't perfectly know (its IMU dead-reckons and drifts), and it occasionally senses the range and bearing to known landmarks scattered around the map. Predict grows the ellipse; each landmark sighting shrinks and reshapes it. This is Thrun's EKF localization, the workhorse of real robot navigation, in your browser — and you can break it.

Play with it and the whole lesson announces itself through behavior, not equations:

• The ellipse breathes. Between landmark sightings it grows (predict-only dead-reckoning, Chapter 4). Each sighting punches it smaller (update, Chapter 5). A robot near many landmarks has a tight ellipse; one in an open stretch with no landmarks watches its ellipse swell — exactly the IMU drift from Lesson 1, now visible.
• The estimate tracks the truth. With good initialization and moderate noise, the green estimate hugs the grey truth, snapping back toward it after every landmark. This is the EKF working: linearizations stay near the truth, so the local-straight-line approximation holds.
• Bad init breaks it. Flip Bad init on. Now the filter starts with a confident-but-wrong pose. Its Jacobians are computed at that wrong point, so the corrections push in slightly wrong directions, the estimate gets more wrong, the next linearization is anchored even further off — and the estimate runs away from the truth. The filter is "sure" (small ellipse) and completely wrong. That is divergence.

A guided experiment — make it diverge, then save it

Don't just watch; falsify. Run this sequence:

1. Reset, Bad init OFF, press Run. The green estimate converges to the grey truth and stays glued. Healthy baseline.
2. Reset, push Sensor noise R to 3×, Run. The ellipse stays larger (the filter trusts noisy measurements less) and the estimate is jumpier, but it still tracks — more uncertainty, not divergence. The EKF degrades gracefully under honest noise.
3. Reset, flip Bad init ON, Run. The estimate starts far off with a wrong heading. Watch it fail to converge — sometimes it crawls back if a strong landmark sighting drags it home, but often it diverges, the green line wandering off while its ellipse stays deceptively small.
4. With Bad init still ON, also push Motion noise Q to 3×, Run. Counterintuitively this can help: a larger Q keeps the ellipse big, which makes the filter trust measurements more and stay humble enough to be pulled back. Over-confidence (tiny ellipse) is what kills a badly-initialized EKF; staying uncertain buys time for the landmarks to fix it.

The crucial contrast is steps 2 and 3. Step 2 (honest noise, good init) degrades gracefully — the filter just gets fuzzier. Step 3 (good noise, bad init) can detonate — not because the noise is high, but because the linearization point is far from the truth, so the tangent-line approximation is a lie and the corrections point the wrong way. The EKF's fate is decided by where it draws its tangent, not by how noisy the world is. That single insight is the whole reason Chapter 9 (debugging) and Lesson 8 (the UKF, which linearizes more cleverly) exist.

Chapter 7: Use Cases & Real Products — The Workhorse of Navigation

Every lesson in this series ends with the same four chapters — Use Cases, Practical Application, Debugging, Connections — because a concept you can't point at in a shipped product is a concept you don't really own. The EKF is special here: it is arguably the single most-deployed estimation algorithm in the world. For decades, if a machine knew where it was, an EKF was almost certainly involved.

GPS/INS integration — the EKF that flies everything

The headline application is GPS/INS integration (INS = inertial navigation system, the high-grade cousin of the IMU). Every airliner, missile, ship, and increasingly every car runs an EKF that fuses a fast-but-drifting inertial system with a slow-but-absolute GPS — the complementary pairing from Lesson 1, but now the GPS measurement is a nonlinear function of position (a pseudorange to each satellite is a distance, a square root). The IMU prediction is nonlinear too (attitude integration on the rotation group is deeply nonlinear). So the fuser cannot be a plain Kalman filter; it is an EKF. This is the algorithm that kept inertial-guided aircraft on course before GPS and that snaps your phone's blue dot back when you leave a tunnel.

Why the EKF, specifically, won

There are fancier nonlinear filters (the UKF of Lesson 8, particle filters, modern factor graphs). The EKF dominated anyway, for unglamorous engineering reasons:

How to spot an EKF in a product spec

You can now reverse-engineer the estimator from marketing copy:

• "Sensor fusion of IMU + GPS / GNSS" with bounded drift → almost certainly an EKF (the measurement is a nonlinear range/pseudorange).
• "9-axis fusion" / "sensor-fusion orientation" (phones, drones, VR controllers) → an EKF (or a close cousin) fusing accel/gyro/mag, whose attitude math is nonlinear.
• "SLAM" on a budget platform → historically EKF-SLAM; modern large-scale SLAM has moved to graphs, but small robots still ship EKFs.
• "Kalman filter" in a product that clearly has angles, distances, or attitudes → it's an EKF; "Kalman filter" is the colloquial name even when the real thing is extended.

Chapter 8: Practical Application — Building and Trusting Your EKF

You know the equations. This recurring "now build it" chapter turns them into a checklist you can run on any real EKF project: how to get the Jacobians right, how to initialize, how to tune, and how to know whether the EKF is even the right tool or whether you should reach for the UKF.

The implementation skeleton

An EKF is shockingly little code once you have f, h, and their Jacobians. The whole thing is the five predict-update equations wrapped in a loop:

python
import numpy as np

def ekf_step(x, P, u, z, Q, R, f, h, F_jac, H_jac):
    # --- PREDICT ---
    x_pred = f(x, u)                       # move mean through the TRUE function
    F = F_jac(x, u)                        # motion Jacobian at current estimate
    P_pred = F @ P @ F.T + Q               # grow covariance (the one approximation)

    # --- UPDATE ---
    y = z - h(x_pred)                      # innovation (use TRUE h to predict z)
    y[1] = wrap_angle(y[1])              # wrap the bearing residual to (-pi, pi]!
    H = H_jac(x_pred)                      # measurement Jacobian at the PREDICTED state
    S = H @ P_pred @ H.T + R               # innovation covariance
    K = P_pred @ H.T @ np.linalg.inv(S)    # Kalman gain
    x_new = x_pred + K @ y                 # corrected mean
    P_new = (np.eye(len(x)) - K @ H) @ P_pred  # corrected covariance (shrinks)
    return x_new, P_new

Notice three things baked into the skeleton: the Jacobians are recomputed every step at the current state (that's "extended"); h is the true function while H is its Jacobian (the two-jobs distinction from Chapter 1); and there is an angle-wrap on the innovation, which we'll see in Chapter 9 is a notorious bug if omitted.

Getting the Jacobians right — analytic vs finite-difference

You have two ways to get F and H, and a professional uses both:

• Analytic (hand-derived). Differentiate f and h on paper, as in Chapter 5. Fast at runtime, exact, but error-prone to derive (sign slips, dropped chain-rule factors). Always finite-difference-check every entry once.
• Numerical (finite-difference). Compute each Jacobian column by nudging one input by ε and measuring the response, at runtime. Slower (one extra function evaluation per state dimension) and slightly less accurate, but impossible to get algebraically wrong. Great for prototyping or when f/h are too gnarly to differentiate by hand.

The pragmatic recipe: derive analytically for speed, but keep a finite-difference function and a unit test that asserts the two agree to a tolerance. The test runs in milliseconds and catches the most expensive class of EKF bug before it ships.

python
def fd_jacobian(h, x, eps=1e-5):
    n = len(x); m = len(h(x))
    J = np.zeros((m, n))
    for j in range(n):
        dx = np.zeros(n); dx[j] = eps
        J[:, j] = (h(x + dx) - h(x - dx)) / (2*eps)   # central difference, more accurate
    return J

# unit test you should always have:
assert np.allclose(H_jac(x), fd_jacobian(h, x), atol=1e-4)

Initialization — where you start decides whether you converge

The showcase hammered this: the EKF linearizes around its current estimate, so a wildly wrong initial estimate produces wildly wrong linearizations and can diverge. Two rules:

• Initialize the mean as well as you can. Use a first GPS fix, a known starting pose, or a coarse global localization to seed x̂. Don't start at the origin and hope.
• Initialize the covariance P honestly large. If you're unsure of the start, make P big — this tells the filter "I'm not confident, trust the measurements." A large initial P lets early measurements pull the estimate toward truth before the linearization can do much damage. The classic blunder is starting with a tiny P (overconfident) around a wrong mean — the filter then ignores the corrective measurements and locks onto its wrong belief.

Tuning Q and R — the two dials that matter

Once it runs, an EKF lives or dies on two covariances: Q (process noise — how much you trust your motion model) and R (measurement noise — how much you trust your sensor). The intuition is the trust slider:

• R too small (overtrust the sensor) → the estimate is jumpy, chasing every noisy measurement.
• R too large (undertrust the sensor) → the estimate is sluggish, ignoring good data, slow to correct drift.
• Q too small (overtrust the model) → the ellipse stays tight, the filter gets overconfident and can ignore measurements — a fast road to divergence if the model is wrong.
• Q too large (undertrust the model) → the ellipse stays fat, the filter leans hard on measurements; safer against divergence but noisier.

R you can often measure (sit the sensor still and compute its variance). Q is usually tuned — and a slightly-too-large Q is a common deliberate choice, because the extra humility guards against the linearization error the EKF inevitably has (you saw this in showcase step 4: raising Q rescued the badly-initialized filter).

When the EKF suffices — and when to reach for the UKF

The deciding question is: how curved is the function across the spread of your uncertainty? A flowchart:

Chapter 9: Debugging & Failure Modes — Why EKFs Diverge

The EKF is the filter most likely to lie to you. Because it approximates, it can be confidently, catastrophically wrong — small ellipse, far from the truth — in a way the linear Kalman filter (which is exact when its assumptions hold) cannot. This recurring chapter catalogs the classic EKF failure modes, the symptom each shows, and the specific test that exposes it. If you remember one thing: a diverging EKF is almost always a Jacobian bug, a bad linearization point, or an angle-wrap bug — in that order of frequency.

Trap 1 — The wrong Jacobian (the #1 EKF bug)

You hand-derived H or F and made a sign error, dropped a chain-rule factor, or transposed a row. The filter still runs — nothing crashes — but the gain pushes corrections in subtly (or wildly) wrong directions, and the estimate drifts off or oscillates. This is the single most common EKF bug, and the most insidious, because the code looks correct and the filter looks like it's "almost working."

Trap 2 — Divergence from a bad linearization point

The Jacobian is correct, but it's being evaluated at an estimate that's far from the truth (bad initialization, or the filter already started slipping). The tangent line is drawn at the wrong place, so it has the wrong slope, so corrections point wrong, so the estimate gets further from truth, so the next linearization is even worse. The error compounds and the filter diverges — runs away and never returns — often while reporting a shrinking covariance, so it looks increasingly confident as it gets increasingly wrong.

The NIS test — an objective consistency check

You don't have to eyeball innovations — there's a number for it. The NIS (Normalized Innovation Squared) checks whether the filter's reported uncertainty is honest:

where y is the innovation and S = H P H^T + R is the innovation covariance. In words: NIS is the innovation, scaled by how big the filter claimed the innovation should be. If the filter's covariance is honest, NIS follows a chi-squared distribution whose average equals the number of measurement dimensions. So for a 2-D range-bearing measurement, a healthy filter has NIS averaging around 2.

• NIS consistently > 2 (much larger): the filter is overconfident — its real errors are bigger than its covariance admits. Innovations keep surprising it more than they should. This is the divergence warning. Fix: increase Q or R.
• NIS consistently < 2 (much smaller): the filter is underconfident — its covariance is too fat, it's not trusting its own data. Wasteful but safe. Fix: decrease Q or R.

Plot NIS over time against the chi-squared confidence bounds. It is the single best objective health monitor for any EKF, and it works in the field with no ground truth — you only need the innovations the filter already computes.

Trap 3 — Angle wraparound (the bug that bites everyone once)

Bearings and headings are angles, and angles wrap: +179° and −179° are 2° apart, not 358° apart. If your predicted bearing is +179° and the measurement is −179°, the naive innovation z − h = −179 − 179 = −358° — a gigantic phantom surprise that yanks the filter violently the wrong way, when the true innovation is just +2°. Every angular residual must be wrapped into (−π, π] before it's used.

Trap 4 — Inconsistency along unobservable directions

The subtlest one. Some directions of your state may be unobservable from your measurements — e.g. a single range measurement tells you distance to a landmark but nothing about which way around the landmark you are. The covariance should stay large in unobservable directions forever. But because the EKF re-linearizes at slightly different points each step, it can "leak" spurious information into those directions and become falsely confident about something it cannot actually know — a known flaw in EKF-SLAM that makes the map artificially crisp and the estimate inconsistent.

A debugging checklist for any EKF

Chapter 10: Connections, References & Cheat-Sheet

You can now fuse a drifting motion source with a curved measurement — the problem that defeats the linear Kalman filter and that nearly every navigating machine on Earth must solve. Before naming what's next, here is everything in one place: the cheat-sheet to screenshot, the motto, the sources, and the cross-links.

The EKF — cheat-sheet

The motto

EKF vs KF vs UKF — one quick-reference table

Screenshot this. It places the EKF among its neighbors:

Where this lesson sits in the series

This was Lesson 7 of 22 — the leap from linear to nonlinear fusion. The recurring chapter structure (Why → concepts → showcase → Use Cases, Practical, Debugging, Connections) repeats every lesson, so you always know where you are. The road ahead:

Next up, Lesson 8: The Unscented Kalman Filter (UKF). The EKF's one weakness is the linearization: a tangent line is a poor model of a sharply curved function across a wide uncertainty. The UKF attacks this differently — instead of differentiating the function (which needs a Jacobian and assumes the curve is locally straight), it samples the function at a handful of cleverly-chosen points (sigma points) and reconstructs the output Gaussian from where those samples land. No Jacobians at all, and it captures curvature the EKF flattens away. You'll see exactly when the extra cost is worth it.

Related lessons on Engineermaxxing

• Lesson 5: The Kalman Filter — the linear engine this lesson extends; the source of the five equations, the Kalman gain trust dial, and the predict/update loop.
• Lesson 8: The Unscented Kalman Filter — the derivative-free alternative for sharply nonlinear systems; where the EKF's tangent-line approximation breaks, the UKF's sigma points hold.
• Extended Kalman Filter (estimation lesson) — a complementary deep-dive on the EKF from the core estimation track, with additional worked examples.
• AA274A: Kalman, EKF & UKF — Stanford's robot-autonomy treatment of all three filters side by side, with the localization framing.

References

1. Simon, D. Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches. Wiley, 2006. The cleanest engineering derivation of the EKF, with the linearization and Jacobian treatment used in this lesson. doi:10.1002/0470045345
2. Thrun, S., Burgard, W., and Fox, D. Probabilistic Robotics. MIT Press, 2005. Chapter 3 presents the EKF as the workhorse of robot localization — the exact range-and-bearing framing of this lesson. probabilistic-robotics.org
3. Kalman, R. E. "A New Approach to Linear Filtering and Prediction Problems." Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45, 1960. The original filter the EKF extends. doi:10.1115/1.3662552
4. Smith, R., Self, M., and Cheeseman, P. "Estimating Uncertain Spatial Relationships in Robotics." Autonomous Robot Vehicles, Springer, pp. 167–193, 1990. The foundation of EKF-SLAM — the EKF with landmarks in the state vector. doi:10.1007/978-1-4613-8997-2_14