Kalman Filtering & GPS Smoothing for Waste Telematics

A refuse vehicle reports position from the worst radio environment in the fleet: low speed under tree canopy, long dwell beside steel-sided containers, and repeated satellite handoff in the urban corridors it services all day. The raw track that arrives is not wrong in one obvious way — it is jittery. Consecutive fixes wander a few metres in random directions while the truck sits still, then a multipath reflection throws a single point across the street. Feed that stream straight into the routing geometry and the VRP Route Optimization Algorithms build a distance matrix out of noise: phantom detours inflate inter-stop distances, a stationary truck accrues fictitious travel, and the service-verification record shows the vehicle on the wrong side of a franchise boundary it never crossed.

This is the position-smoothing sub-system of the Telematics & Sensor Data Ingestion pipeline, and it sits at a very specific place in the flow. The kinematic pre-filter documented in Validating GPS Coordinates in Python runs before this stage: it rejects points a vehicle could not physically have reported — out-of-bounds coordinates, low-accuracy fixes, impossible velocities. That hard velocity gate is necessary but not sufficient. It throws away the gross spikes, but the surviving track is still jagged, because a fix that passed the gate can still be five metres off. A rejection filter can only delete; it cannot reconstruct. What produces a smooth, defensible position track — one whose every point is a weighted reconciliation of the model’s prediction and the sensor’s measurement — is a Kalman filter, and that is what this page builds.

The distinction matters operationally. Without smoothing, raw multipath jitter corrupts the matrix and produces impossible routes. With only a hard velocity gate, you get a track that is plausible but still visibly jagged, so distances are still inflated and stops still smear. The filter closes that gap: it carries a running estimate of where the truck is and how fast it is moving, predicts the next position, and corrects that prediction by exactly the amount the new measurement justifies — no more. Every code block below is complete and pasteable into a Python 3.10+ ingestion worker.

Prerequisites

Pin the numerical stack so a smoothed track is reproducible when an auditor re-runs the ingestion months later. The filter is pure linear algebra on top of NumPy; Pydantic types the observation contract at the boundary.

python --version   # 3.10 or newer
pip install \
  numpy==2.1.1 \
  pydantic==2.9.2

Every fix that reaches the filter has already cleared the kinematic pre-filter, so it is inside WGS84 bounds, below the accuracy ceiling, and monotonic in time. What the filter needs on top of that is a typed observation carrying the measurement, its reported accuracy, and a UTC timestamp — the accuracy feeds the measurement-noise covariance, and the timestamp gives the elapsed dt that drives the prediction. Model it explicitly rather than passing loose tuples:

from __future__ import annotations
from datetime import datetime
from pydantic import BaseModel, Field, field_validator


class GPSObservation(BaseModel):
    """One pre-filtered fix entering the smoother. Physics already validated upstream."""
    asset_id: str = Field(min_length=3, max_length=32)
    lat: float = Field(ge=-90.0, le=90.0)
    lon: float = Field(ge=-180.0, le=180.0)
    accuracy_m: float = Field(gt=0.0, le=50.0)   # CEP; drives measurement noise R
    ts: datetime                                  # tz-aware UTC, strictly increasing

    @field_validator("ts")
    @classmethod
    def require_utc(cls, v: datetime) -> datetime:
        if v.tzinfo is None:
            raise ValueError("ts must be timezone-aware UTC")
        return v

The state the filter tracks is four-dimensional — position and velocity in each geographic axis:

$$\mathbf{x} = \begin{bmatrix} \varphi & \lambda & \dot{\varphi} & \dot{\lambda} \end{bmatrix}^{\mathsf{T}}$$

where $\varphi$ is latitude and $\lambda$ is longitude in degrees, and $\dot{\varphi}, \dot{\lambda}$ are their rates in degrees per second. Carrying velocity as explicit state is the whole point of the constant-velocity model: a lone measurement cannot tell jitter from motion, but a filter that already believes the truck is nearly stationary will discount a fix that suddenly claims otherwise. Working in degrees keeps the observation contract identical to what the pre-filter emits; because a refuse route spans a small geographic extent, the local scale distortion is absorbed into the noise tuning below rather than requiring a projected metric frame.

Core Implementation

A linear Kalman filter alternates two steps. Predict advances the state with a motion model and grows the uncertainty to reflect that time has passed without a measurement. Update folds in the new measurement, weighting it against the prediction by the Kalman gain, and shrinks the uncertainty to reflect the information gained.

For the constant-velocity model, the state-transition matrix $F$ propagates position by velocity over the elapsed interval $\Delta t$, and the measurement matrix $H$ projects the 4-D state onto the 2-D position we actually observe:

$$F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \qquad H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}$$

The predict step projects state and covariance forward,

$$\mathbf{x}_{k\mid k-1} = F\,\mathbf{x}_{k-1}, \qquad P_{k\mid k-1} = F\,P_{k-1}\,F^{\mathsf{T}} + Q$$

where $Q$ is the process-noise covariance — how much the truck can accelerate between fixes. The update step forms the innovation $\mathbf{y}$ and its covariance $S$, computes the Kalman gain, and corrects:

$$\mathbf{y}_k = \mathbf{z}_k - H\,\mathbf{x}_{k\mid k-1}, \qquad S_k = H\,P_{k\mid k-1}\,H^{\mathsf{T}} + R$$

$$K_k = P_{k\mid k-1}\,H^{\mathsf{T}}\,\bigl(H\,P_{k\mid k-1}\,H^{\mathsf{T}} + R\bigr)^{-1} = P_{k\mid k-1} H^{\mathsf{T}} S_k^{-1}$$

$$\mathbf{x}_{k\mid k} = \mathbf{x}_{k\mid k-1} + K_k\,\mathbf{y}_k, \qquad P_{k\mid k} = (I - K_k H)\,P_{k\mid k-1}$$

The gain $K_k$ is the reconciliation dial: when measurement noise $R$ is large relative to the predicted uncertainty, $K_k$ is small and the filter trusts its model; when the prediction is uncertain, $K_k$ approaches one and the filter follows the measurement. The class below implements exactly these equations with NumPy, tuned for a refuse vehicle that rarely exceeds roughly 50 km/h and spends much of its shift near zero.

import numpy as np


class ConstantVelocityGPSFilter:
    """A 4-state (lat, lon, vlat, vlon) constant-velocity Kalman filter for GPS smoothing."""

    def __init__(
        self,
        init_lat: float,
        init_lon: float,
        accuracy_m: float,
        accel_deg_s2: float = 3.0e-6,   # process noise: plausible acceleration of a refuse truck
        init_vel_var: float = 1.0e-6,   # prior variance on the unknown initial velocity
    ) -> None:
        # State vector x = [lat, lon, vlat, vlon]^T.
        self.x = np.array([init_lat, init_lon, 0.0, 0.0], dtype=np.float64)

        # Measurement matrix: we observe position only.
        self.H = np.array([[1.0, 0.0, 0.0, 0.0],
                           [0.0, 1.0, 0.0, 0.0]], dtype=np.float64)

        # Initial covariance: position known to sensor accuracy, velocity unknown.
        pos_var = self._meters_to_deg2(accuracy_m)
        self.P = np.diag([pos_var, pos_var, init_vel_var, init_vel_var])

        self._accel_var = accel_deg_s2 ** 2   # spectral density of the acceleration noise
        self._I = np.eye(4)

    @staticmethod
    def _meters_to_deg2(accuracy_m: float) -> float:
        """Convert a CEP in metres to a degree-squared position variance (~111 km per degree)."""
        deg = accuracy_m / 111_000.0
        return deg ** 2

    def _F(self, dt: float) -> np.ndarray:
        F = np.eye(4)
        F[0, 2] = dt
        F[1, 3] = dt
        return F

    def _Q(self, dt: float) -> np.ndarray:
        """Continuous white-noise-acceleration process model discretised over dt."""
        q = self._accel_var
        dt2, dt3, dt4 = dt * dt, dt ** 3, dt ** 4
        return q * np.array([
            [dt4 / 4, 0,       dt3 / 2, 0],
            [0,       dt4 / 4, 0,       dt3 / 2],
            [dt3 / 2, 0,       dt2,     0],
            [0,       dt3 / 2, 0,       dt2],
        ], dtype=np.float64)

    def predict(self, dt: float) -> None:
        if dt <= 0:
            raise ValueError("dt must be positive; timestamps must strictly increase")
        F = self._F(dt)
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + self._Q(dt)

    def update(self, lat: float, lon: float, accuracy_m: float,
               gate_threshold: float = 9.21) -> bool:
        """Fold in one measurement. Returns False if the innovation gate rejects it."""
        z = np.array([lat, lon], dtype=np.float64)
        r = self._meters_to_deg2(accuracy_m)
        R = np.diag([r, r])

        y = z - self.H @ self.x                       # innovation
        S = self.H @ self.P @ self.H.T + R            # innovation covariance
        S_inv = np.linalg.inv(S)

        # Squared Mahalanobis distance; chi-square(2 dof) 99% ~ 9.21.
        mahalanobis_sq = float(y.T @ S_inv @ y)
        if mahalanobis_sq > gate_threshold:
            return False                              # outlier: caller continues predict-only

        K = self.P @ self.H.T @ S_inv                 # Kalman gain
        self.x = self.x + K @ y
        # Joseph-form update keeps P symmetric positive-definite under rounding.
        A = self._I - K @ self.H
        self.P = A @ self.P @ A.T + K @ R @ K.T
        return True

    @property
    def position(self) -> tuple[float, float]:
        return float(self.x[0]), float(self.x[1])

Two implementation choices are load-bearing. The innovation gate inside update is what keeps a surviving multipath fix — one that cleared the upstream velocity gate but is still several metres off — from yanking the estimate: a measurement whose squared Mahalanobis distance exceeds the chi-square threshold is refused, and the caller advances on the prediction alone. And the covariance is updated in Joseph form, $P = (I-KH)P(I-KH)^{\mathsf{T}} + KRK^{\mathsf{T}}$, rather than the shorter $(I-KH)P$; the longer expression is algebraically identical but numerically robust, preserving symmetry and positive-definiteness across thousands of stops so the matrix never goes singular mid-shift.

Driving the filter over a stream is a matter of computing dt from consecutive observation timestamps, predicting, then updating:

from typing import Iterable


def smooth_track(observations: Iterable[GPSObservation]) -> list[dict]:
    """Run the filter over one asset's ordered fixes, emitting a smoothed record per fix."""
    obs = list(observations)
    if not obs:
        return []

    first = obs[0]
    kf = ConstantVelocityGPSFilter(first.lat, first.lon, first.accuracy_m)
    smoothed: list[dict] = []
    prev_ts = first.ts

    for o in obs:
        dt = (o.ts - prev_ts).total_seconds()
        if dt > 0:
            kf.predict(dt)
        accepted = kf.update(o.lat, o.lon, o.accuracy_m)
        s_lat, s_lon = kf.position
        smoothed.append({
            "asset_id": o.asset_id,
            "ts": o.ts.isoformat(),
            "raw_lat": o.lat, "raw_lon": o.lon,
            "smoothed_lat": s_lat, "smoothed_lon": s_lon,
            "gate_passed": accepted,
            "pos_var_deg2": float(kf.P[0, 0]),
        })
        prev_ts = o.ts
    return smoothed

The smoothed positions this emits are what the Distance Matrix Construction stage consumes when it builds the travel-time graph, and the same dt-driven cadence is why the smoother pairs naturally with the adaptive intervals set by GPS Polling Strategies. For the special case of a static asset — a bin whose fixes should collapse to one coordinate rather than track a trajectory — use the position-only variant in Kalman Filter GPS Smoothing for Bin Coordinates.

Regulatory Mapping

Smoothing is a numerically defensible operation only if it can prove it never invented a position. Each parameter and behaviour below traces to a rule an inspector or municipal contract officer can cite, and the binding obligation is that the filter reconciles raw fixes rather than fabricating them.

The controlling principle is that a Kalman-smoothed coordinate is a derived value, and a derived value used as compliance evidence must be reproducible from inputs an auditor can independently inspect. That is why smooth_track emits raw_lat/raw_lon alongside the smoothed pair and carries pos_var_deg2: the filter’s output is falsifiable. It also honours the position-time ordering the pre-filter enforces — the non-monotonic reject documented in Validating GPS Coordinates in Python guarantees the strictly increasing timestamps the predict step depends on, which the DOT/FMCSA Rule Mapping reference turns into hours-of-service constraints. A filter that quietly discarded the innovation would break that chain; a filter that logs residuals keeps it. The Bin Sensor API Sync layer applies the same reconcile-not-fabricate discipline to fill-level telemetry.

Validation & Verification

Two properties must be proven in CI: the filter genuinely reduces error on a noisy track, and its innovation gate genuinely rejects a spike. Prove the first with a synthetic ground-truth trajectory plus additive noise, and assert the smoothed root-mean-square error is below the raw RMS error. Prove the second by injecting a fix far outside the innovation covariance and asserting the update refuses it.

import numpy as np
from datetime import datetime, timezone, timedelta


def _rms(errors: np.ndarray) -> float:
    return float(np.sqrt(np.mean(errors ** 2)))


def test_filter_reduces_rms_error() -> None:
    rng = np.random.default_rng(42)
    # Ground truth: a straight eastward leg at ~constant speed, 60 fixes @ 1 Hz.
    n = 60
    true_lat = 40.7000 + np.zeros(n)
    true_lon = -74.0100 + np.arange(n) * 1.0e-4      # ~11 m per step east
    noise_deg = 8.0 / 111_000.0                       # ~8 m CEP jitter
    meas_lat = true_lat + rng.normal(0, noise_deg, n)
    meas_lon = true_lon + rng.normal(0, noise_deg, n)

    kf = ConstantVelocityGPSFilter(meas_lat[0], meas_lon[0], accuracy_m=8.0)
    sm_lat, sm_lon = np.empty(n), np.empty(n)
    for i in range(n):
        if i > 0:
            kf.predict(dt=1.0)
        kf.update(meas_lat[i], meas_lon[i], accuracy_m=8.0)
        sm_lat[i], sm_lon[i] = kf.position

    raw_rms = _rms(np.hypot(meas_lat - true_lat, meas_lon - true_lon))
    smooth_rms = _rms(np.hypot(sm_lat - true_lat, sm_lon - true_lon))
    assert smooth_rms < raw_rms, "filter must reduce RMS position error"


def test_innovation_gate_rejects_spike() -> None:
    kf = ConstantVelocityGPSFilter(40.7000, -74.0100, accuracy_m=5.0)
    for _ in range(5):                                # settle on a stationary track
        kf.predict(dt=1.0)
        kf.update(40.7000, -74.0100, accuracy_m=5.0)
    # A fix ~1.5 km away in one second is statistically impossible under S.
    accepted = kf.update(40.7135, -74.0100, accuracy_m=5.0)
    assert accepted is False, "gate must reject an out-of-covariance measurement"

In production, emit the innovation and gate decision as a structured record so the CI assertion has a logged counterpart an auditor can query. Follow the site-wide JSONFormatter pattern, with fields specific to smoothing:

import json
import logging


class JSONFormatter(logging.Formatter):
    def format(self, record: logging.LogRecord) -> str:
        payload = {
            "event": record.getMessage(),
            "asset_id": getattr(record, "asset_id", None),
            "innovation_lat_deg": getattr(record, "innovation_lat_deg", None),
            "innovation_lon_deg": getattr(record, "innovation_lon_deg", None),
            "mahalanobis_sq": getattr(record, "mahalanobis_sq", None),
            "gate_passed": getattr(record, "gate_passed", None),
            "pos_var_deg2": getattr(record, "pos_var_deg2", None),
        }
        return json.dumps(payload, separators=(",", ":"))


logger = logging.getLogger("gps_smoothing")
_handler = logging.StreamHandler()
_handler.setFormatter(JSONFormatter())
logger.addHandler(_handler)
logger.setLevel(logging.INFO)

The two fields an auditor reads first are mahalanobis_sq and gate_passed: a sustained rise in the gated-out rate is the earliest signal that a vehicle’s receiver is degrading or that the process-noise tuning no longer matches how the truck is being driven. Run these tests with pytest==8.2.0 so the smoother is covered by the same suite as the rest of the ingestion framework.

Failure Modes & Edge Cases

Each of the following is a real way a GPS Kalman filter fails in production, with the mitigation that contains it.

1. Filter divergence. If the process-noise Q is set too small, the filter becomes overconfident: P collapses, the gain approaches zero, and the estimate stops tracking reality — every real move then looks like an outlier and gets gated out. The symptom is a smoothed track frozen behind the truck. Detect it by watching the gated-out rate and the trace of P; recover by widening accel_deg_s2 so the model admits the acceleration the vehicle actually exhibits.

def guard_against_divergence(kf: ConstantVelocityGPSFilter, recent_gate_rejects: int) -> None:
    """If the gate is rejecting almost everything, the model is too confident — reset velocity uncertainty."""
    if recent_gate_rejects >= 5:
        kf.P[2, 2] = max(kf.P[2, 2], 1.0e-6)   # re-inflate velocity variance
        kf.P[3, 3] = max(kf.P[3, 3], 1.0e-6)
        logger.warning("filter_reinflated", extra={"asset_id": None, "gate_passed": False})

2. Over-smoothing real turns. A constant-velocity model assumes straight-line motion, so a sharp turn at an intersection produces a burst of large innovations the filter initially fights, rounding the corner. This is inherent to the model, not a bug, but it is bounded: keep Q large enough that a legitimate turn is absorbed within a fix or two rather than smeared across the whole block. If corners must be crisp, the fix is a richer motion model (constant-turn), not a smaller Q — shrinking Q trades corner lag for divergence.

3. Gap handling during a dropout. When the receiver goes dark under a bridge or in a tunnel, no measurement arrives. Do not fabricate one. Run predict-only — advance the state with predict(dt) and let P grow — so the estimate coasts on its last velocity while honestly widening its uncertainty. Mark those fixes as extrapolated so downstream never treats a coasted position as a measured proof-of-service.

def coast_through_gap(kf: ConstantVelocityGPSFilter, gap_seconds: float,
                      step: float = 1.0) -> list[tuple[float, float]]:
    """Predict-only across a dropout; positions are extrapolated, never measured."""
    coasted: list[tuple[float, float]] = []
    remaining = gap_seconds
    while remaining > 1e-9:
        dt = min(step, remaining)
        kf.predict(dt)
        coasted.append(kf.position)
        remaining -= dt
    return coasted

4. NaN from singular covariance. If R is zero (an accuracy of exactly 0 m) or P degrades under floating-point rounding, np.linalg.inv(S) can return inf/nan and poison every subsequent state. The GPSObservation schema already forbids accuracy_m <= 0, and the Joseph-form update keeps P symmetric; add a final guard that refuses to commit a non-finite state and re-seeds from the raw fix instead.

if not np.all(np.isfinite(kf.x)):
    logger.error("non_finite_state", extra={"asset_id": None, "gate_passed": False})
    kf.__init__(obs.lat, obs.lon, obs.accuracy_m)   # re-seed cleanly from the raw measurement

5. Cold start on the first fix. With no prior velocity, the initial estimate must not over-trust its own guess. Seed velocity variance generously (as the constructor does) so the first few updates are measurement-led, then let the filter earn its confidence as fixes accumulate.

Integration Checklist

Complete every item before a smoothed track feeds the routing matrix:

• Every observation passes the GPSObservation schema and has already cleared the upstream kinematic pre-filter.
• Timestamps are strictly increasing per asset so dt is always positive.
• Process noise accel_deg_s2 is tuned to the vehicle class, not left at a copied default.
• Measurement noise R is derived from each fix’s reported accuracy_m, not hardcoded.
• The innovation gate is active, and its gate_threshold matches the chosen chi-square confidence level.
• Covariance uses the Joseph-form update; a singular-S / non-finite-state guard re-seeds cleanly.
• Dropouts run predict-only and coasted positions are flagged as extrapolated, never logged as measured.
• Each smoothed fix retains its raw lat/lon and emits mahalanobis_sq, gate_passed, and pos_var_deg2.
• The RMS-reduction and gate-rejection tests run in CI and both pass.

FAQ

Why not just average the last few GPS fixes instead of running a Kalman filter?

A moving average has no motion model, so it lags whenever the truck actually moves and cannot distinguish a real displacement from jitter — it smooths both equally. The Kalman filter carries velocity as state and weights each measurement by its stated accuracy against the model’s confidence, so it suppresses jitter while still following genuine motion, and it exposes an innovation you can gate and log for audit. An average gives you none of that.

Does smoothing risk fabricating a position the truck never occupied?

Only if you discard the evidence. This implementation keeps the raw fix beside every smoothed one, logs the innovation and gate decision, and emits the position variance, so every smoothed coordinate is reproducible from inputs an auditor can inspect. A predict-only coasted position during a dropout is explicitly flagged as extrapolated. The smoother reconciles measurements; it never overwrites or invents them.

Should a stationary bin use this same constant-velocity filter?

No. A constant-velocity model assumes the asset is moving, and applying it to a static bin lets the velocity state drift on pure noise, producing a coordinate that slowly wanders. For a bin whose fixes should collapse to a single stable point, use the position-only recursive estimator in Kalman Filter GPS Smoothing for Bin Coordinates, which models zero motion and shrinks variance toward one coordinate.

How do I choose the process-noise value for a refuse vehicle?

Start from the acceleration the vehicle can plausibly exhibit between fixes — a heavy truck at low speed is a few tenths of a metre per second squared — convert to degrees, and use that as accel_deg_s2. Then tune empirically: if legitimate turns are being gated out, Q is too small; if the track is jittery, it is too large. Watch the gated-out rate and the trace of P in the structured logs to find the balance.

Why gate inside the filter when the pre-filter already rejected velocity spikes?

The two gates catch different failures. The upstream velocity gate rejects grossly impossible jumps using raw point-to-point speed. The innovation gate rejects fixes that are individually plausible but statistically inconsistent with the filter’s current belief — a five-metre multipath offset that passed the velocity check but would still distort the estimate. Layering them is what turns a merely legal track into a smooth one.

Up: Telematics & Sensor Data Ingestion

Related

• Validating GPS Coordinates in Python — the kinematic pre-filter that runs before this smoother and guarantees its ordered, in-bounds input.
• Kalman Filter GPS Smoothing for Bin Coordinates — the position-only variant for static assets that resolve to one coordinate.
• Distance Matrix Construction — the routing stage that consumes the smoothed track to build travel-time geometry.
• GPS Polling Strategies — adaptive cadence that sets the sampling interval the filter’s dt depends on.
• Async Batch Processing — bounded fan-out of smoothed fixes to the solver.