Extended Kalman Filter

The extended Kalman filter (EKF) applies the Kalman filter to systems whose motion or measurement model is nonlinear. At each step it replaces the nonlinear functions by first-order Taylor expansions around the current estimate and runs the ordinary Kalman predict and update equations on the result, keeping the Kalman filter’s compact belief, a mean and a covariance, and its low cost. It is probably the most widely used estimator for nonlinear systems (Julier & Uhlmann, 2004), and in computer vision it was the basis of early real-time visual SLAM and of filter-based visual-inertial odometry. Unlike the Kalman filter on a linear-Gaussian model, it is an approximation: not optimal, and liable to diverge.

Problem

Many measurements depend nonlinearly on the state: a camera observes a 3D point through perspective projection, radar and stereo report range and bearing, and inertial measurements must be rotated into the world frame by the current orientation. A Gaussian passed through a nonlinear function is no longer Gaussian, so the exact Bayesian filter has no closed form. The EKF keeps a Gaussian belief anyway and approximates how it propagates.

Inputs and Outputs

Inputs: differentiable transition and measurement functions $f$ and $h$ with their Jacobians; noise covariances $Q$ and $R$; an initial estimate $\hat{\mathbf{x}}_{0 \mid 0}$ with covariance $P_{0 \mid 0}$; and a measurement $\mathbf{z}_k$ at each step, or none.

Outputs, at each step: the estimate $\hat{\mathbf{x}}_{k \mid k}$ and its approximate covariance $P_{k \mid k}$, plus the predicted measurement and innovation covariance used for gating and data association.

Intuition

A smooth function looks linear when examined closely enough. If the uncertainty around the estimate is small, the motion and measurement functions are nearly linear over the region where the state is likely to be, and the Kalman equations apply to that local linear model. The EKF moves the estimate through the true nonlinear functions, and the uncertainty through the Jacobian, which says how a small error in each state component changes the prediction or the measurement. A bearing measurement of a target 100 m away is almost linear in its position over an uncertainty of a meter; the same measurement at 2 m is not.

Algorithm

1. Initialize $\hat{\mathbf{x}}_{0 \mid 0}$ and $P_{0 \mid 0}$.
2. For each time step $k = 1, 2, \ldots$:
   1. Predict. Propagate the estimate through $f$, and the covariance with the Jacobian $F_k$ of $f$ evaluated at the previous estimate.
   2. Update, if a measurement $\mathbf{z}_k$ is available. Compute the innovation with the nonlinear $h$, then the gain and the updated state and covariance with the Jacobian $H_k$ of $h$ evaluated at the predicted state.
   3. Otherwise, keep the prediction as the estimate.

Mathematical Formulation

Nonlinear state-space model

The EKF assumes additive Gaussian noise around nonlinear functions:

$$\mathbf{x}_k = f(\mathbf{x}_{k-1}) + \mathbf{w}_k, \qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, Q)$$ $$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R)$$

The notation follows the Kalman filter: $\hat{\mathbf{x}}_{k \mid k-1}$ is the prediction of $\mathbf{x}_k$ from measurements up to $k-1$, $\hat{\mathbf{x}}_{k \mid k}$ the estimate after the update, and $P_{k \mid k-1}$, $P_{k \mid k}$ their covariances.

Linearization

Expanding $f$ around the previous estimate and $h$ around the prediction to first order,

$$f(\mathbf{x}_{k-1}) \approx f(\hat{\mathbf{x}}_{k-1 \mid k-1}) + F_k \, (\mathbf{x}_{k-1} - \hat{\mathbf{x}}_{k-1 \mid k-1}), \qquad h(\mathbf{x}_k) \approx h(\hat{\mathbf{x}}_{k \mid k-1}) + H_k \, (\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k-1})$$

with the Jacobians

$$F_k = \left. \frac{\partial f}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k-1 \mid k-1}}, \qquad H_k = \left. \frac{\partial h}{\partial \mathbf{x}} \right|_{\hat{\mathbf{x}}_{k \mid k-1}} .$$

The errors then evolve linearly, and the Kalman covariance equations apply with $F_k$ and $H_k$ in place of $F$ and $H$.

Predict

$$\hat{\mathbf{x}}_{k \mid k-1} = f(\hat{\mathbf{x}}_{k-1 \mid k-1})$$ $$P_{k \mid k-1} = F_k P_{k-1 \mid k-1} F_k^\top + Q$$

Update

$$\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k \mid k-1})$$ $$S_k = H_k P_{k \mid k-1} H_k^\top + R$$ $$K_k = P_{k \mid k-1} H_k^\top S_k^{-1}$$ $$\hat{\mathbf{x}}_{k \mid k} = \hat{\mathbf{x}}_{k \mid k-1} + K_k \mathbf{y}_k$$ $$P_{k \mid k} = (I - K_k H_k) P_{k \mid k-1}$$

Two things differ from the Kalman filter: the state and the predicted measurement go through the nonlinear $f$ and $h$, and the Jacobians are re-evaluated at every step, so the gain and covariance depend on the estimate and hence on the data. When noise enters nonlinearly, as in $\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{w}_k)$, its covariance is also mapped through the Jacobian with respect to the noise, $L_k Q L_k^\top$.

Example: range and bearing

A sensor at the origin measuring the range and bearing of a target at $(p_x, p_y)$, with the constant-velocity state $\mathbf{x} = (p_x, p_y, v_x, v_y)^\top$, has

$$h(\mathbf{x}) = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \\ \operatorname{atan2}(p_y, p_x) \end{bmatrix}, \qquad H_k = \begin{bmatrix} p_x / r & p_y / r & 0 & 0 \\ -p_y / r^2 & p_x / r^2 & 0 & 0 \end{bmatrix}$$

with $r = \sqrt{p_x^2 + p_y^2}$ and the entries evaluated at the predicted position. The bearing row scales as $1/r$, so near the sensor the linear approximation holds over a smaller region. The motion model here is linear, so $F_k = F$; a coordinated-turn model or orientation integration would make $F_k$ state-dependent too.

Parameters

• $Q$ and $R$. As in the Kalman filter, they balance trust in the model against the measurements. In an EKF they also absorb linearization error, and inflating them slightly beyond the physical noise is a common remedy for overconfidence.
• Initial estimate and covariance. More important than in the linear case, because the first Jacobians are evaluated at $\hat{\mathbf{x}}_{0 \mid 0}$; a poor guess, or a covariance too small to cover its error, can lead to a wrong solution.
• State parameterization. Cartesian versus polar coordinates, or the choice of orientation representation, change how nonlinear $f$ and $h$ are, and therefore how good the approximation is.

Complexity

The cost per step is that of the Kalman filter: $O(n^3)$ for the covariance prediction and $O(n^2 m + n m^2 + m^3)$ for the update with state dimension $n$ and measurement dimension $m$, plus evaluating $f$, $h$, and their Jacobians. Memory is $O(n^2)$. In EKF-based SLAM the state contains every landmark and the covariance is dense, so each update costs $O(n^2)$ even when only a few landmarks are observed; this quadratic growth limits such systems to modest map sizes.

Implementation

A target moves at nearly constant velocity; a sensor at the origin reports range (standard deviation 0.5 m) and bearing (2°), which at the starting distance of 100 m means about 3.5 m of cross-range error. The baseline converts each raw measurement to Cartesian coordinates.

import numpy as np

dt = 1.0
q = 0.01                          # acceleration variance per axis
sig_r, sig_b = 0.5, np.deg2rad(2.0)   # range (m) and bearing (rad) noise

# Linear constant-velocity motion, state x = [px, py, vx, vy].
F = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1,  0],
              [0, 0, 0,  1]], dtype=float)
G = np.array([[dt**2 / 2, 0], [0, dt**2 / 2], [dt, 0], [0, dt]])
Q = q * G @ G.T
R = np.diag([sig_r**2, sig_b**2])

def h(x):
    """Range and bearing of the target seen from a sensor at the origin."""
    px, py = x[0], x[1]
    return np.array([np.hypot(px, py), np.arctan2(py, px)])

def H_jac(x):
    """Jacobian of h, evaluated at the predicted state."""
    px, py = x[0], x[1]
    r2 = px**2 + py**2
    r = np.sqrt(r2)
    return np.array([[ px / r,  py / r, 0, 0],
                     [-py / r2, px / r2, 0, 0]])

def wrap(a):
    return (a + np.pi) % (2 * np.pi) - np.pi

def ekf_step(x, P, z):
    # Predict (f is linear here, so F_k = F).
    x = F @ x
    P = F @ P @ F.T + Q
    # Update with the linearized measurement model.
    Hk = H_jac(x)
    y = z - h(x)
    y[1] = wrap(y[1])                    # bearing innovation in (-pi, pi]
    S = Hk @ P @ Hk.T + R
    K = np.linalg.solve(S, Hk @ P).T     # P H^T S^-1 (S, P symmetric)
    x = x + K @ y
    I_KH = np.eye(4) - K @ Hk
    P = I_KH @ P @ I_KH.T + K @ R @ K.T  # Joseph form
    return x, P, y @ np.linalg.solve(S, y)

rng = np.random.default_rng(1)
truth = np.array([80.0, -60.0, -1.0, 2.0])     # starts 100 m away
z0 = h(truth) + rng.normal(0, [sig_r, sig_b])
x = np.array([z0[0] * np.cos(z0[1]), z0[0] * np.sin(z0[1]), 0.0, 0.0])
P = np.diag([16.0, 16.0, 4.0, 4.0])      # bearing error is ~3.5 m at 100 m

raw_err, ekf_err, nis = [], [], []
for k in range(60):
    truth = F @ truth + G @ rng.normal(0, np.sqrt(q), 2)
    z = h(truth) + rng.normal(0, [sig_r, sig_b])
    x, P, e = ekf_step(x, P, z)
    raw = np.array([z[0] * np.cos(z[1]), z[0] * np.sin(z[1])])
    raw_err.append(np.linalg.norm(raw - truth[:2]))
    ekf_err.append(np.linalg.norm(x[:2] - truth[:2]))
    nis.append(e)

print(f"mean position error, raw (range, bearing) -> (x, y): {np.mean(raw_err[10:]):.2f} m")
print(f"mean position error, EKF:                           {np.mean(ekf_err[10:]):.2f} m")
print(f"mean normalized innovation squared (expect ~2):     {np.mean(nis[10:]):.2f}")
print("final velocity estimate:", np.round(x[2:], 2), " true:", np.round(truth[2:], 2))

Output:

mean position error, raw (range, bearing) -> (x, y): 1.20 m
mean position error, EKF:                           0.77 m
mean normalized innovation squared (expect ~2):     1.90
final velocity estimate: [-2.14  2.04]  true: [-2.27  1.95]

The EKF reduces the mean position error from 1.20 m to 0.77 m and recovers the unmeasured velocity. The mean normalized innovation squared is close to 2, the measurement dimension, so the reported covariance agrees with the actual errors. Wrapping the bearing innovation to $(-\pi, \pi]$ prevents a spurious innovation of nearly $2\pi$ when the target crosses the $\pm\pi$ boundary, and the Joseph-form update keeps $P$ symmetric and positive semi-definite.

Properties and Behavior

• Not optimal. The EKF computes the exact Kalman update for an approximate model. Its estimate is in general neither the posterior mean nor the mode, and its covariance only approximates the true error covariance. For a polar-to-Cartesian conversion, Julier and Uhlmann (2004) showed that the linearized mean is biased and the variance underestimated.
• Divergence. Because the Jacobians depend on the estimate, an error in the estimate produces a wrong gain. If linearization errors are large compared with the assumed noise, the reported covariance shrinks while the actual error grows, and the filter discounts new measurements and can drift away without recovering. Julier and Uhlmann (2004) summarize decades of experience: the EKF is reliable mainly for systems that are nearly linear over the interval between updates.
• Inconsistency. A filter is consistent when its covariance matches its actual errors. Julier and Uhlmann (2001) proved that full-covariance EKF SLAM always produces an inconsistent map for a stationary vehicle with no process noise, a range-bearing sensor, and nonzero angular uncertainty. Simulations suggested the same for moving vehicles, with the inconsistency apparent only after several hundred updates.

Limitations

• Strong nonlinearity. Large uncertainty relative to the curvature of $f$ or $h$, such as a newly initialized feature with almost unknown depth, breaks the first-order approximation.
• Non-Gaussian and multimodal beliefs. One Gaussian cannot represent ambiguous measurements or a state that may lie in one of several regions; multiple-hypothesis methods or a particle filter are needed.
• Jacobians. Analytic Jacobians of realistic models are long and error-prone, and a wrong one degrades the filter in ways that are hard to diagnose (Julier & Uhlmann, 2004); checking them against finite differences, or using automatic differentiation, is standard practice. Jacobians do not exist at discontinuities or singularities, such as a point at the camera center in perspective projection.
• Orientation. Rotations do not form a vector space; adding a correction to Euler angles or a quaternion breaks constraints or hits singularities, which motivates the error-state formulation.

Variants

The iterated extended Kalman filter repeats the update, relinearizing $h$ at the newly updated estimate until it stops changing. Bell and Cathey (1993) showed that this is the Gauss–Newton method for approximating a maximum likelihood estimate that combines the prediction and the new measurement, which connects the EKF to nonlinear least squares. In their example, the iterated update converged correctly as the measurement became more accurate, while the standard update did not.

The error-state (or indirect) EKF estimates a small error relative to a nominal state, which is propagated separately by the full nonlinear model, typically by integrating inertial measurements. The error stays small and nearly linear, orientation errors use a minimal three-parameter rotation vector, and after each update the error is folded into the nominal state and reset to zero. Filter-based visual-inertial systems commonly use this form, among them the multi-state constraint Kalman filter (Mourikis & Roumeliotis, 2007).

The unscented Kalman filter avoids Jacobians: it passes a small, deterministically chosen set of sigma points, $2n$ or $2n + 1$ for an $n$-dimensional state, through the nonlinear functions and computes the mean and covariance of the results. Julier and Uhlmann (2004) showed that this captures the transformed mean and covariance correctly to second order, implicitly including the second-order bias correction that linearization omits, at the same order of cost as the EKF. With their basic point set, the gain in their polar-to-Cartesian example was mainly in the mean; the covariance was of similar accuracy to linearization.

The extended information filter propagates the inverse covariance instead, which makes measurement updates additive and suits multi-sensor fusion.

History

The EKF was developed at NASA’s Ames Research Center soon after Kalman’s 1960 paper. According to McGee and Schmidt (1985), Stanley Schmidt’s group, studying midcourse navigation for a circumlunar mission, first applied the filter to equations linearized about a nominal trajectory and soon relinearized about the current estimate instead: the filter now called the extended Kalman filter. The work fed into Apollo navigation; the article on Kalman’s paper covers this history.

Uses in Computer Vision

• Visual SLAM. MonoSLAM (Davison et al., 2007) estimated the pose and velocity of a single moving camera and a sparse map of 3D landmarks in one EKF, at 30 Hz on standard hardware.
• Visual-inertial odometry. The multi-state constraint Kalman filter (MSCKF) of Mourikis and Roumeliotis (2007) is an EKF that keeps a window of recent camera poses in its state. Its measurement model expresses the constraints between all poses that observed a feature without adding the feature’s 3D position to the state, making the cost linear in the number of features.
• Range and bearing tracking. Targets observed in polar coordinates by radar, sonar, stereo, or LiDAR, and bearing-only tracking with a single camera, where range must be inferred from motion.
• Object tracking. In object tracking, an EKF is used when the state is 3D but the detections are 2D image positions, related by perspective projection.