Extended Kalman Filter for Heading-Aware GPS Smoothing

The linear filter described in Kalman filtering for GPS noise reduction tracks position and velocity components, and it works well on straight roads. Its weakness is corners: a constant-velocity model has no notion of turning, so a genuine turn is an unmodelled acceleration that the filter smooths through. The result is a path that cuts the corner, and at low sampling rates it cuts it by enough to matter.

A constant-turn-rate-and-velocity model fixes this by putting heading and turn rate in the state. The motion model is then non-linear, which is what makes it an extended Kalman filter rather than a linear one. This page covers the state design, the Jacobian, the angle wrapping that catches everyone, and the honest answer about when the extra complexity is worth it.

Compatibility and Configuration Requirements

The State and the Model

The state is five numbers: position, speed, heading and turn rate.

x = [px, py, v, psi, psi_dot]

Propagation over Δt, for a non-zero turn rate:

px' = px + (v / psi_dot) * ( sin(psi + psi_dot*dt) - sin(psi) )
py' = py + (v / psi_dot) * ( cos(psi) - cos(psi + psi_dot*dt) )
v'  = v
psi' = psi + psi_dot * dt
psi_dot' = psi_dot

from __future__ import annotations

import numpy as np

EPS = 1e-4          # turn rates below this are treated as straight-line motion


def wrap(a: np.ndarray | float):
    """Normalise an angle or array of angles into (-pi, pi]."""
    return (a + np.pi) % (2 * np.pi) - np.pi


def propagate(x: np.ndarray, dt: float) -> np.ndarray:
    px, py, v, psi, w = x
    if abs(w) < EPS:                       # straight line: the general form divides by zero
        return np.array([px + v * dt * np.cos(psi),
                         py + v * dt * np.sin(psi),
                         v, wrap(psi), w])
    return np.array([
        px + (v / w) * (np.sin(psi + w * dt) - np.sin(psi)),
        py + (v / w) * (np.cos(psi) - np.cos(psi + w * dt)),
        v,
        wrap(psi + w * dt),
        w,
    ])


def jacobian(x: np.ndarray, dt: float) -> np.ndarray:
    """Partial derivatives of `propagate` with respect to the state."""
    px, py, v, psi, w = x
    F = np.eye(5)
    F[3, 4] = dt
    if abs(w) < EPS:
        F[0, 2] = dt * np.cos(psi); F[0, 3] = -v * dt * np.sin(psi)
        F[1, 2] = dt * np.sin(psi); F[1, 3] = v * dt * np.cos(psi)
        return F
    s0, s1 = np.sin(psi), np.sin(psi + w * dt)
    c0, c1 = np.cos(psi), np.cos(psi + w * dt)
    F[0, 2] = (s1 - s0) / w
    F[0, 3] = (v / w) * (c1 - c0)
    F[0, 4] = (v * dt / w) * c1 - (v / w**2) * (s1 - s0)
    F[1, 2] = (c0 - c1) / w
    F[1, 3] = (v / w) * (s1 - s0)
    F[1, 4] = (v * dt / w) * s1 - (v / w**2) * (c0 - c1)
    return F

The abs(w) < EPS branch is not an optimisation. The general form divides by the turn rate, and a vehicle driving straight has a turn rate that is numerically indistinguishable from zero — so without the branch the filter produces infinities on every motorway.

Angle Wrapping

The single most common EKF bug in this application is the heading residual. When the measured course is 359 degrees and the predicted heading is 1 degree, the naive difference is 358 degrees, and the filter applies an enormous correction that spins the state.

Wrap every angular difference before it enters the update:

def update(x, P, z, H, R, angle_idx: int | None = 3):
    """Standard EKF update, with the angular residual wrapped."""
    y = z - H @ x
    if angle_idx is not None:
        y[angle_idx] = wrap(y[angle_idx])
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x_new = x + K @ y
    x_new[3] = wrap(x_new[3])
    P_new = (np.eye(len(x)) - K @ H) @ P
    return x_new, P_new

Two wraps, not one. The residual is wrapped so the correction is the short way round, and the updated state is wrapped so the heading stays in range rather than accumulating past pi over a long sequence of left turns.

The symptom of a missing wrap is characteristic: the filter behaves perfectly for most of a trace and then produces a violent excursion at one point, always where the heading crossed north. It is easy to mistake for a GPS spike, which is why it survives so long in so many implementations.

Tuning the Turn-Rate Noise

The CTRV filter has one more tuning parameter than the linear one, and it is the one that decides the filter’s character. The turn-rate process noise says how quickly the vehicle’s rate of turn can change.

Too small and the filter believes the turn rate is nearly constant, so it enters a corner late and leaves it late — overshooting on both sides. The trace looks smooth and follows a path the vehicle did not take.

Too large and the turn rate tracks measurement noise, so the filtered heading oscillates and the position wobbles along a straight road.

The physically motivated starting point is the maximum angular acceleration the vehicle class can produce. A van can change its turn rate by roughly 0.5 rad/s²; an articulated truck by considerably less. Setting the process noise standard deviation to that value over the sampling interval gives a filter that can follow anything the vehicle can physically do and nothing more.

def Q_ctrv(dt: float, sigma_a: float = 0.6, sigma_alpha: float = 0.4) -> np.ndarray:
    """Process noise for linear acceleration and angular acceleration."""
    Q = np.zeros((5, 5))
    Q[2, 2] = (sigma_a * dt) ** 2          # speed
    Q[3, 3] = (0.5 * sigma_alpha * dt**2) ** 2   # heading
    Q[4, 4] = (sigma_alpha * dt) ** 2      # turn rate
    return Q

Validate by measuring, not by looking. Take a corpus of high-rate traces, downsample, filter, and measure the positional error at corners specifically — the mean over a whole trace is dominated by straight sections where every filter agrees.

When It Is Worth It

The EKF costs more than the linear filter in three ways: more code, more tuning parameters, and a failure mode at zero speed that the linear filter does not have. It is worth paying when corners matter and the sampling interval is long.

At 1 Hz, a constant-velocity filter cuts a typical urban corner by two to four metres, which is comfortably inside the receiver’s own error and changes no downstream answer. At 10 seconds it cuts by ten to twenty; at 30 seconds by tens of metres, which is enough to put a matched path on the wrong arm of a junction.

The zero-speed failure deserves care. Below about a metre per second the turn rate is unobservable and the heading state will drift freely, producing a parked vehicle that slowly rotates. Freeze both states below the threshold:

if x[2] < 1.0:
    x[4] = 0.0                       # turn rate
    P[3:5, :] = P[:, 3:5] = 0.0      # and stop their covariance growing

Common Pitfalls Specific to This Technique

No small-turn-rate branch. Division by a turn rate that is numerically zero on every straight road, producing infinities the first time the filter meets a motorway.

Unwrapped heading residual. Works for months and then produces one violent excursion, always at a north crossing, and always mistaken for a GPS spike.

Filtering in degrees. The motion model is Cartesian and the turn rate is in radians per second; mixing units gives a filter that appears tuned and is not.

Leaving heading unconstrained at rest. A parked vehicle rotates in the output, which then produces spurious heading changes for anything reading the smoothed course.

Related

• Kalman filtering for GPS noise reduction — parent topic and the linear filter this extends
• Adaptive Kalman tuning for mixed fleet dynamics — setting process noise per vehicle class
• Directionality and heading synchronisation — the circular arithmetic this filter depends on
• Correcting heading drift using accelerometer fusion — an alternative when an IMU is available
• GPS Data Preprocessing & Cleaning Fundamentals — parent section