Directionality & Heading Synchronization for Fleet Telematics

Raw telematics streams rarely deliver a clean directional signal. GPS receivers emit latitude, longitude, and a timestamp — but the accompanying course-over-ground (COG) value is frequently absent, stale, or corrupted by multipath reflections, urban canyon geometry, or positional jitter at low speed. Without a rigorous heading synchronization step, downstream analytics — turn detection, lane-level route reconstruction, and driver behaviour scoring — operate on physically implausible angular data, silently degrading every metric that depends on them.

This guide provides a complete, field-tested methodology for computing, validating, and synchronizing vehicle heading data in Python. It covers the mathematics of circular quantities, velocity-aware validation, vectorized azimuth estimation, and the routing engine hooks required to cross-check synchronized headings against the underlying road network. The page sits within the broader Trajectory Analysis & Map Matching Techniques pipeline, immediately upstream of candidate road segment selection.

Prerequisites

Before implementing heading synchronization, verify your environment and data schema meet the following baseline requirements.

Software environment

• Python 3.10+ with pandas>=2.0, numpy>=1.24, scipy>=1.10, and geopandas>=0.14
• shapely>=2.0 for geometric projection when cross-checking against road centrelines

Input data schema

Mathematical background

• Forward bearing on a sphere via arctan2 (haversine azimuth formula)
• Circular statistics: mean angle, circular variance, unit-vector representation
• Basic interpolation: linear and monotone cubic from SciPy’s interpolation API

Coordinate reference system

All spatial operations assume WGS84 (EPSG:4326) for raw ingestion. Distance-sensitive computations — angular acceleration thresholds, candidate buffer radii — may require temporary projection to a local metric CRS. See Coordinate Reference System Mapping for Fleet Data for a production-ready projection workflow.

NMEA message context

Telematics heading data typically originates from NMEA 0183 $GPRMC (COG, speed over ground) or $HEHDT (true heading from gyrocompass) sentences. Understanding which sentence type your TCU emits determines whether the heading_deg field represents magnetic heading, true heading, or course over ground — they are not interchangeable at high heading-change rates.

Why Naive Approaches Fail

The two most common naïve approaches both produce systematic errors that corrupt downstream analytics.

Direct linear interpolation across the 0°/360° boundary. Interpolating between 355° and 005° as scalars yields midpoints near 180° — exactly the wrong direction. A vehicle turning slightly right through north appears to swing completely around. Every smoothing operation applied to heading must use circular arithmetic.

Trusting raw COG at low speed. GPS course-over-ground is derived from consecutive position differences. Below approximately 5 km/h, the positional noise in successive fixes (typically 3–8 m CEP for commercial grade receivers) dominates the angular estimate. A stationary truck with 4 m of GPS wander produces a continuously spinning bearing vector that has no relationship to the vehicle’s physical orientation.

Both failure modes propagate silently: the heading column is populated with numbers in the 0–360 range, the dtype is valid, and no runtime exception fires. Catching them requires domain-aware validation before they reach the Hidden Markov Model map matching candidate scoring stage.

The Mathematics of Circular Quantities

Heading is a circular variable — 359° and 1° are 2° apart, not 358° apart. This distinction requires careful handling at three points in the pipeline.

Forward azimuth (haversine-based)

The bearing from point A (φ₁, λ₁) to point B (φ₂, λ₂) on the WGS84 ellipsoid (approximated as a sphere for short distances) is:

Δλ = λ₂ - λ₁
y  = sin(Δλ) · cos(φ₂)
x  = cos(φ₁) · sin(φ₂) - sin(φ₁) · cos(φ₂) · cos(Δλ)
θ  = atan2(y, x)          # result in radians, range [-π, π]
θ° = (degrees(θ) + 360) mod 360   # normalise to [0, 360)

Circular mean

The circular mean of a heading sequence avoids the scalar mean artefact. Given angles θ₁…θₙ:

μ = atan2( mean(sin(θᵢ)), mean(cos(θᵢ)) )

Used in sliding-window quality checks to detect sustained directional variance.

Unit-vector interpolation

To interpolate between θ_a and θ_b at fraction t, project onto the unit circle, interpolate each component, then recover the angle:

x(t) = (1-t)·cos(θ_a) + t·cos(θ_b)
y(t) = (1-t)·sin(θ_a) + t·sin(θ_b)
θ(t) = atan2(y(t), x(t))

This is the same technique used by graphics libraries for quaternion SLERP, adapted to 2D polar coordinates.

Step-by-Step Workflow

1. Data Ingestion & Timestamp Normalization

Parse raw CSV or Parquet streams, enforce UTC timestamps, and sort chronologically. Remove exact-duplicate timestamps (keep the row with a non-null heading if one exists). Flag records with physically impossible spatial jumps — for commercial fleets a threshold of 150 km/h is a safe upper bound — and either discard or quarantine them before computing bearings.

import numpy as np
import pandas as pd

def ingest_and_normalize(path: str) -> pd.DataFrame:
    df = pd.read_parquet(path)  # or pd.read_csv with parse_dates
    df["timestamp"] = pd.to_datetime(df["timestamp"], utc=True)
    df = df.sort_values("timestamp").reset_index(drop=True)

    # Remove duplicate timestamps, preferring rows with a valid heading
    df["_has_heading"] = df["heading_deg"].notna().astype(int)
    df = (
        df.sort_values(["timestamp", "_has_heading"], ascending=[True, False])
          .drop_duplicates(subset="timestamp", keep="first")
          .drop(columns="_has_heading")
          .reset_index(drop=True)
    )

    # Flag implausible spatial jumps (>150 km/h implies data error)
    dt_s = df["timestamp"].diff().dt.total_seconds().fillna(1)
    dlat = np.radians(df["lat"].diff().fillna(0))
    dlon = np.radians(df["lon"].diff().fillna(0))
    a = np.sin(dlat / 2) ** 2 + (
        np.cos(np.radians(df["lat"])) *
        np.cos(np.radians(df["lat"].shift())) *
        np.sin(dlon / 2) ** 2
    )
    dist_m = 6_371_000 * 2 * np.arcsin(np.sqrt(a.clip(0, 1)))
    speed_ms = dist_m / dt_s.clip(lower=0.1)
    df["_jump_flag"] = speed_ms > (150 / 3.6)
    return df

Expected output shape: a timestamp-indexed DataFrame with monotonic UTC timestamps, no duplicates, and a _jump_flag boolean column.

2. Heading Validation & Low-Speed Gap Detection

GPS-derived COG becomes unreliable below approximately 5 km/h. At walking pace or while manoeuvring in a depot, the noise-to-signal ratio inverts: the positional error exceeds the actual displacement between fixes. Mark these intervals explicitly so the next stage can apply kinematic fallback rather than trusting the sensor.

def validate_headings(df: pd.DataFrame, min_speed_kmh: float = 5.0) -> pd.DataFrame:
    df = df.copy()

    # Normalise raw heading to [0, 360) where present
    if "heading_deg" in df.columns:
        df["heading_deg"] = df["heading_deg"] % 360
    else:
        df["heading_deg"] = np.nan

    # Low-speed mask: COG unreliable below threshold
    if "speed_kmh" in df.columns:
        df["_low_speed"] = df["speed_kmh"] < min_speed_kmh
    else:
        # Derive speed from consecutive distance / delta-t
        dt_s = df["timestamp"].diff().dt.total_seconds().clip(lower=0.1)
        dlat = np.radians(df["lat"].diff().fillna(0))
        dlon = np.radians(df["lon"].diff().fillna(0))
        a = np.sin(dlat / 2) ** 2 + (
            np.cos(np.radians(df["lat"])) *
            np.cos(np.radians(df["lat"].shift(1))) *
            np.sin(dlon / 2) ** 2
        )
        dist_m = 6_371_000 * 2 * np.arcsin(np.sqrt(a.clip(0, 1)))
        df["_low_speed"] = (dist_m / dt_s * 3.6) < min_speed_kmh

    # Combined mask: missing or low-speed heading needs replacement
    df["_needs_fill"] = df["heading_deg"].isna() | df["_low_speed"]
    return df

This timestamp synchronization foundation also catches OBD-II units that batch-transmit buffered pings with identical timestamps — a common source of false zero-speed detections when the OBD adapter buffers at rest.

3. Vectorized Azimuth Computation

Compute forward bearings between consecutive coordinate pairs using the haversine azimuth formula. NumPy’s vectorized arctan2 eliminates Python-level loops and processes millions of rows in milliseconds.

def compute_azimuths(df: pd.DataFrame) -> np.ndarray:
    """Return forward bearing for each row toward the next row (last row = NaN)."""
    lat1 = np.radians(df["lat"].values[:-1])
    lon1 = np.radians(df["lon"].values[:-1])
    lat2 = np.radians(df["lat"].values[1:])
    lon2 = np.radians(df["lon"].values[1:])

    d_lon = lon2 - lon1
    y = np.sin(d_lon) * np.cos(lat2)
    x = (
        np.cos(lat1) * np.sin(lat2)
        - np.sin(lat1) * np.cos(lat2) * np.cos(d_lon)
    )
    bearing_deg = (np.degrees(np.arctan2(y, x)) + 360) % 360

    # Pad to original length; the last point uses the previous bearing
    return np.append(bearing_deg, bearing_deg[-1] if len(bearing_deg) > 0 else np.nan)

Apply circular arithmetic immediately — the % 360 normalisation must follow the degree conversion, not precede it, to correctly handle negative arctan2 outputs.

4. Circular Interpolation for Gap Filling

Once computed azimuths replace low-speed and missing sensor readings, remaining gaps (signal loss, prolonged tunnel traversal) require interpolation. Direct linear interpolation across a pandas NaN chain is incorrect for heading because of the wrap-around problem. The unit-vector approach resolves this in O(n) time.

from scipy.interpolate import interp1d

def circular_interpolate(df: pd.DataFrame, col: str = "sync_heading_deg") -> pd.DataFrame:
    df = df.copy()
    ts_int = df["timestamp"].astype(np.int64).values
    angles = df[col].values

    valid = ~np.isnan(angles)
    if valid.sum() < 2:
        return df  # insufficient data; leave as-is

    ts_v = ts_int[valid]
    cos_v = np.cos(np.radians(angles[valid]))
    sin_v = np.sin(np.radians(angles[valid]))

    f_cos = interp1d(ts_v, cos_v, kind="linear", bounds_error=False, fill_value=(cos_v[0], cos_v[-1]))
    f_sin = interp1d(ts_v, sin_v, kind="linear", bounds_error=False, fill_value=(sin_v[0], sin_v[-1]))

    cos_filled = f_cos(ts_int)
    sin_filled = f_sin(ts_int)

    df[col] = (np.degrees(np.arctan2(sin_filled, cos_filled)) + 360) % 360
    return df

For datasets larger than ~10 M rows, replace scipy.interpolate.interp1d with a chunked NumPy interp call on the cos and sin components separately — it avoids building the full function object for each chunk.

5. Outlier Filtering & Quality Flagging

Remove directional spikes that exceed physically plausible angular acceleration. A loaded delivery truck cannot rotate faster than approximately 30–40°/s at motorway speed; 90°/s is a safe fleet-wide ceiling. Emit a heading_quality score so downstream consumers can selectively exclude low-confidence records.

def filter_and_flag(
    df: pd.DataFrame,
    raw_heading_col: str = "heading_deg",
    computed_bearing: np.ndarray | None = None,
    min_speed_kmh: float = 5.0,
    max_angular_rate_deg_s: float = 90.0,
) -> pd.DataFrame:
    df = df.copy()

    if computed_bearing is None:
        computed_bearing = compute_azimuths(df)

    # Start with raw sensor heading
    df["sync_heading_deg"] = df[raw_heading_col].copy()

    # Replace low-speed / missing with computed azimuth
    replace_mask = df["_needs_fill"]
    df.loc[replace_mask, "sync_heading_deg"] = computed_bearing[replace_mask.values]

    # Apply circular interpolation for any remaining NaN
    df = circular_interpolate(df, col="sync_heading_deg")

    # Detect angular acceleration spikes
    dt_s = df["timestamp"].diff().dt.total_seconds().clip(lower=0.1)
    h_rad = np.radians(df["sync_heading_deg"].values)
    diff_rad = np.arctan2(
        np.sin(np.diff(h_rad, prepend=h_rad[0])),
        np.cos(np.diff(h_rad, prepend=h_rad[0]))
    )
    ang_rate = np.abs(np.degrees(diff_rad)) / dt_s.values
    spike_mask = ang_rate > max_angular_rate_deg_s

    # Replace spikes with locally interpolated value
    df.loc[spike_mask, "sync_heading_deg"] = np.nan
    df = circular_interpolate(df, col="sync_heading_deg")

    # Quality flags:
    #  1 = raw sensor heading used
    #  2 = computed azimuth used (low-speed or gap fill)
    #  3 = kinematic interpolation (extended signal loss)
    #  0 = unresolvable
    df["heading_quality"] = 1
    df.loc[replace_mask, "heading_quality"] = 2
    df.loc[spike_mask, "heading_quality"] = 3
    df.loc[df["sync_heading_deg"].isna(), "heading_quality"] = 0

    return df

The heading_quality flag is the contract between this module and its consumers. Speed profiling routines and HMM emission probability models can down-weight quality-2 and quality-3 headings proportionally rather than treating them as equivalent to hardware-measured values.

Math Model: Numerical Stability in Circular Arithmetic

The two core stability pitfalls are scalar wrapping and accumulated floating-point error in long interpolation chains.

Wrapping in difference calculations. When computing turn angle as θ_next - θ_prev, values near the 0°/360° boundary produce differences like 358° for a 2° left turn. The correct form:

Δθ = atan2( sin(θ_next - θ_prev), cos(θ_next - θ_prev) )

In NumPy:

delta = np.degrees(
    np.arctan2(
        np.sin(np.radians(h_next - h_prev)),
        np.cos(np.radians(h_next - h_prev))
    )
)

Accumulated error in unit-vector chains. After projecting to cos/sin, interpolating, and recovering with arctan2, the norm of the interpolated vector is not guaranteed to be 1.0 — especially near 90°/270° where sin or cos passes through zero. For most telematics applications the angular error is below 0.1° and can be ignored. If sub-degree accuracy is required, normalize the interpolated vector before calling arctan2.

Routing Engine Integration

Synchronized headings have two primary integration points with routing engines: candidate filtering and bearing-penalty scoring.

OSRM bearing filter

The OSRM HTTP API accepts a bearings parameter on the /match and /nearest endpoints:

GET /nearest/v1/driving/{lon},{lat}?bearings={heading};{tolerance}

A bearing of 270 with tolerance 20 restricts candidates to road segments oriented 250°–290°. Set tolerance to 25–35° for normal driving; widen to 45° for roads with poor alignment between OSM geometry and actual lane position. Always supply synchronized heading rather than raw sensor COG to avoid over-restricting candidates during cornering sequences.

import requests

def osrm_nearest_with_bearing(lat: float, lon: float, heading_deg: float,
                               tolerance: int = 30,
                               osrm_base: str = "http://localhost:5000") -> dict:
    url = (
        f"{osrm_base}/nearest/v1/driving/{lon},{lat}"
        f"?bearings={int(heading_deg)};{tolerance}"
        f"&number=3"
    )
    resp = requests.get(url, timeout=5)
    resp.raise_for_status()
    return resp.json()

Coordinate order reminder: OSRM uses longitude,latitude in the URL path — the reverse of most Python geospatial libraries that follow the GeoJSON [lon, lat] convention. Swapping these coordinates is one of the most common integration bugs and produces silently incorrect candidate sets.

Valhalla trace_route bearing

Valhalla’s /trace_route endpoint accepts a heading field per shape point. Supply sync_heading_deg directly:

def build_valhalla_trace(df: pd.DataFrame) -> dict:
    shape = [
        {
            "lat": row.lat,
            "lon": row.lon,
            "time": int(row.timestamp.timestamp()),
            "heading": int(row.sync_heading_deg),
            "heading_tolerance": 40 if row.heading_quality <= 2 else 60,
        }
        for row in df.itertuples()
        if row.heading_quality > 0
    ]
    return {"shape": shape, "costing": "auto", "shape_match": "map_snap"}

Widen heading_tolerance for quality-3 interpolated headings to prevent Valhalla from rejecting valid trace points. Exclude quality-0 records entirely — they carry no directional information and may force implausible routing decisions.

GraphHopper map matching

GraphHopper’s map matching API accepts GeoJSON with optional bearing constraints per point. As of GraphHopper 8.x, pass bearing in the gpx_accuracy envelope or as a custom property in the extended GeoJSON format. Consult the GraphHopper Map Matching documentation for the exact field names, which differ between the REST API and the Java client.

Operational Troubleshooting

from scipy.ndimage import uniform_filter1d

def circular_median_smooth(angles_deg: np.ndarray, window: int = 3) -> np.ndarray:
    cos_s = uniform_filter1d(np.cos(np.radians(angles_deg)), size=window)
    sin_s = uniform_filter1d(np.sin(np.radians(angles_deg)), size=window)
    return (np.degrees(np.arctan2(sin_s, cos_s)) + 360) % 360

Deployment Checklist

• Timestamps converted to UTC and sorted monotonically before any bearing computation
• Duplicate timestamps deduplicated with heading-priority tie-breaking
• Low-speed threshold (`min_speed_kmh`) set per fleet type (5 km/h default; lower for cycling or pedestrian modes)
• Angular acceleration gate (`max_angular_rate_deg_s`) validated against actual vehicle turn data, not just a default value
• Circular arithmetic used throughout — no scalar subtraction on angular columns
• `heading_quality` flag persisted to the output table and indexed for downstream filtering
• OSRM `bearings` parameter supplied with appropriate tolerance per speed bracket
• Coordinate order (`lon,lat` vs `lat,lon`) verified for each routing engine endpoint
• Device heading reference type (COG / magnetic / true / chassis) documented per TCU model
• Pipeline order confirmed: coordinate denoising precedes heading synchronization
• Memory budget validated for maximum expected daily vehicle-trace volume
• Quality-0 records excluded from routing engine calls

Integration with IMU Fusion

For fleets operating in environments with frequent GPS signal loss — underground car parks, tunnels, dense urban canyons — GPS-derived heading gaps can extend beyond the 5–10 second range where circular interpolation remains reliable. In these cases, an inertial measurement unit can bridge the gap. See Correcting Heading Drift Using Accelerometer Fusion for a production complementary filter and Extended Kalman Filter implementation that synchronizes IMU yaw rates with GPS-derived azimuths.

The interface between this module and IMU fusion is the heading_quality flag: records with quality 2 or 3 spanning more than a configurable window (typically 8–12 seconds) should trigger an IMU fusion request rather than continued interpolation. Implement this as a gap detector over the quality column:

def detect_extended_gaps(df: pd.DataFrame, max_interp_seconds: float = 10.0) -> pd.Series:
    """Return boolean mask for rows that fall inside gaps too long for interpolation alone."""
    needs_fill = df["_needs_fill"]
    gap_start = needs_fill & ~needs_fill.shift(1, fill_value=False)
    gap_group = gap_start.cumsum().where(needs_fill, other=0)

    gap_durations = (
        df.groupby(gap_group)["timestamp"]
        .transform(lambda g: (g.max() - g.min()).total_seconds())
    )
    return needs_fill & (gap_durations > max_interp_seconds)

Related

• Correcting Heading Drift Using Accelerometer Fusion — IMU/GPS fusion for extended signal-loss intervals
• Hidden Markov Model Map Matching in Python — how synchronized heading feeds HMM emission and transition probabilities
• Speed Profiling from Raw GPS Coordinates — companion kinematic feature that shares the same low-speed validity constraints
• Kalman Filtering for GPS Noise Reduction — coordinate-level denoising that must precede heading synchronization
• Coordinate Reference System Mapping for Fleet Data — CRS projection patterns for distance-sensitive heading calculations

Parent: Trajectory Analysis & Map Matching Techniques