M8-T2 — EKF Localization with Landmarks

Learning objectives

After this lesson, you should be able to:

• Describe the EKF localisation loop with several landmarks, processing the detections one at a time.
• Associate a detection with a landmark by the smallest Mahalanobis distance and reject it with a χ² gate.
• Explain why wrong associations are catastrophic and when ambiguity makes them likely.
• State the practical limits of the gate and the remedies that exist.

The intuition

The robot knows where five landmarks are (the black dots) and gets a range and a bearing for each one it sees, plus occasional false detections (a person's leg, a reflection). The detections arrive unlabelled. For each one the filter asks: given where I think I am, which landmark would produce this reading?

The filter can compute, for each landmark $j$, the reading it expects, $h_j(\bar\mu)$, and how far from it a reading may fall: the innovation covariance $S_j = H_j\bar\Sigma H_j^\top + R$. It combines the two into a single number, the squared Mahalanobis distance $d_j^2 = \mathbf{y}_j^\top S_j^{-1}\mathbf{y}_j$, which measures how surprising the reading would be if landmark $j$ were its source, in units of the expected scatter. Pick the smallest. If even the smallest exceeds the χ² threshold (9.21 for 99% and two degrees of freedom), accept that this detection is not any landmark and throw it away.

The bar chart shows what is at stake. With the right landmark known (the idealisation of the previous lesson) the error is 6 cm. With gating it is the same on the spread map (6.5 cm): the gate sorted 6 555 detections with 7 wrong, and threw away 795 false ones. Without a gate (always take the nearest landmark) the filter is lost: 96 cm, and a mean NEES of 408 (3 expected). A false detection is accepted as a landmark, corrects the pose by a large wrong amount, and the next wrong detection is matched with a wrong pose.

On the crowded map, where landmarks come in pairs 0.6 m apart, the gate has a harder job: 563 wrong associations out of 7 856, and the error rises to 8.7 cm (NEES 3.3). It is no longer a case of throwing away outliers but of choosing between two plausible landmarks, and nothing but the filter's own accuracy can decide.

A wrong association is worse than a missing one: it injects a confident, wrong correction. When in doubt, discard the detection. A filter that ignores a few good readings loses a little; a filter that believes a wrong one can lose the robot.

Definitions

Landmark map

Known positions $\mathbf{m}_1,\dots,\mathbf{m}_L$. The measurement function for landmark $j$ is $h_j(\mathbf{x}) = (\rho_j, \phi_j)$, range and bearing.

Data association $c_t$

The unknown index of the landmark that produced a detection (or “none”).

Mahalanobis distance $d_j^2$

$\mathbf{y}_j^\top S_j^{-1}\mathbf{y}_j$ with $\mathbf{y}_j = \mathbf{z} - h_j(\bar\mu)$ (bearing wrapped) and $S_j = H_j\bar\Sigma H_j^\top + R$.

Gate

A threshold on $d^2$ taken from the $\chi^2_2$ quantile: 9.21 accepts 99% of true detections, 5.99 accepts 95%.

Individual compatibility nearest neighbour (ICNN)

Choose $\hat c = \arg\min_j d_j^2$ and accept if $d^2_{\hat c} < \gamma$.

Derivation: the gate from the innovation distribution

If detection $\mathbf{z}$ really comes from landmark $j$ and the filter is consistent, its innovation is Gaussian with the known covariance. That distribution is the gate.

1. $$\mathbf{y}_j = \mathbf{z} - h_j(\bar\mu);\sim;\mathcal{N}(0, S_j)$$

   For the correct landmark, the innovation is zero-mean with covariance $S_j = H_j\bar\Sigma H_j^\top + R$ (innovation lesson, linearised here).

2. $$d_j^2 = \mathbf{y}_j^\top S_j^{-1}\mathbf{y}_j;\sim;\chi^2_2$$

   Whitening and summing two squared standard normals (the Mahalanobis result of module 4): χ² with two degrees of freedom, the measurement dimension.

3. $$P(d_j^2 < \gamma) = 1 - e^{-\gamma/2}\quad\Rightarrow\quad \gamma = -2\ln(0.01) = 9.21$$

   For two degrees of freedom the χ² tail has the closed form $e^{-\gamma/2}$. A 99% gate lets through 99% of true detections. One in a hundred correct detections will be wrongly thrown away; that is the price of the gate.

Sequential updates

When several detections arrive in one scan, and their noises are independent, the filter can process them one after another, using the posterior of one as the prior of the next. This gives the same result as a joint update with a block-diagonal $R$, at the cost of $2\times2$ inversions instead of one large one. It also lets each detection be gated against the pose as corrected by the earlier ones, which sharpens the gates.

What can go wrong

The gate is built from the filter's own covariance. When the filter is overconfident, $S$ is too small and good detections are rejected, so the filter stops correcting and gets more overconfident (divergence, as in module 7). When it is underconfident, $S$ is large and so is the gate: more false detections come in. When two landmarks are closer than the size of the gate, the nearest-neighbour rule has no information to choose, and when it chooses wrongly the correction pulls the robot by the distance between them.

The gate is a test against the filter’s own belief. It protects an accurate filter from outliers, but a filter that has already gone wrong cannot use it to recover.

A worked example

The map of the figure has five landmarks, a sensor range of 7 m, a range noise of 15 cm and a bearing noise of 0.05 rad. Each scan has a 30% chance of one false detection (uniform in range and bearing). We run three filters on 40 runs of 60 steps each, ignoring the first 10 steps:

1. Known associations. The ideal: each detection is given its landmark, false detections are discarded. Position error 6.3 cm, mean NEES 2.64 (3 expected).
2. Gated nearest neighbour. The detections come unlabelled. Of 6 555 accepted, 7 are wrong (a false detection that passed the gate); 795 are rejected. The error is 6.5 cm, the NEES 2.70: as good as the ideal.
3. Nearest neighbour without a gate. Every detection is assigned to something: 866 wrong associations out of 7 350. The filter is lost: 0.96 m, NEES 408.
4. Crowded map. With landmarks in pairs 0.6 m apart, the gated filter makes 563 wrong associations (7%), and the error is 8.7 cm against 7.4 cm for known associations. The ungated filter again fails (1.28 m).

import math
from robotprob import Rng, ekf_predict, ekf_update, mean, nees, chi2_ppf

dt, v, w = 0.5, 1.0, 0.2
sv, sw, sr, sb = 0.05, 0.03, 0.15, 0.05
SPREAD = [(4.0, 3.0), (-2.0, 6.0), (6.0, 8.0), (-1.0, 0.0), (0.0, 9.0)]
CROWDED = [(4.0, 3.0), (4.5, 3.4), (-2.0, 6.0), (-1.6, 5.7), (0.0, 9.0)]
LM = SPREAD
RANGE = 7.0
wrap = lambda a: math.atan2(math.sin(a), math.cos(a))
def f(x, u): return [x[0] + u[0]*dt*math.cos(x[2]), x[1] + u[0]*dt*math.sin(x[2]), wrap(x[2] + u[1]*dt)]
def F(x, u): return [[1, 0, -u[0]*dt*math.sin(x[2])], [0, 1, u[0]*dt*math.cos(x[2])], [0, 0, 1]]
def hh(lm):
    def h(x):
        dx, dy = lm[0]-x[0], lm[1]-x[1]
        return [math.hypot(dx, dy), wrap(math.atan2(dy, dx) - x[2])]
    return h
def HH(lm):
    def H(x):
        dx, dy = lm[0]-x[0], lm[1]-x[1]; q = dx*dx + dy*dy; r = math.sqrt(q)
        return [[-dx/r, -dy/r, 0.0], [dy/q, -dx/q, -1.0]]
    return H
Q = [[(sv*dt)**2, 0, 0], [0, (sv*dt)**2, 0], [0, 0, (sw*dt)**2]]
R = [[sr**2, 0], [0, sb**2]]
res = lambda z, zp: [z[0]-zp[0], wrap(z[1]-zp[1])]
gate = chi2_ppf(0.99, 2)

def run(seed, mode, steps=60, clutter=0.3):
    global LM
    rng = Rng(seed)
    xt = [0.0, 0.0, 0.0]
    x, P = [0.2, -0.2, 0.1], [[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.05]]
    wrong = used = rejected = 0; out = []
    for t in range(steps):
        xt = f(xt, (v + rng.gauss(0, sv), w + rng.gauss(0, sw)))
        x, P = ekf_predict(x, P, f, F, Q, (v, w))
        # the sensor: every landmark in range is detected, plus sometimes one clutter detection that belongs to nothing
        obs = []
        for i, lm in enumerate(LM):
            zt = hh(lm)(xt)
            if zt[0] < RANGE:
                obs.append((i, [zt[0] + rng.gauss(0, sr), wrap(zt[1] + rng.gauss(0, sb))]))
        if rng.random() < clutter:
            obs.append((-1, [rng.uniform(1, RANGE), rng.uniform(-math.pi, math.pi)]))
        for true_id, z in obs:
            if mode == 'known':
                if true_id < 0: continue
                j = true_id
            else:
                best, j = None, None
                for k, lm in enumerate(LM):
                    h, H = hh(lm), HH(lm)
                    y = res(z, h(x))
                    Hm = H(x)
                    S = [[sum(Hm[a][c]*P[c][d]*Hm[b][d] for c in range(3) for d in range(3)) + R[a][b] for b in range(2)] for a in range(2)]
                    det = S[0][0]*S[1][1] - S[0][1]*S[1][0]
                    d2 = (S[1][1]*y[0]**2 - 2*S[0][1]*y[0]*y[1] + S[0][0]*y[1]**2) / det
                    if best is None or d2 < best: best, j = d2, k
                if mode == 'gated' and best > gate:
                    rejected += 1
                    continue
                used += 1
                if j != true_id: wrong += 1
            x, P, y, S, K = ekf_update(x, P, z, hh(LM[j]), HH(LM[j]), R, res)
        out.append((math.hypot(x[0]-xt[0], x[1]-xt[1]), nees([x[0]-xt[0], x[1]-xt[1], wrap(x[2]-xt[2])], P)))
    return out, wrong, used, rejected

def table(name, lms):
    global LM
    LM = lms
    print(name)
    print('  mode      rms pos error  mean NEES  wrong associations  rejected  used')
    for mode in ('known', 'gated', 'ungated'):
        E, N, W, U, Rj = [], [], 0, 0, 0
        for seed in range(1, 41):
            o, wr, us, rj = run(seed, mode)
            E += [e*e for e, n in o[10:]]; N += [n for e, n in o[10:]]
            W += wr; U += us; Rj += rj
        print(' ', mode.ljust(8), str(round(math.sqrt(mean(E)), 3)).rjust(12), str(round(mean(N), 2)).rjust(10), str(W).rjust(14), str(Rj).rjust(11), str(U).rjust(8))
print('gate (99%, 2 dof):', round(gate, 2))
table('landmarks well apart', SPREAD)
table('landmarks in close pairs (0.6 m)', CROWDED)

gate (99%, 2 dof): 9.21
landmarks well apart
  mode      rms pos error  mean NEES  wrong associations  rejected  used
  known           0.063       2.64              0           0        0
  gated           0.065        2.7              7         795     6555
  ungated         0.955     408.18            866           0     7350
landmarks in close pairs (0.6 m)
  mode      rms pos error  mean NEES  wrong associations  rejected  used
  known           0.074       2.63              0           0        0
  gated           0.087       3.29            563         745     7856
  ungated         1.284     752.93           2084           0     8601

In robotics

Landmarks, scans, poles and tags

Landmark localisation appears in warehouses with reflective tags, in agricultural robots using tree trunks or poles, in AGVs with reflectors, and in drones with fiducial markers. In each, the identity of the landmark is either read directly (a coded tag: no association problem) or must be inferred, as here. The Navigation course covers the localisation problem from the particle-filter side in its localisation lessons.

Making association easier

• Make landmarks distinguishable by shape, colour, size or code, and use that as an extra test.
• Space landmarks well relative to the sensor and pose uncertainty. Pairs closer than the gate cannot be separated by geometry.
• Use the whole scan (joint compatibility): test all detections together against the map, which resolves ambiguities that individual tests cannot. Joint compatibility branch and bound (JCBB) does this; RANSAC-style methods do too.
• Delay the decision (multi-hypothesis filters track several associations until one wins).

Unknown maps

The same problem is harder in SLAM, where the map itself is uncertain and a new landmark is another hypothesis. The same gate then decides between “an existing landmark” and “a new one”, and the cost of a wrong decision is a corrupted map as well as a lost robot. Module 10 and 11 return to it.

Practical setting of the gate

A 99% gate is a common default. A tighter gate rejects more outliers but throws out good detections when the filter is slightly overconfident; a looser one lets more clutter in. Look at your logged NIS: the fraction of true detections rejected should be close to 1%.

Common misconceptions

Key takeaways

1. EKF localisation is a loop: predict, then for each detection compute expected readings and innovation covariances, associate, and update sequentially.
2. Associate by the smallest Mahalanobis distance d² = yᵀS⁻¹y and accept it only if it passes the χ² gate (9.21 for 99% with two degrees of freedom).
3. The gate protects an accurate filter from clutter: the gated filter matched the ideal (6.5 cm against 6.3 cm); the ungated one lost the robot (0.96 m).
4. Landmarks closer than the gate are ambiguous; wrong associations then cost accuracy, and in the worst case the robot.
5. Prefer distinguishable landmarks, well-spaced maps, joint tests, and discarding detections when in doubt.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 7.5 (EKF localisation with known and unknown correspondences) and section 10.2 (EKF SLAM). Paid book.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 . Free to read as the author’s PDF; the book is copyrighted.
• J. Neira and J. D. Tardós, “Data association in stochastic mapping using the joint compatibility test”, IEEE Transactions on Robotics and Automation 17 (6), 2001. Paid journal article.
• C. Stachniss, “Kalman Filter & EKF” (video lecture) . Free to watch.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Both the EKF and its association rules rest on linearisation. The next lesson replaces the Jacobian with a small set of sample points, the sigma points, which handle the curvature of the function better and need no derivatives.

Related content

Nothing below is required to finish this lesson.

• Before
  M8-T1: Linearization and the Extended Kalman Filter

  Derive the EKF by first-order expansion of the motion and measurement functions, and state where its approximation enters.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T5: Extended Kalman Filter
• SLAM M3-T7: EKF-SLAM
• SLAM M3-L5: Implement a Simplified EKF-SLAM