Extended Kalman Filter: How It Works, Step by Step

The extended Kalman filter (EKF) is the standard way to run a Kalman filter on a system whose motion or sensor model isn't linear. It works by re-linearizing that nonlinear model at every time step — using a first-order Taylor expansion around the current estimate — and then running the ordinary linear Kalman filter equations on the result. That trick makes it the workhorse behind GPS/INS navigation, robot localization, and radar or LiDAR tracking, but it comes with sharp edges: it can diverge on strongly nonlinear systems, and every motion or measurement model needs a Jacobian matrix computed correctly at every step. This guide goes through the math step by step, works a compact numerical example, and shows where the derivative-free unscented Kalman filter (UKF) is the better tool.

If you haven't seen the plain (linear) Kalman filter yet, our sensor fusion algorithms guide covers it and where the Kalman family sits alongside Bayesian and deep-learning approaches. This article picks up from there and stays entirely on the EKF and its nonlinear alternatives.

How the EKF works: linearize, then run the Kalman equations

The plain Kalman filter assumes both the motion model and the measurement model are linear: the next state is a matrix times the previous state, and the measurement is a matrix times the state. Most real systems break that assumption immediately. A ground vehicle's heading update involves sines and cosines. A radar or sonar reports range and bearing in polar coordinates while the state you actually want — position and velocity — lives in Cartesian coordinates. An inertial measurement unit's orientation update, the kind we cover in our guide to IMU sensors, is a nonlinear rotation, not a matrix multiply.

Write the general nonlinear system as a state transition function f and a measurement function h:

x_k = f(x_k−1, u_k) + w_k
z_k = h(x_k) + v_k

The EKF's entire idea is to replace the constant matrices of the linear filter with fresh, locally linear approximations of f and h at every step — the first term of a Taylor expansion around the current best estimate. Those local approximations are the Jacobian matrices F_k and H_k, and everything else is exactly the linear Kalman filter's predict/update cycle, as documented in the DSTL/Alan Turing Institute's open-source Stone Soup tracking framework:

Predict
x̂_k|k−1 = f(x̂_k−1|k−1, u_k)
P_k|k−1 = F_k P_k−1|k−1 F_k^T + Q_k

Update
K_k = P_k|k−1 H_k^T (H_k P_k|k−1 H_k^T + R_k)⁻¹
x̂_k|k = x̂_k|k−1 + K_k (z_k − h(x̂_k|k−1))
P_k|k = (I − K_k H_k) P_k|k−1

Q_k and R_k are the process and measurement noise covariances, same as in the linear filter. The only new ingredients are F_k = ∂f/∂x, evaluated at x̂_k−1|k−1, and H_k = ∂h/∂x, evaluated at x̂_k|k−1: the Jacobians of the motion and measurement functions, recomputed at every single step because the linearization point moves as the estimate moves.

The Jacobians: why they're the hard part

A Jacobian is just the matrix of first partial derivatives of a vector-valued function — for a measurement function h(x) with one output and two state variables, H is a 1×2 row vector of ∂h/∂x₁ and ∂h/∂x₂. Deriving it correctly is a purely mechanical calculus exercise, but "mechanical" doesn't mean "safe." In their widely cited 2004 review of the EKF and its alternatives, Simon Julier and Jeffrey Uhlmann note that Jacobian equations "frequently produce many pages of dense algebra that must be converted to code," and that this is a real source of bugs precisely because it's hard to know, at runtime, whether a given filter's poor performance comes from a wrong Jacobian or from something else entirely.

There's a practical way around hand-deriving the Jacobian symbolically: approximate it numerically with finite differences — perturb each state variable slightly, re-evaluate f or h, and divide by the perturbation. The Stone Soup framework linked above uses exactly this approach for its EKF implementation, trading a small amount of numerical accuracy for the ability to plug in almost any differentiable model without deriving anything by hand. It's a reasonable default when the analytic Jacobian is unwieldy or when you're prototyping; a hand-derived analytic Jacobian is still faster and more numerically precise once the model is stable.

A worked example: tracking position from a range-only sensor

Here's the mechanism end to end on a single time step. All the numbers below are worked by hand from the equations above to illustrate the mechanics — they are not measurements from a real sensor or a real deployment.

A target moves along a straight line at roughly constant velocity. A range-only sensor sits 20 m to the side of that line and reports the straight-line distance to the target — not its position directly, which is exactly the kind of nonlinearity that forces you off the plain Kalman filter (the same setup shows up in bearing/range tracking from radar, covered in our LiDAR vs radar comparison).

State x = [position p, velocity v]. Starting estimate x̂ = [30 m, 2 m/s], with covariance P = [[4, 0], [0, 0.25]] (a position uncertainty of 2 m and a velocity uncertainty of 0.5 m/s, one standard deviation). The motion model is linear here — constant velocity over a 1 s step — so F = [[1, 1], [0, 1]] and no motion Jacobian is needed; only the sensor is nonlinear.

Predict (Δt = 1 s, process noise Q = diag(0.1, 0.01)):
x̂_k|k−1 = [32 m, 2 m/s]
P_k|k−1 ≈ [[4.35, 0.25], [0.25, 0.26]]

Linearize the sensor at the predicted state. With a lateral offset d = 20 m, the range model is h(p) = √(p² + d²). At p = 32 m, h(32) ≈ 37.74 m, and the Jacobian is H = [∂h/∂p, 0] = [p/√(p²+d²), 0] ≈ [0.848, 0] — the velocity component drops out because range doesn't depend on velocity directly.

Update with a measurement z = 38.0 m and measurement noise R = 1.0 m²:
Innovation y = z − h(x̂_k|k−1) ≈ 0.26 m
Innovation covariance S ≈ 4.13 m²
Kalman gain K ≈ [0.89, 0.05]
x̂_k|k ≈ [32.24 m, 2.01 m/s]
P_k|k ≈ [[1.05, 0.06], [0.06, 0.25]]

Notice what the gain does: because the sensor's Jacobian has a much larger entry for position than for velocity, most of the correction lands on the position estimate (+0.24 m) and almost none on velocity (+0.01 m/s) — the filter only nudges the states the measurement actually informs. Notice also that the position variance dropped sharply, from 4.35 to about 1.05: a single informative range reading collapsed most of the uncertainty the prediction step had added.

Where the EKF breaks down

The EKF's weaknesses aren't edge cases — they're the reason alternatives like the UKF exist at all. Julier and Uhlmann summarize three of them directly:

• Divergence on strongly nonlinear systems. The linear approximation is only reliable if the true function is well approximated by a straight line over the span of the current uncertainty. If it isn't, the estimate degrades — and at worst the filter's covariance and state estimate diverge from reality altogether, with no built-in signal that it's happening.
• The Jacobian has to exist. Some models are discontinuous (a sensor that jumps between quantization levels, a mode that switches abruptly) or have singularities (perspective projection is a commonly cited example), and a Jacobian simply isn't defined at those points.
• Deriving it correctly is error-prone, as covered above — and a wrong Jacobian doesn't usually throw an error, it just quietly produces a worse filter.

Julier and Uhlmann also make a broader observation worth carrying into any EKF project: by the time they published their review in 2004, the EKF already had more than 35 years of accumulated use behind it, and their conclusion from that experience was that it is "difficult to implement, difficult to tune, and only reliable for systems that are almost linear on the time scale of the updates." That's not a reason to avoid it — it remains the most widely used nonlinear estimator for good reasons of simplicity and speed — but it's a reason to test it against ground truth before trusting it in a strongly nonlinear regime.

The unscented Kalman filter: a derivative-free alternative

The UKF, introduced by Julier and Uhlmann, sidesteps the Jacobian problem entirely by not linearizing the function at all. Instead of approximating f or h with a straight line, it picks a small, deterministic set of sample points — "sigma points" — chosen so that their weighted mean and covariance exactly match the current state estimate's mean and covariance. It then passes each sigma point through the true, unmodified nonlinear function and reconstructs the mean and covariance of the outputs from the transformed points. No derivative is computed anywhere in the process.

The accuracy gain is not just qualitative. Julier and Uhlmann show that this "unscented transformation" captures the posterior mean and covariance correctly to the second order of a Taylor series expansion, compared to the EKF's first-order accuracy from linearization — and it does so "with the same order of calculations as linearization," so the extra accuracy isn't paid for with a proportionally heavier computational budget. The trade-off is a different kind of tuning: the UKF has its own spread and scaling parameters that control how far the sigma points sit from the mean, and getting those wrong causes its own (different) estimation problems.

EKF vs UKF at a glance

Implementing an EKF in Python

The open-source FilterPy library implements the same predict/update structure used above as an ExtendedKalmanFilter class. You set the dimensions, the initial state and covariance, and the noise matrices, then supply your own functions for the measurement Jacobian and the measurement prediction:

from filterpy.kalman import ExtendedKalmanFilter
import numpy as np

ekf = ExtendedKalmanFilter(dim_x=2, dim_z=1)
ekf.x = np.array([30.0, 2.0])          # [position, velocity]
ekf.F = np.array([[1., 1.], [0., 1.]]) # constant-velocity motion model
ekf.P *= np.array([[4., 0.], [0., 0.25]])
ekf.Q = np.diag([0.1, 0.01])
ekf.R = np.array([[1.0]])

d = 20.0  # sensor's lateral offset from the target's path

def HJacobian(x):
    p = x[0]
    r = np.sqrt(p**2 + d**2)
    return np.array([[p / r, 0.0]])

def Hx(x):
    p = x[0]
    return np.array([np.sqrt(p**2 + d**2)])

ekf.predict()
ekf.update(np.array([38.0]), HJacobian, Hx)
print(ekf.x, ekf.P)

That's the same worked example from above, run through the library instead of by hand — predict() applies F and Q, and update() calls HJacobian and Hx internally to build H, the innovation, and the Kalman gain. For production tracking rather than a single script, frameworks like Stone Soup (linked earlier) already wire up EKF, UKF, and particle filters behind a common interface, which is usually less work than maintaining your own filter implementation — including for embedded deployments on boards like the Jetson Orin Nano, where the EKF's low computational cost compared to a particle filter is often the deciding factor.

Frequently asked questions about the extended Kalman filter

What is the difference between an extended Kalman filter and a standard Kalman filter?

The standard (linear) Kalman filter assumes both the motion model and the measurement model are linear functions of the state. The EKF handles nonlinear versions of those functions by linearizing them at every step with a first-order Taylor expansion, using Jacobian matrices in place of the standard filter's constant matrices. Everything else in the predict/update cycle is identical.

What is the difference between the EKF and the UKF?

The EKF approximates a nonlinear function with a straight line (its Jacobian) at the current estimate; the UKF instead pushes a small set of sample points through the exact, unmodified nonlinear function. That gives the UKF second-order accuracy against the EKF's first-order accuracy, and it needs no derivatives at all — at the cost of its own tuning parameters for how the sample points are spread.

Why do we need a Kalman-family filter instead of just using the raw sensor reading?

A single sensor reading is a noisy, incomplete snapshot of the state. A Kalman-family filter combines that reading with a model of how the state evolves over time and with the uncertainty of both the model and the sensor, producing an estimate that is provably better (in a least-squares sense, for the linear case) than trusting either the model or the sensor alone — which is also the core idea behind sensor fusion more broadly.

Does the EKF guarantee that its estimate will converge?

No. Convergence depends on how well the true nonlinear function is approximated by its linearization over the span of the current uncertainty. For strongly nonlinear systems, or when the initial estimate is far from the truth, the EKF's covariance can become inconsistent with its actual error and the filter can diverge, with no built-in warning that it has happened.

How is the Jacobian matrix calculated in practice?

Two ways: derive it analytically by differentiating the motion or measurement function symbolically and coding the result directly, or approximate it numerically with finite differences (perturb each state variable slightly and see how the function output changes). The analytic route is faster and more precise once verified; the numerical route, used by tracking frameworks like Stone Soup, avoids hand-deriving algebra for every new model at the cost of a small amount of accuracy.