Adaptive Kalman Tuning for Mixed-Fleet Dynamics

The Kalman filter for GPS noise reduction works well when a single process-noise value sigma_a matches the vehicle class it was tuned for. Freight fleets pick a low sigma_a (around 0.3 m/s²) to enforce smooth motorway trajectories; last-mile delivery fleets pick a higher value (around 1.5 m/s²) to keep up with frequent turns and stop-start traffic. The problem appears the moment one ingestion pipeline serves a mixed fleet — motorbike couriers weaving through traffic, panel vans on suburban rounds, and articulated trucks on trunk roads — and a single fixed Q is asked to fit all three. Set it low and the filter lags every motorbike turn; set it high and it injects jitter into every truck’s motorway cruise.

This page extends the base cluster with an innovation-adaptive variant: instead of hand-picking one sigma_a per deployment, the filter adjusts sigma_a online from the statistics of its own innovation sequence, a technique generally known as innovation-based adaptive estimation or covariance matching. The same mechanism doubles as an automatic vehicle-dynamics detector, since a vehicle that suddenly needs a larger sigma_a to explain its innovations is, by definition, maneuvering more aggressively than its recent history.

Compatibility and Configuration Requirements

Adaptive Kalman Filter Implementation

The class below builds on the constant-velocity state-space model from the base cluster and adds an innovation buffer, a windowed chi-squared comparison, and a bounded update rule for sigma_a. Each constructor parameter documents the effect of raising or lowering it.

from collections import deque
import numpy as np


class AdaptiveFleetKalmanFilter:
    """
    Constant-velocity Kalman filter with innovation-adaptive process noise.

    Extends the fixed-Q formulation used for a single vehicle class by
    letting sigma_a drift within bounds based on the windowed normalised
    innovation squared (NIS). This removes the need to hand-tune a
    separate sigma_a per vehicle class in a mixed fleet.

    Parameters
    ----------
    dt : float
        Nominal time step in seconds; overridden per fix at runtime.
    sigma_a_init : float
        Starting process noise (RMS acceleration std dev, m/s^2). Seed
        this from a vehicle-class lookup table (e.g. 0.3 for heavy
        trucks, 0.8 for vans, 1.5 for motorbikes) so adaptation starts
        close to the right regime instead of drifting from a cold value.
    sigma_z : float
        Base measurement noise in metres at HDOP=1, same role as in the
        base filter.
    innovation_window : int
        Number of most recent NIS values averaged before adapting.
        Smaller windows (5-10) react faster to genuine regime changes
        but oscillate more; larger windows (20-40) are stable but lag
        behind sudden braking or turning events.
    sigma_a_min : float
        Lower clip on sigma_a. Prevents adaptation from collapsing
        process noise to near zero during a long, unusually smooth
        stretch, which would make the filter over-confident and slow
        to react to the next manoeuvre.
    sigma_a_max : float
        Upper clip on sigma_a. Prevents a burst of bad fixes from
        driving process noise to a value that makes the filter simply
        track the raw measurements, defeating the purpose of filtering.
    adaptation_gain : float
        Fraction of the gap between observed and expected NIS applied
        to sigma_a on each adaptation step. Values around 0.1-0.3 are
        stable; values above 0.5 tend to oscillate (see pitfalls below).
    chi2_gate : float
        NIS threshold, for a 2-dimensional observation, above which a
        single step is treated as a gross outlier rather than evidence
        of a genuine dynamics change, and adaptation is skipped for
        that step. 9.21 corresponds to the 99% quantile of a
        chi-squared distribution with 2 degrees of freedom.
    """

    def __init__(
        self,
        dt: float = 1.0,
        sigma_a_init: float = 0.5,
        sigma_z: float = 4.0,
        innovation_window: int = 15,
        sigma_a_min: float = 0.1,
        sigma_a_max: float = 4.0,
        adaptation_gain: float = 0.2,
        chi2_gate: float = 9.21,
    ):
        self.dt = dt
        self.sigma_a = sigma_a_init
        self.sigma_z = sigma_z
        self.sigma_a_min = sigma_a_min
        self.sigma_a_max = sigma_a_max
        self.adaptation_gain = adaptation_gain
        self.chi2_gate = chi2_gate

        # Expected NIS mean for a 2-dimensional observation is the
        # observation dimension itself (a standard chi-squared property).
        self.expected_nis = 2.0

        self._nis_buffer = deque(maxlen=innovation_window)

        self.x = np.zeros(4)
        self.P = np.diag([100.0, 100.0, 25.0, 25.0])
        self.H = np.array([[1, 0, 0, 0],
                           [0, 1, 0, 0]], dtype=float)

    def _build_F(self, dt: float) -> np.ndarray:
        return np.array([[1, 0, dt, 0],
                         [0, 1, 0, dt],
                         [0, 0, 1,  0],
                         [0, 0, 0,  1]], dtype=float)

    def _build_Q(self, dt: float) -> np.ndarray:
        sa2 = self.sigma_a ** 2
        return sa2 * np.array([
            [dt**3 / 3, 0,         dt**2 / 2, 0        ],
            [0,         dt**3 / 3, 0,         dt**2 / 2],
            [dt**2 / 2, 0,         dt,        0        ],
            [0,         dt**2 / 2, 0,         dt       ],
        ])

    def initialise(self, x0: float, y0: float):
        """Seed from the first valid fix."""
        self.x = np.array([x0, y0, 0.0, 0.0])

    def predict(self, dt: float) -> tuple:
        F = self._build_F(dt)
        Q = self._build_Q(dt)
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + Q
        self.P = (self.P + self.P.T) / 2
        return self.x.copy(), self.P.copy()

    def update(self, z: np.ndarray, hdop: float = 1.0) -> tuple:
        """
        Correct state with a new position measurement and adapt
        sigma_a from the resulting innovation.

        Parameters
        ----------
        z    : (2,) array — observed [easting, northing] in metres
        hdop : float      — horizontal dilution of precision; scales R
        """
        R = np.eye(2) * (self.sigma_z * hdop) ** 2
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        I_KH = np.eye(4) - K @ self.H
        self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T
        self.P = (self.P + self.P.T) / 2

        nis = float(y.T @ np.linalg.inv(S) @ y)
        self._adapt_sigma_a(nis)

        return self.x.copy(), self.P.copy()

    def _adapt_sigma_a(self, nis: float) -> None:
        """
        Windowed innovation-based adaptation of sigma_a (covariance
        matching). Skips adaptation on gross-outlier steps so a single
        bad fix cannot push sigma_a toward sigma_a_max on its own.
        """
        if nis > self.chi2_gate:
            return

        self._nis_buffer.append(nis)
        if len(self._nis_buffer) < self._nis_buffer.maxlen:
            return

        windowed_nis = float(np.mean(self._nis_buffer))
        error_ratio = (windowed_nis - self.expected_nis) / self.expected_nis
        adjustment = 1.0 + self.adaptation_gain * error_ratio
        self.sigma_a = float(
            np.clip(self.sigma_a * adjustment, self.sigma_a_min, self.sigma_a_max)
        )

Key parameter notes:

• _adapt_sigma_a only fires once the buffer is full, so the filter behaves exactly like the fixed-sigma_a base filter for the first innovation_window steps of every trip. This is intentional: adapting on a half-full window amplifies noise.
• windowed_nis > expected_nis means the filter has been under-trusting real dynamics (too smooth a Q for what the vehicle is actually doing), so sigma_a scales up. windowed_nis < expected_nis means Q is larger than the vehicle needs, so sigma_a scales down toward a smoother trajectory.
• The gate check happens before the buffer append, not after, so a single spike neither adapts on its own nor contaminates the next several windowed averages.

Vehicle-Class Baseline Table

Seed sigma_a_init from operational vehicle class rather than starting every filter instance at the same cold value; this shortens the number of steps needed before the adaptive loop converges on a sensible regime.

Execution and Tuning Guidelines

Run the filter identically to the base Kalman filtering cluster workflow: project to a metric CRS, synchronise timestamps, pre-filter gross spikes with a rolling median filter, then call predict/update per fix. The only addition is reading kf.sigma_a after each update call if you want to log or export the adapted value for observability.

• innovation_window — raise it (20-40) for heavy trucks where dynamics change slowly and you want a stable, low-noise sigma_a trajectory; lower it (5-10) for motorbikes and couriers where you want the filter to notice a new regime, such as entering a motorway, within a few seconds.
• sigma_a_min / sigma_a_max — set these from the vehicle-class table above, not from a single fleet-wide guess. A truck’s sigma_a_max should stay well below a motorbike’s sigma_a_min is not required, but the ranges should reflect physically plausible acceleration envelopes per class.
• adaptation_gain — start at 0.2. Raise toward 0.3-0.4 only if you have validated that the innovation sequence is genuinely non-stationary (vehicle alternates between very different driving regimes within a trip); higher gains without that justification mostly amplify measurement noise into the process-noise estimate.
• chi2_gate — 9.21 (99% quantile, 2 degrees of freedom) is a reasonable default. Lower it toward 5.99 (95% quantile) if your fleet already runs strict outlier removal upstream and you want adaptation to react to smaller innovation spikes; raise it if gross spikes still reach the filter and you are seeing sigma_a pinned at sigma_a_max after isolated bad fixes.

Validate the result the same way as the base filter: overlay raw, fixed-Q, and adaptive-Q traces on a map tile for a trip that crosses vehicle-behaviour regimes (urban delivery leg followed by a motorway leg), and confirm the adaptive trace tracks corners as tightly as a high-sigma_a filter would during the urban leg while staying as smooth as a low-sigma_a filter during the motorway leg.

Common Pitfalls

Up: Kalman Filtering for GPS Noise Reduction | GPS Data Preprocessing & Cleaning Fundamentals

Related

• Kalman Filtering for GPS Noise Reduction — the fixed-Q base filter this page extends with online process-noise adaptation
• Implementing a rolling median filter for GPS drift removal — upstream pre-filter that keeps gross spikes from reaching the innovation gate
• Outlier Removal in Raw Telematics Streams — complementary statistical rejection that reduces how often chi2_gate needs to fire
• Hampel Filter vs Z-Score for GPS Spike Rejection — a robust rolling detector suited to the same mixed-fleet spike patterns
• Speed Profiling from Raw GPS Coordinates — consumes the adapted filter’s inferred velocity to build per-segment speed profiles