Kalman Filtering for GPS Noise Reduction

Fleet telematics pipelines routinely ingest raw positional streams contaminated by multipath interference, atmospheric delays, satellite geometry shifts, and receiver clock jitter. Naive smoothing techniques — a simple moving average, for example — introduce lag and are blind to the vehicle’s kinematic model, causing them to smear corners and misplace deceleration events. The Kalman filter solves this differently: it maintains a probabilistic state estimate that is continuously updated by each new measurement, weighting observations against a physical motion model in real time.

When integrated into a broader GPS Data Preprocessing & Cleaning strategy, a well-tuned Kalman filter transforms erratic point clouds into smooth, physically plausible trajectories suitable for routing optimisation, ETA prediction, driver behaviour scoring, and regulatory compliance auditing.

Prerequisites

Before deploying state-space estimators, confirm the following:

Python environment: Python 3.10+, numpy ≥ 1.24, pyproj ≥ 3.5. The scipy package is optional but useful for chi-squared NEES checks.

Coordinate projection. Apply Coordinate Reference System mapping for fleet data before the filter. Filtering on raw WGS84 latitude/longitude degrees is invalid: one degree of longitude varies from ~111 km at the equator to 0 km at the poles, which destroys the Euclidean distance assumptions inside the prediction step. Project to a local UTM zone; EPSG:3857 Web Mercator is not suitable because its scale factor changes significantly with latitude.

Timestamp alignment. The filter’s state transition matrix contains an explicit time-delta Δt. Inconsistent or duplicated timestamps produce nonsensical Δt values and cause covariance divergence. Timestamp synchronisation for multi-device GPS logs establishes a monotonic UTC index before the filter runs.

Minimum data fields per record:

• Synchronised UTC timestamp (monotonic, no gaps greater than 2× expected sampling interval)
• Easting and northing in a metric projected CRS
• HDOP or tracked-satellite count for dynamic measurement noise scaling
• Optional but recommended: reported speed or heading for post-hoc validation

State-Space Architecture for Fleet Kinematics

The discrete-time Kalman filter alternates between two phases: prediction and correction. For fleet applications, a constant-velocity (CV) model captures the majority of road driving. The state vector x_k holds position and velocity in the projected plane:

x_k = [x, y, v_x, v_y]ᵀ

The state transition matrix F_k propagates this forward over a discrete time step Δt:

F_k = | 1  0  Δt  0 |
      | 0  1  0   Δt|
      | 0  0  1   0 |
      | 0  0  0   1 |

The observation matrix H extracts only the two position components, since a standard GPS receiver reports position but not velocity:

H = | 1  0  0  0 |
    | 0  1  0  0 |

This separation lets the filter infer velocity by correlating sequential positions — acting as a low-noise differentiator that is robust to high-frequency measurement error.

Process Noise Q

Q models unaccounted dynamics: sudden braking, steering inputs, road gradient changes. The discrete white-noise acceleration model scales Q with Δt:

Q = σ_a² × | Δt³/3  0      Δt²/2  0     |
            | 0      Δt³/3  0      Δt²/2 |
            | Δt²/2  0      Δt     0     |
            | 0      Δt²/2  0      Δt    |

where σ_a is the RMS acceleration standard deviation. For heavy freight: σ_a ≈ 0.3 m/s². For last-mile delivery vehicles: σ_a ≈ 1.5 m/s².

Step-by-Step Workflow

Step 1 — Project coordinates and synchronise timestamps

from pyproj import Transformer
import pandas as pd
import numpy as np

# Project WGS84 → UTM zone 32N (adjust zone for your region)
transformer = Transformer.from_crs("EPSG:4326", "EPSG:32632", always_xy=True)

df = pd.read_parquet("raw_telemetry.parquet")
df = df.sort_values("utc_ts").reset_index(drop=True)

df["easting"], df["northing"] = transformer.transform(
    df["longitude"].values, df["latitude"].values
)
# dt in seconds between consecutive fixes
df["dt"] = df["utc_ts"].diff().dt.total_seconds().fillna(1.0).clip(lower=0.01)

Expected output shape: same DataFrame with new easting, northing, and dt columns; no NaN timestamps.

Step 2 — Initialise the filter

class FleetKalmanFilter:
    """
    Constant-velocity Kalman filter for projected GPS coordinates.

    Parameters
    ----------
    dt : float
        Nominal time step in seconds (used to seed matrices; updated per fix).
    sigma_a : float
        Process noise — RMS acceleration std dev in m/s².
        Use 0.3 for heavy freight; 1.5 for urban last-mile vehicles.
    sigma_z : float
        Base measurement noise in metres (std dev of position fix at HDOP=1).
        Typical GPS consumer receiver: 3–5 m.
    """
    def __init__(self, dt: float = 1.0, sigma_a: float = 0.5, sigma_z: float = 4.0):
        self.dt = dt
        self.sigma_a = sigma_a
        self.sigma_z = sigma_z

        # State vector [x, y, vx, vy]
        self.x = np.zeros(4)

        # Initial covariance — high uncertainty before any corrections
        self.P = np.diag([100.0, 100.0, 25.0, 25.0])

        # Observation matrix (position only)
        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
        # Enforce symmetry to prevent floating-point drift
        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.

        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                     # innovation
        S = self.H @ self.P @ self.H.T + R           # innovation covariance
        K = self.P @ self.H.T @ np.linalg.inv(S)    # Kalman gain
        self.x = self.x + K @ y
        I_KH = np.eye(4) - K @ self.H
        # Joseph form for numerical stability
        self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T
        self.P = (self.P + self.P.T) / 2
        return self.x.copy(), self.P.copy()

Key parameter notes:

• sigma_z is the standard deviation of the GPS position error at HDOP=1. Consumer-grade vehicle trackers typically have sigma_z between 3 m and 6 m.
• The Joseph-form covariance update (I_KH @ P @ I_KH.T + K @ R @ K.T) is more numerically stable than the simpler (I - K @ H) @ P form because it remains positive-definite even with small floating-point errors.

Step 3 — Run the predict–correct loop

kf = FleetKalmanFilter(sigma_a=0.5, sigma_z=4.0)

smoothed_rows = []
for i, row in df.iterrows():
    z = np.array([row["easting"], row["northing"]])
    hdop = float(row.get("hdop", 1.0))
    dt = float(row["dt"])

    if i == 0:
        kf.initialise(z[0], z[1])
        x_est = kf.x.copy()
    else:
        kf.predict(dt)
        x_est, _ = kf.update(z, hdop=hdop)

    smoothed_rows.append({
        "utc_ts":    row["utc_ts"],
        "east_filt": x_est[0],
        "north_filt": x_est[1],
        "vx_ms":     x_est[2],
        "vy_ms":     x_est[3],
    })

smoothed = pd.DataFrame(smoothed_rows)

Expected output: smoothed contains one row per input fix with filtered easting/northing and inferred velocity components. Speed magnitude is np.hypot(smoothed["vx_ms"], smoothed["vy_ms"]).

Step 4 — Back-project to WGS84 (optional)

Most downstream systems — including stop detection and HMM map-matching — consume WGS84 coordinates. Re-project after filtering, not before:

back = Transformer.from_crs("EPSG:32632", "EPSG:4326", always_xy=True)
smoothed["lon_filt"], smoothed["lat_filt"] = back.transform(
    smoothed["east_filt"].values,
    smoothed["north_filt"].values,
)

Probability Model and Numerical Stability

The predict–correct cycle is governed by four quantities:

Prediction step:

x̂⁻ = F · x̂
P⁻ = F · P · Fᵀ + Q

Correction step:

y  = z − H · x̂⁻               (innovation / residual)
S  = H · P⁻ · Hᵀ + R           (innovation covariance)
K  = P⁻ · Hᵀ · S⁻¹             (Kalman gain)
x̂  = x̂⁻ + K · y
P  = (I − K · H) · P⁻          (Joseph form in code above)

The Kalman gain K automatically balances two competing sources: when R is large (degraded satellite geometry, high HDOP), K shrinks and the filter trusts its kinematic model. When R is small (clear sky, HDOP ≈ 1), K grows and the filter snaps toward the measurement.

Log-space considerations. Scalar Kalman formulations work in normal space, but extended or unscented variants sometimes accumulate numerical error faster. For the constant-velocity case here, the Joseph-form update and the (P + Pᵀ) / 2 symmetry enforcement are sufficient; full square-root filtering (via Cholesky decomposition) is only necessary when processing thousands of steps without any re-initialisation.

Dynamic R Scaling from HDOP

Static R assumes constant satellite visibility. In urban canyons and during cold starts, scale R at each step:

R_k = np.eye(2) * (sigma_z * hdop_k) ** 2

This prevents the filter from over-trusting degraded fixes. HDOP values above 4 indicate geometry too poor for reliable correction; some implementations skip the measurement update entirely above a configurable threshold (e.g., hdop > 6).

Routing Engine Integration Notes

The filtered trajectory feeds directly into routing and matching stages. Two integration points matter most:

OSRM match service. OSRM’s /match endpoint accepts a radiuses parameter (in metres) per coordinate. After Kalman filtering, derive a per-point radius from the posterior covariance: radius_k = 3 * sqrt(P_k[0,0]) (3-sigma position uncertainty). This tightens the snapping tolerance for high-quality fixes and relaxes it for uncertain ones, reducing false turn restrictions.

Coordinate order. OSRM and Valhalla both accept [longitude, latitude] order, not [easting, northing]. Always back-project to WGS84 and confirm axis order before sending to any routing API; a silent lat/lon swap produces fixes in a different hemisphere with no error response.

Velocity output as speed profile input. The inferred vx_ms and vy_ms can seed speed profiling from raw GPS coordinates without an additional numerical differentiation step, improving segment-level speed estimates particularly where raw fixes are sparse (1–5 s intervals).

Operational Troubleshooting

Deployment Checklist

• Coordinates projected to a metric CRS (UTM or similar) before filter initialisation
• Timestamps sorted, monotonic, and free of duplicates — gap detection in place
• Rolling median pre-filter applied upstream to remove gross outliers
• HDOP or satellite count available for dynamic R scaling per fix
• Joseph-form covariance update and symmetry enforcement in place
• Tunnel / signal-loss gap handling implemented (P reset after extended outage)
• Velocity plausibility clamp configured per vehicle class
• Innovation sequence monitored in production (zero-mean white-noise check)
• NEES check wired to alerting (flag trips where >5% of innovations exceed 3σ)
• Stream partitioned by vehicle_id + trip_id; out-of-order packets handled
• Back-projected WGS84 output validated against map tile before sending to routing API
• Coordinate axis order confirmed ([lon, lat]) for OSRM / Valhalla API calls

Parent topic: GPS Data Preprocessing & Cleaning Fundamentals

Related:

• Implementing a rolling median filter for GPS drift removal — upstream outlier removal that stabilises Kalman filter initialisation
• Coordinate Reference System mapping for fleet data — projecting WGS84 to a metric CRS before state-space filtering
• Timestamp synchronisation for multi-device GPS logs — establishing a consistent Δt across mixed OBD-II and mobile devices
• Outlier removal in raw telematics streams — complementary statistical rejection for jump-magnitude outliers the Kalman gain cannot absorb
• Speed profiling from raw GPS coordinates — using Kalman-inferred velocity to build segment-level speed profiles without extra differentiation