VehicleMapping HD AV Spatial Data

Map-Matching GNSS Traces to Lane-Level Geometry

A GNSS fix with a metre of lateral uncertainty cannot, on its own, say which of two adjacent lanes a vehicle is in. A sequence of such fixes can, because the road constrains what sequences are possible: a vehicle that was in lane 2 is in lane 2 or an adjacent lane, never three lanes over, and the lane graph already knows that.

This task builds that matcher for localization and map matching — a hidden Markov model whose hidden states are lanes and whose transitions are the graph's own edges.

The three terms that decide a match, and what each one can and cannot resolve:

What Each Scoring Term Can Resolve Three panels naming a scoring term, the ambiguity it resolves and the ambiguity it leaves. lateral offset resolves: distant lanes fails: adjacent lanes when σ > half a lane width heading agreement resolves: opposing carriageways fails: parallel lanes same direction, same heading transition constraint not an edge resolves: impossible sequences the only term using more than one fix the first two score a fix in isolation; the third is what turns a sequence of guesses into a trajectory a matcher with only the first two is a nearest-lane snap with extra steps

Prerequisites #

  • Python 3.10+, NumPy 1.24+, shapely 2.0+, networkx 3.x.
  • Input: a GNSS trace with per-fix accuracy and heading, and the lane graph with geometry attached.
  • Upstream stage: the lane graph from road network graph construction, including lane-change edges.
  • Output: one lane assignment per fix, plus the log-likelihood of the chosen sequence.

Step-by-Step #

1. Build candidates from the fix's own accuracy #

python
import numpy as np

def candidates(fix, lane_index, min_r: float = 3.5, max_r: float = 40.0):
    """Lanes within 3σ of the fix, floored at a lane width and capped."""
    r = float(np.clip(3.0 * fix.sigma_m, min_r, max_r))
    return list(lane_index.query_within(fix.xy, r))

Scaling by the reported accuracy is what keeps the matcher honest in an urban canyon: the fix knows it is poor, and the candidate set widens to include the lane the vehicle is actually in rather than confidently excluding it.

2. Score the emission from offset and heading #

python
def emission(fix, lane, sigma_lat=1.0, sigma_hdg=0.35) -> float:
    d = lane.lateral_offset(fix.xy)
    dh = np.arctan2(np.sin(fix.heading - lane.heading_at(fix.xy)),
                    np.cos(fix.heading - lane.heading_at(fix.xy)))
    return -0.5 * (d / sigma_lat) ** 2 - 0.5 * (dh / sigma_hdg) ** 2

Key parameters: sigma_lat should track the fix's reported accuracy rather than being constant; sigma_hdg at 0.35 rad tolerates normal heading noise while making a 180° disagreement effectively impossible. Wrapping the heading difference through arctan2 is what stops a vehicle heading due north from being scored against a lane at 359° as though they disagreed by a full turn.

3. Constrain transitions to the graph #

python
def transition(prev_lane, next_lane, G, change_cost=2.5, succ_cost=0.3) -> float:
    if prev_lane == next_lane:
        return 0.0
    if not G.has_edge(prev_lane, next_lane):
        return -np.inf
    return -change_cost if G[prev_lane][next_lane]["kind"] == "lane_change" else -succ_cost

The -inf is the whole point. A smoother makes an impossible sequence unlikely; this makes it impossible, so no amount of GNSS noise can produce a lane assignment the road does not permit.

4. Decode over a bounded window #

python
def viterbi(fixes, G, lane_index, window: int = 8) -> list:
    out, prev = [], None
    for i in range(0, len(fixes), window):
        chunk = fixes[i:i + window]
        states = [candidates(f, lane_index) for f in chunk]
        score = {s: emission(chunk[0], s) + (0.0 if prev is None
                                             else transition(prev, s, G))
                 for s in states[0]}
        back = [{s: prev for s in states[0]}]

        for t in range(1, len(chunk)):
            nxt, bp = {}, {}
            for s in states[t]:
                best, arg = -np.inf, None
                for p, sc in score.items():
                    v = sc + transition(p, s, G)
                    if v > best:
                        best, arg = v, p
                if np.isfinite(best):
                    nxt[s] = best + emission(chunk[t], s)
                    bp[s] = arg
            score, back = nxt, back + [bp]
            if not score:
                raise RuntimeError("no admissible lane sequence — check the graph")

        end = max(score, key=score.get)
        path = [end]
        for bp in reversed(back[1:]):
            path.append(bp[path[-1]])
        out.extend(reversed(path))
        prev = end
    return out

Carrying prev across windows is what keeps the boundaries from behaving like restarts; without it the assignment can jump at every window edge, reintroducing the flicker the matcher exists to remove.

Which term resolves each ambiguity, and the one case where none of them can:

Which Scoring Term Resolves Each Ambiguity Four rows pairing an ambiguity with the scoring term that resolves it, including one case none of them resolves. four ambiguities, and the term that settles each two adjacent lanes within half a lane width the transition term over a window resolved opposing carriageways closer than the fix uncertainty heading, 180° apart resolved overlapping turn lanes same asphalt, different exits heading plus the graph resolved lane against hard shoulder parallel and graph-adjacent none of the three needs a lane-type prior the last row is why lane type belongs in the emission model as a prior rather than being left to geometry

Verification & Acceptance Criteria #

python
def assert_matching(assigned, truth, G) -> None:
    acc = np.mean([a == t for a, t in zip(assigned, truth)])
    assert acc >= 0.98, f"lane accuracy {acc:.3f} below 0.98"

    for a, b in zip(assigned, assigned[1:]):
        assert a == b or G.has_edge(a, b), "an impossible transition was emitted"

    flips = sum(1 for a, b in zip(assigned, assigned[1:]) if a != b)
    real = sum(1 for a, b in zip(truth, truth[1:]) if a != b)
    assert flips <= real + 1, f"{flips} transitions against {real} real changes"

Acceptance gate: lane accuracy ≥98% against a hand-labelled trace; zero transitions the graph does not contain — which is structural rather than statistical, so a single violation is a bug; and a transition count within one of the truth.

What the window length costs and buys, which is why an offline trace and a live stack use different values:

Viterbi Window Length: Stability Against Latency Four rows pairing a Viterbi window length with its stability and the latency it introduces. window length at 1 Hz GNSS 1 fix no transition term at all flips whenever noise exceeds half a lane instant, useless 3 fixes some smoothing still chases noise on a bad stretch ~1.5 s late 8 fixes the live default stable, transitions match reality ~4 s late 30 fixes offline reprocessing maximally stable ~15 s late the same matcher runs both settings — the window is a parameter, not a different algorithm

Common Errors & Fixes #

The matcher reports the opposing carriageway. Heading is not in the emission, or the heading difference is not wrapped. Add both.

No admissible sequence at a junction. The lane graph lacks the junction expansion, so entry and exit lanes have no path between them. Expand junctions as in building routing graphs from OpenDRIVE junctions.

Assignments jump at window boundaries. prev is not carried across chunks. Thread it through.

Accuracy is high and the trace still flickers on one stretch. The lane-change cost is too low relative to the emission noise there. Raise it, or better, feed the fix's own reported accuracy into sigma_lat so noisy stretches automatically weight the transition term more heavily.

The candidate set is empty in a canyon. The radius is fixed. Scale it by the reported accuracy.

FAQ #

Why include heading in the emission model? #

Because position alone cannot separate the two carriageways of a dual carriageway when they are closer together than the fix uncertainty, and getting that wrong sends the vehicle the wrong way. Heading differs by 180 degrees between them, which is far outside any plausible noise, so a heading term resolves the ambiguity that a position term cannot. It also helps at junctions, where several candidate lanes overlap in space and diverge in direction.

How long should the Viterbi window be? #

Long enough that a genuine lane change dominates the noise, short enough that the answer is not late. At 1 hertz GNSS and urban speeds that is around five to ten fixes — roughly one to two lane-change durations. A longer window is more stable and reports changes later, which for an offline trace is free and for a live stack is a real cost, so the same matcher is normally run with different window lengths in the two settings.

What should the candidate search radius be? #

Scaled by the fix's own reported accuracy rather than fixed — about three standard deviations, floored at a lane width and capped at something that keeps the candidate set small. A fixed radius is either too tight in an urban canyon, where the true lane is excluded and the matcher confidently picks a wrong one, or too loose on open road, where the candidate set fills with lanes the vehicle could not be in.

Up one level: Localization & Map Matching — the stage this matcher implements.