Localization & Map Matching

Every other stage in sensor fusion and spatial data alignment brings sensor data into a common frame. This one closes the loop the other way: it takes the fused observation and asks where the vehicle is relative to the map, which is the only question the planner can act on.

The two halves of that answer are different in kind. A metric pose with a covariance is a continuous estimate; a lane assignment is a discrete decision. Treating them as one problem is the most common structural mistake here — it produces stacks that report centimetre precision and put the vehicle in the wrong lane, and stacks that know the lane and cannot hold a lane centre.

The two questions, their consumers, and the accuracy each actually needs:

There is a third quantity, easy to leave implicit, that turns out to matter as much as either answer: the uncertainty attached to each. A pose without a covariance cannot be fused with anything, because a filter has no basis on which to weigh it against its own prediction; a lane assignment without a likelihood cannot be overruled when a later observation contradicts it. Both halves of this stage therefore have to report how much they should be believed, and both have to be able to report very little — which is the property that makes a tunnel survivable rather than a silent failure.

That requirement shapes the whole design. It rules out any component that returns a bare answer, which excludes most convenient library defaults; it means the scan matcher has to expose its own optimisation internals rather than just its result; and it means the map-matching stage has to keep the runner-up hypotheses rather than committing to the best one per frame. None of that is expensive, and all of it is easy to skip in a first implementation, which is why the first implementation is usually the one that reports centimetre confidence in a tunnel.

The other structural decision worth making early is where the map-relative estimate lives relative to the vehicle's own odometric frame. Keeping them separate — a smooth, drift-prone odometric pose and a jumpy, drift-free map pose, related by a transform that is itself estimated — lets a control loop consume the smooth one while a planner consumes the map one, and lets a correction be applied without discontinuity in the frame the controller sees. Collapsing them into a single pose is simpler and puts every map correction straight into the control loop as a step.

Approach Comparison #

Four approaches are in production use, and the right answer is usually two of them running together.

Scan matching against the map carries the metric pose and graph-constrained matching carries the lane assignment; GNSS provides the coarse prior that keeps scan matching inside its basin of convergence, exactly as registration method selection describes. Nearest-lane snapping is listed because it is what most stacks start with, and because its failure mode — a lane assignment that flickers between neighbours — is the reason the graph constraint exists.

Stage-by-Stage Implementation #

Stage 1 — Establish the metric pose against the map #

The constraint: the map-relative pose has to be estimated against map features, not against the previous frame, or the error is a random walk. Scan matching supplies this by registering the live sweep to the map's own geometry using the machinery in point cloud registration techniques.

The output is not a pose but a pose and a covariance, and the covariance is the part downstream stages actually consume. A scan match that returns a pose with no uncertainty is unusable in a filter, because the filter has no basis on which to weight it against the prediction.

Stage 2 — Match to a lane over a window, not per frame #

The constraint: the lane assignment must be temporally consistent, and consistency comes from the graph rather than from smoothing. Treating the sequence of poses as observations of a hidden lane sequence — a hidden Markov model whose transitions are the lane graph's own edges — makes an assignment that disagrees with connectivity impossible rather than merely unlikely.

import numpy as np

def emission_logprob(pose, lane, sigma_lat: float = 0.35) -> float:
    """How well this pose is explained by being in this lane."""
    d = lane.lateral_offset(pose.xy)
    inside = abs(d) <= lane.width / 2.0
    return -0.5 * (d / sigma_lat) ** 2 + (0.0 if inside else -2.0)

The extra penalty for being outside the lane body is deliberate and small: it biases toward the containing lane without making a genuinely straddling pose impossible, which happens legitimately during a lane change.

Stage 3 — Constrain transitions by the lane graph #

def transition_logprob(prev_lane, next_lane, G, change_penalty: float = 2.5) -> float:
    if prev_lane == next_lane:
        return 0.0
    if not G.has_edge(prev_lane, next_lane):
        return -np.inf                       # the map says this cannot happen
    kind = G[prev_lane][next_lane].get("kind")
    return -change_penalty if kind == "lane_change" else -0.4

Returning negative infinity for a non-edge is what makes this a map-aware matcher rather than a smoother: no amount of observation likelihood can produce a lane sequence the road does not permit. The lane-change penalty is what keeps a noisy stretch from being explained as a rapid sequence of changes.

What the graph constraint fixes, on the same pose sequence:

Stage 4 — Degrade honestly where the map stops helping #

The constraint: when the map-relative correction becomes unobservable, the covariance must grow. The failure this prevents is a localizer that reports the same 0.05 m uncertainty inside a tunnel that it reports in a structured urban scene, so every consumer keeps trusting a pose that is drifting.

Observability is measurable from the geometry itself: the structure tensor of the matched map features tells you which directions are constrained, and a small eigenvalue in the along-track direction is exactly the tunnel case. Inflating the covariance along the weak eigenvector, rather than isotropically, keeps the lateral estimate — which is still good — usable.

Validation & QC Automation #

def assert_localization(track, ground_truth, lanes) -> None:
    lat = lateral_errors(track, ground_truth)
    assert np.percentile(np.abs(lat), 95) <= 0.20, "lateral p95 over budget"

    along = along_track_errors(track, ground_truth)
    assert np.percentile(np.abs(along), 95) <= 1.0, "along-track p95 over budget"

    flips = sum(1 for a, b in zip(track.lanes, track.lanes[1:]) if a != b)
    real = ground_truth.lane_change_count
    assert flips <= real + 1, f"{flips} lane transitions against {real} real changes"

    inside = np.mean([e <= 3.0 * s for e, s in zip(np.abs(lat), track.sigma_lat)])
    assert inside >= 0.99, "covariance is optimistic — errors exceed 3σ too often"

The enforced thresholds: lateral p95 ≤0.20 m; along-track p95 ≤1.0 m; lane transitions within one of the real count; and a covariance whose 3σ bound contains ≥99% of the errors, which is the check that catches an over-confident filter.

Edge Cases & Failure Patterns #

A tunnel with no lateral structure. Along-track observability collapses. Inflate along the weak eigenvector, not isotropically, and expect the pose to drift at the odometry's rate until the exit.

A newly resurfaced road. Scan matching degrades because the map's surface features no longer exist. This is a map-freshness problem, and it is the signal that feeds change detection and map maintenance.

A lane change during a matching window. The window smooths across it and reports the change late. Shorten the window or run a forward filter alongside the smoother; a late lane assignment is safer than a flickering one but not free.

A confident match to the wrong carriageway. Dual carriageways separated by less than the pose uncertainty are genuinely ambiguous; use heading, which differs by 180°, as an emission term rather than relying on position alone.

Covariance that never grows. The filter's process noise is tuned for a well-observed scene. Derive the inflation from the observability of the matched features rather than from a fixed schedule.

What degrades first in each environment, which is the fact a fallback policy has to encode:

Performance & Scale Notes #

Map matching is cheap: the candidate lane set is the handful within a few metres, and the Viterbi pass over a one-second window is microseconds. Scan matching dominates, at tens of milliseconds per frame, and its cost is governed by the point counts discussed in accelerating ICP with a KD-tree and voxel downsampling.

What does scale badly is the map query. Keep the resident map indexed spatially and query the ring around the pose rather than the tile, or the candidate set grows with tile density and the matcher's cost becomes a function of where the vehicle is rather than of what it is doing.

FAQ #

What is the difference between localization and map matching? #

Localization estimates a metric pose — where the vehicle is, in metres, with a covariance. Map matching answers a discrete question — which lane it is in — and the two are not interchangeable. A pose with 0.2 metre uncertainty is excellent localization and still ambiguous between two adjacent lanes; a confident lane match with a metre of along-lane error is useless for lateral control. Production stacks run both and use each where it is strong.

Why match to a lane rather than to the nearest point? #

Because the nearest point has no memory and the road does. A per-frame nearest-lane query flips between adjacent lanes whenever noise exceeds half the lane width, producing a match that jitters even though the vehicle drove straight. Matching over a window, with transitions constrained by the lane graph's own connectivity, turns the question into one the map can help answer: the vehicle can only be in a lane its previous lane connects to.

What happens where the map has no distinctive features? #

The map-based correction becomes unobservable in at least one direction, and the honest response is to widen the covariance rather than to keep reporting a confident pose. In a tunnel or a long featureless cutting the along-track direction goes first, because every cross-section looks alike; lateral stays observable from the walls. A localizer that reports the same covariance there as in a structured urban scene is the failure this stage exists to prevent.

How accurate does map matching need to be? #

Lateral accuracy has to be comfortably better than half a lane width so the lane assignment is unambiguous — around 0.2 metres against a 3.2 metre lane. Along-track accuracy can be looser, a metre or so, because its consumers are stop lines and junction entries rather than lateral control. Stating the two separately is what keeps a stack from over-engineering the easy axis to meet a requirement that only applies to the hard one.

Related #

• Map-Matching GNSS Traces to Lane-Level Geometry — the graph-constrained matcher in full.
• NDT Localization Against an HD Map — the metric pose half.
• Fusing Odometry and Map Constraints with an EKF — combining the two with an honest covariance.
• Registration Method Selection — choosing the scan matcher this stage depends on.
• Change Detection & Map Maintenance — what a persistent localization degradation feeds into.

Up one level: Sensor Fusion & Spatial Data Alignment — the pipeline whose fused output this stage consumes.