VehicleMapping HD AV Spatial Data

Fusing Odometry and Map Constraints with an EKF

Odometry is smooth, available everywhere and drifts without bound. A map-relative scan match does not drift and is unavailable exactly where the map is featureless. Fusing them is the standard answer, and the standard way to get it wrong is to fuse them with hand-tuned constant covariances, which throws away the one thing each source knows about itself.

This task builds the filter for localization and map matching so that both sources carry their own uncertainty, outliers are gated rather than tuned around, and the lane assignment contributes as the weak lateral constraint it actually is.

What each input contributes, and where each stops contributing:

Three Sources and Where Each One Stops Helping Three uncertainty traces across a drive with a tunnel section, showing odometry growth, scan-match collapse along track, and the constant weak lane constraint. tunnel 0 high σ odometry σ — grows, reset by each update scan-match along-track σ — collapses in the tunnel lane pseudo-measurement — weak, lateral, constant nothing in the filter needs to know a tunnel exists — the covariances describe it which is why a hand-tuned constant covariance cannot reproduce this behaviour

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (stats.chi2).
  • Input: wheel and IMU odometry, a scan-match pose with covariance, and a lane assignment.
  • Upstream stage: NDT localization against an HD map for the pose, and the graph matcher for the lane.
  • Output: a fused pose and covariance at the odometry rate.

Step-by-Step #

1. Propagate with odometry #

python
import numpy as np

def predict(x, P, u, dt, Q):
    """Advance the planar state [x, y, theta] with a velocity/yaw-rate input."""
    v, w = u
    th = x[2]
    x_next = x + dt * np.array([v * np.cos(th), v * np.sin(th), w])
    F = np.array([[1.0, 0.0, -dt * v * np.sin(th)],
                  [0.0, 1.0,  dt * v * np.cos(th)],
                  [0.0, 0.0,  1.0]])
    return x_next, F @ P @ F.T + Q * dt

Scaling the process noise by dt rather than adding a per-step constant is what makes the filter behave identically at 50 Hz and 100 Hz — a surprisingly common source of a filter that was tuned at one rate and is over-confident at another.

2. Update from the scan match, using its covariance #

python
def update(x, P, z, R, H=None):
    H = np.eye(3) if H is None else H
    y = z - H @ x
    y[2] = np.arctan2(np.sin(y[2]), np.cos(y[2]))       # wrap the heading residual
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x_new = x + K @ y
    P_new = (np.eye(len(x)) - K @ H) @ P
    return x_new, P_new, y, S

R is the matcher's own reported covariance, not a constant. Wrapping the heading residual is not optional: an unwrapped residual near ±π produces a correction of nearly a full turn.

3. Gate on Mahalanobis distance #

python
from scipy.stats import chi2

GATE = chi2.ppf(0.99, df=3)      # ≈ 11.34

def accept(y, S) -> bool:
    return float(y @ np.linalg.inv(S) @ y) <= GATE

The gate is derived, not chosen, so it can be reasoned about: at 99% confidence a valid observation is rejected one time in a hundred, and the rejection rate becomes a diagnostic. A gate firing on 20% of frames is telling you the covariances are wrong, not that the sensor is faulty.

4. Add the lane constraint as a lateral pseudo-measurement #

python
def lane_pseudo_measurement(x, P, lane, width_frac: float = 0.33):
    """The map says the vehicle is near this lane's centre — laterally only."""
    n = lane.normal_at(x[:2])                   # unit lateral direction
    H = np.array([[n[0], n[1], 0.0]])           # observes lateral offset only
    z = np.array([0.0])                          # target: zero offset from centre
    y = z - H @ x + H @ lane.centre_at(x[:2]).reshape(-1)
    R = np.array([[(width_frac * lane.width) ** 2]])
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    return x + (K @ y).ravel(), (np.eye(3) - K @ H) @ P

Key parameters: width_frac at 0.33 makes the constraint weak — one standard deviation is about a metre on a normal lane — so it cannot overrule a good scan match and can still stop a long odometric stretch from drifting laterally out of the lane. The observation matrix has a single row precisely because the map says nothing about along-track position.

Why the lane constraint has to be lateral only:

Lateral-Only against Two-Dimensional Lane Constraint Two estimated tracks through a tunnel, one drifting only along track inside its lane, the other pulled to a fabricated along-track position. lateral-only — stays in lane, 4 m along-track error, honestly reported true position two-dimensional — pulled to a nominal station the vehicle never reached and reported confidently the map genuinely constrains one axis; applying it to both invents information

Verification & Acceptance Criteria #

python
def assert_filter(track, truth) -> None:
    err = np.abs(np.array(track.xy) - np.array(truth.xy))
    nis = np.array(track.nis)                      # normalized innovation squared
    assert 1.0 <= nis.mean() <= 5.0, f"NIS mean {nis.mean():.2f} — covariances are wrong"

    rej = np.mean(track.gated)
    assert rej <= 0.05, f"{rej:.1%} of updates gated — check R, not the sensor"

    inside = np.mean([e <= 3.0 * s for e, s in zip(err[:, 0], track.sigma_lat)])
    assert inside >= 0.99, "filter is over-confident"

Acceptance gate: mean normalized innovation squared between 1 and 5 for a three-dimensional measurement — the standard consistency check, and the one that catches both over- and under-confident covariances; gate rejection rate ≤5%; and ≥99% of lateral errors inside 3σ.

What the consistency statistics say, which is the only way to tell a tuned filter from a plausible one:

Reading the Filter Consistency Statistics Four rows pairing a normalized-innovation-squared reading with what it says about the filter covariances. normalized innovation squared, three degrees of freedom mean ≈ 0.5 far below 3 covariances too large ignoring good updates mean ≈ 3 the expected value covariances consistent the target mean ≈ 12 far above 3 covariances too small over-confident mean ≈ 3, gating > 5% right on average wrong in the tail check R, not the gate none of the four is visible in the estimate itself — a filter can look smooth and correct in every one of them

Common Errors & Fixes #

The estimate slides during a tunnel and reports high confidence. A constant R is being used. Pass the matcher's Hessian-derived covariance through.

Heading jumps by nearly 2π occasionally. The innovation is not wrapped. Wrap it in the update.

The gate rejects a fifth of all updates. The covariances are inconsistent, usually because process noise is far too small. Check the NIS before touching the gate.

The filter behaves differently at a higher odometry rate. Process noise is added per step rather than scaled by dt.

The lane constraint fights the scan match. width_frac is too small, making a weak prior into a strong one. A third of the lane width is about right; anything under a fifth starts to overrule real observations.

FAQ #

Why use the matcher's own covariance rather than a tuned constant? #

Because the matcher knows something the tuner cannot: how observable the pose was in that particular frame. A constant covariance makes the filter trust a tunnel frame exactly as much as an urban one, so the estimate follows a scan match that is sliding along the tunnel. Feeding the Hessian-derived covariance through means the filter automatically leans on odometry where the map stops helping, which is the behaviour a tuned constant is always trying and failing to approximate.

What is a lane pseudo-measurement? #

An observation the filter treats like a sensor reading but which comes from the map: given a lane assignment, the vehicle is near that lane's centre, laterally, with an uncertainty of roughly a third of the lane width. It is weak information, and that is the point — it cannot fight a good scan match, and it stops a purely odometric stretch from drifting laterally out of its lane. Applying it along-track as well is the mistake, since the map says nothing about where along the lane the vehicle is.

How should the Mahalanobis gate be set? #

From the chi-square distribution for the measurement's dimension at a chosen confidence — about 11.3 for three degrees of freedom at 99 percent. Setting it by eye either rejects good updates during genuine manoeuvres or admits the occasional wildly wrong scan match that a symmetric environment produced. Counting rejections is as important as making them: a gate that fires on more than a few percent of frames is reporting that the covariances are wrong, not that the sensor is.

Up one level: Localization & Map Matching — the stage this filter closes.