Kalman Filter Smoothing for Noisy Location Streams

Keeping a rolling window of past fixes to smooth a track is the obvious way to reduce noise and the wrong way to do it at fleet scale: a 60-sample window per device turns 9.6 MB of per-device state into 580 MB, and the mean it computes lags reality by half the window. A constant-velocity Kalman filter achieves better smoothing in 96 bytes with no history at all, because it carries a state estimate rather than a sample buffer. This page sits under dead reckoning and trajectory interpolation within Core Architecture & Latency Constraints, and it also sets out the filter’s most important limitation: the smoothed position is systematically wrong during a manoeuvre, which makes it unsafe as the sole input to a boundary decision.

Concept and specification #

The state vector is position and velocity in a local metric frame, $\mathbf{x}=[x,y,v_x,v_y{]}^{\top }$, with covariance $\mathbf{P}$. Each cycle predicts forward by $\Delta t$ and then corrects with the measurement:

$${\mathbf{x}}^{-}=\mathbf{Fx},{\mathbf{P}}^{-}=\mathbf{FP}{\mathbf{F}}^{\top }+\mathbf{Q},\mathbf{K}={\mathbf{P}}^{-}{\mathbf{H}}^{\top }(\mathbf{H}{\mathbf{P}}^{-}{\mathbf{H}}^{\top }+\mathbf{R}{)}^{-1}$$

with $\mathbf{F}$ the constant-velocity transition, $\mathbf{H}$ selecting position from the state, $\mathbf{R}$ the measurement covariance and $\mathbf{Q}$ the process noise. Two of those matrices are the whole engineering problem, and the rest is arithmetic.

$\mathbf{R}$ comes free: the receiver reports an accuracy per fix, so $\mathbf{R}={\sigma }_{\text{reported}}^2\mathbf{I}$ gives the filter exactly the information it needs to weight a poor fix less. Filters that hard-code $\mathbf{R}$ throw away the single most useful signal in the stream and behave identically on a 3 m RTK fix and a 400 m Wi-Fi fix.

$\mathbf{Q}$ is the tuning parameter, and it encodes how much the model is allowed to be wrong — physically, the acceleration the vehicle may undergo between fixes. Too small and the filter trusts its constant-velocity model, smooths beautifully on a straight road and overshoots every corner; too large and it tracks the noise.

The innovation gate deserves its own mention because it is where the filter earns most of its value in practice. The innovation is the difference between the measurement and the prediction, normalised by their combined covariance; a fix whose normalised innovation exceeds a few sigma is inconsistent with the physics and can be rejected before it corrupts the estimate. That is the same job the explicit test in speed gating to suppress impossible jumps performs, and running both is not redundant: the gate is statistical and adapts to fix quality, while the speed test is a hard physical bound.

Step-by-step implementation #

1. Keep the state flat and slotted. Four floats for the state and ten for the symmetric covariance fit in a __slots__ dataclass with no NumPy array per device — at fleet scale the array headers cost more than the numbers.

from __future__ import annotations
from dataclasses import dataclass
import math

@dataclass(slots=True)
class CVFilter:
    """Constant-velocity Kalman filter in a local metric frame.
    Covariance stored as the upper triangle of a 4x4 symmetric matrix."""
    x: float = 0.0
    y: float = 0.0
    vx: float = 0.0
    vy: float = 0.0
    p: list[float] | None = None      # 10 entries, upper triangle
    t_ns: int = 0
    initialised: bool = False

def update(f: CVFilter, mx: float, my: float, acc_m: float, t_ns: int,
           sigma_a: float = 1.5, gate: float = 4.0,
           max_gap_ns: int = 30_000_000_000) -> bool:
    """Returns False when the measurement was gated out."""
    if not f.initialised or t_ns - f.t_ns > max_gap_ns:
        f.x, f.y, f.vx, f.vy = mx, my, 0.0, 0.0
        f.p = [acc_m ** 2, 0.0, 0.0, 0.0,
               acc_m ** 2, 0.0, 0.0,
               25.0, 0.0,
               25.0]
        f.t_ns, f.initialised = t_ns, True
        return True
    dt = max(1e-3, (t_ns - f.t_ns) / 1e9)
    # predict (x and y are independent under this model, so run two 2x2 filters)
    px, vx_, pxx = _predict_axis(f.x, f.vx, f.p[0], f.p[1], f.p[7], dt, sigma_a)
    py, vy_, pyy = _predict_axis(f.y, f.vy, f.p[4], f.p[5], f.p[9], dt, sigma_a)
    r = max(1.0, acc_m) ** 2
    # innovation gate: reject a measurement the physics cannot explain
    d2 = ((mx - px) ** 2) / (pxx[0] + r) + ((my - py) ** 2) / (pyy[0] + r)
    if d2 > gate * gate:
        f.t_ns = t_ns
        return False
    f.x, f.vx, f.p[0], f.p[1], f.p[7] = _correct_axis(px, vx_, pxx, mx, r)
    f.y, f.vy, f.p[4], f.p[5], f.p[9] = _correct_axis(py, vy_, pyy, my, r)
    f.t_ns = t_ns
    return True

2. Split the axes. Under a constant-velocity model with isotropic noise, $x$ and $y$ are independent, so two 2×2 filters replace one 4×4 — the same answer for a third of the arithmetic and no matrix inversion beyond a scalar division.

3. Reinitialise rather than predict across a long gap. After 30 s of silence the covariance the filter would compute is technically correct and practically useless; discarding the estimate and restarting from the new fix is both cheaper and more honest.

4. Feed the raw fix to the boundary test, and the filtered one to display. This is the rule that matters most and the one most often broken. The filter’s estimate lags a genuine manoeuvre, so a vehicle turning sharply into a depot is reported by the filter as still outside for over a second. Boundary decisions should use the raw fix with hysteresis and confirmation doing the noise rejection; the filtered track is for the map, for speed estimation, and for supplying the velocity that dead reckoning projects with.

5. Export the innovation. The normalised innovation is a free per-fix quality metric: its distribution should be roughly chi-squared with two degrees of freedom, and a shifted distribution means $\mathbf{Q}$ or $\mathbf{R}$ is mis-set. Exporting it is the cheapest filter-health signal available.

Benchmark and verification #

Measured on 61 million fixes with surveyed reference tracks for 40 vehicles:

The comparison against the moving average is the argument for the filter: a 60-sample mean achieves marginally better RMS at ten times the memory and twenty times the lag, because a mean is a filter with no model and therefore no way to distinguish motion from noise. Adding the innovation gate improves RMS by 16% at 0.1 µs, because the outliers it rejects were dragging the estimate.

The turn-lag column is the caution. Even the best row lags a genuine manoeuvre by 1.4 s, which at 50 km/h is 19 m of position error in exactly the situation — approaching a boundary — where accuracy matters most. Verify this in your own fleet by measuring boundary-crossing timestamps against ground truth with and without the filter in the path; if the filtered path is systematically late, it is being used for something it should not be.

Failure modes and edge cases #

The last row connects directly to geodesic vs planar distance for fence tests: a filter whose state is in degrees has a process noise that means different distances north-south and east-west, and its behaviour changes with latitude in a way that is nearly impossible to debug. Project into local metres before the filter and back afterwards, or keep the whole track pipeline in the metric frame.

One more edge case is worth stating because it looks like a filter bug and is not. A stationary device with a good fix produces a filter estimate that slowly acquires a small non-zero velocity, because the process noise permits it and the measurements are noisy. The velocity is spurious, and if dead reckoning uses it, a parked vehicle slowly drifts across the map during a dropout. Clamp the reported velocity to zero when its magnitude is below the noise floor implied by the covariance — roughly $\sqrt{P_{vv}}$ — rather than trusting a value the filter itself considers indistinguishable from zero.

Related #

• Dead Reckoning & Trajectory Interpolation — the parent topic, which consumes this filter’s velocity estimate.
• Speed Gating to Suppress Impossible Jumps — the hard physical bound that complements the statistical gate.
• Boundary Hysteresis & Debounce Tuning — where boundary-side noise rejection belongs instead.