Extended Kalman Filter (EKF)

The extended Kalman filter (EKF) is a recursive algorithm that estimates the state of a system whose motion or measurements are nonlinear, such as a drone's orientation or a target's position seen by a radar that reports range and bearing. The standard Kalman filter (KF) is exact only for linear models with Gaussian noise. The EKF handles nonlinear models by linearizing them around the current estimate at every step with Jacobian matrices of first derivatives, then applying the familiar predict and update equations. Gerald Smith, Stanley Schmidt, and Leonard McGee at NASA Ames developed it in the early 1960s for onboard navigation of a circumlunar vehicle, work that fed into the Apollo navigation computer, and it remains the de facto standard for nonlinear state estimation in navigation systems, GPS receivers, drones, and robots.

How the EKF Works#

Like the Kalman filter, the EKF keeps a state estimate x and a covariance P that describes its uncertainty, but the motion model f and the measurement model h can be any differentiable functions. Each step has two phases:

1. Predict: The estimate passes through the nonlinear motion model, x = f(x). The filter evaluates the Jacobian F = ∂f/∂x at the estimate and grows the uncertainty with the process noise Q: P = F P Fᵀ + Q.
2. Update: A measurement z arrives. The filter evaluates the measurement Jacobian H = ∂h/∂x at the predicted state, computes the residual y = z − h(x) and the gain K = P Hᵀ (H P Hᵀ + R)⁻¹, where R is the measurement noise, and corrects the estimate with x = x + K y and P = (I − K H) P.

The state and the predicted measurement go through the true nonlinear functions; only the covariance and the gain use the linearized Jacobians. When f and h are linear, the Jacobians equal the model matrices and the EKF reduces exactly to the Kalman filter. The approximation is good when the models are close to linear across the spread of the uncertainty and degrades when they curve sharply within it.

A radar that measures range and bearing is the classic case: a target moving in a straight line in x and y is linear in those coordinates, but range √(x² + y²) and bearing atan2(y, x) are not. This EKF tracks such a target and estimates its velocity, which the radar never measures:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Converting each raw range and bearing to x and y gives a mean position error of 1.43 m over 60 steps, while the EKF estimate is off by 0.57 m, and the filter recovers the velocity as (1.01, −0.50) m/s.

Limitations and Variants#

The EKF is not an optimal estimator. If the initial estimate is far off or the models are strongly nonlinear, linearization errors can make the filter overconfident and then diverge, so engineers often inflate the process noise for safety. The Jacobians must be derived by hand or with automatic differentiation, and a wrong derivative is a common source of silent failures. Residuals of angles such as bearing or heading also need wrapping to the range from −π to π. Several variants address these problems: the iterated EKF relinearizes the measurement model during the update, and the error-state EKF estimates a small correction around a nominal state, a common way to track 3D orientation with quaternions in inertial navigation.

EKF vs. Other Filters#

The choice among filters depends on how far the models are from linear and on the shape of the uncertainty:

• Kalman filter: Optimal and cheaper when both models are linear. Bounding-box trackers such as ByteTrack measure box coordinates directly, so a linear filter is enough for them.
• Unscented Kalman filter (UKF): Passes a small, deterministic set of sigma points through the nonlinear functions instead of linearizing them, needs no Jacobians, and is usually more accurate than the EKF for strongly curved models.
• Particle filter: Represents the belief with many weighted random samples, so it can track multimodal distributions, such as a robot unsure which of two hallways it is in, at a much higher compute cost.
• Hidden Markov model (HMM): Applies the same predict-and-update recursion to a finite set of discrete states.

Real-World Applications#

• Drones and aircraft: The PX4 autopilot's EKF2 estimator runs a 24-state EKF that fuses IMU, GPS, magnetometer, barometer, and other sensor data into estimates of orientation, velocity, position, sensor biases, Earth's magnetic field, and wind.
• Robotics: The ROS robot_localization package provides EKF and UKF nodes that fuse wheel odometry, IMUs, and GPS, and EKF-SLAM is the classic approach to simultaneous localization and mapping. EKF-based visual-inertial odometry, such as the multi-state constraint Kalman filter (MSCKF), tracks camera pose from image features and IMU readings.
• Automotive sensor fusion: Driver assistance and autonomous vehicle systems fuse radar range and bearing with camera and lidar detections to track the position and velocity of surrounding cars.
• Camera-based localization: A single camera measures only the direction to an object. Pipelines that place detected objects on a map use Ultralytics YOLO26 detections as EKF measurements: each box center is a pixel position, a nonlinear projection of the object's 3D position through the camera model, which the filter fuses with radar range or the camera platform's GPS and IMU data.