M6-T2 — Histogram Filters and Grid Localization

Learning objectives

After this lesson, you should be able to:

• Implement the prediction and correction steps of the Bayes filter on a grid, with the prediction as a convolution with a motion kernel.
• Discretize a Gaussian motion into a kernel over cell shifts, and choose the cell size from the noise.
• Describe how a multi-modal belief evolves during global localization.
• Estimate the memory and computation of a grid filter, and explain how a model that allows no errors makes a robot unable to recover.

The intuition

Take the corridor of the last lesson and make it a realistic size: 12 m long, divided into 60 cells of 20 cm. It has three identical doors, at 2 m, 5 m and 7 m. Rover starts somewhere in the first 1.5 m, but does not know it. Its belief is flat across the 60 cells, 1.7% each. It then drives forward in steps of 0.6 m (three cells, give or take one), and after each step its door detector reports “door” or “wall”.

The heat map shows the filter at work, row by row. At the top the belief is flat. When the detector first says “wall”, nothing much is learned, since most of the corridor is wall. When it says “door” at step 3, the belief concentrates in three bands, one at each door, shifted to where the robot would be now if it had been at that door. These are three hypotheses about where the robot started. As the robot moves on, each band travels with it, and the readings test them: after a door, three cells of wall and then another door, only the hypothesis that started near the first door is consistent, since the spacing between the first two doors (3 m) differs from that between the last two (2 m). The other bands fade as their predicted readings fail to match the real ones. By step 12 the true position holds 93% of the belief.

Note two properties that make the grid filter attractive. It needs no assumption about the shape of the belief: three peaks are as easy as one. And its two steps are simple loops over a list, with no calculus. The price is that the list must cover every place the robot might be, which gets expensive quickly as the number of dimensions grows.

The kidnapped robot

What if the robot is picked up and put down elsewhere, a test known as the kidnapped-robot problem? The filter is confident about the wrong place, and the true place has a tiny probability. If that probability is not exactly zero, the readings gradually raise it and the filter recovers. If the sensor model allows no errors at all (a door detector that is “never wrong”), a single contradictory reading multiplies the true cell's belief by zero, and it can never come back. In the experiment below, such a model collapses completely after the robot is carried away, and the same model with a 2% allowance for “none of the above” recovers in 7 steps.

A grid filter represents any belief and is easy to implement, but its cost is the number of cells. Never let a probability become exactly zero: leave room for the model to be wrong.

Definitions

Cell $i$

One of the $N$ regions the state space is cut into. The belief is a vector $b = (b_1, \dots, b_N)$ that sums to 1.

Resolution $\Delta$

The width of a cell, in the unit of the state (metres, radians).

Motion kernel $k(s)$

The probability that a motion shifts the robot by $s$ cells.

Convolution $(b * k)$

The operation that replaces every entry of $b$ by a weighted sum of its neighbours, with weights from $k$.

Global localization

Finding the pose from a flat belief, with no initial guess.

Kidnapped robot

Moving the robot to an unknown place without telling it, to test whether it can recover.

Derivation: the kernel, the cost and the choice of cell size

Prediction is a convolution

1. $$\overline{b}_j = \sum_i p(j \mid i, u)\, b_i$$

   The prediction step of the Bayes filter, with the integral replaced by a sum over cells.

2. $$p(j \mid i, u) = k(j - i)$$

   When the chance of moving from cell $i$ to cell $j$ depends only on the shift $j - i$ (the robot does not behave differently at different places) it can be written with a one-argument kernel.

3. $$\overline{b}_j = \sum_i k(j - i)\, b_i = (b * k)_j$$

   This is the definition of a discrete convolution. Each cell’s belief is spread over its neighbours by the same pattern. Probability is conserved because $\sum_s k(s) = 1$.

If the motion also depends on the state (the robot turns, so the direction of the shift depends on its heading) the kernel changes from cell to cell and the sum no longer has this form, but the loop over cells is the same.

The kernel of a Gaussian motion

1. $$k(s) = P\bigl((s - \tfrac12)\Delta \le D < (s + \tfrac12)\Delta\bigr),\quad D \sim \mathcal{N}(\mu, \sigma^2)$$

   A displacement $D$ that falls in the interval of width $\Delta$ centred on a shift of $s$ cells is counted as a shift of $s$ (rounding to the nearest cell).

2. $$k(s) = F_D\bigl((s + \tfrac12)\Delta\bigr) - F_D\bigl((s - \tfrac12)\Delta\bigr)$$

   The probability of an interval is a difference of the cumulative distribution (the density lesson).

3. $$\mu = 0.6\text{ m},\; \sigma = 0.1\text{ m},\; \Delta = 0.2\text{ m}: \quad k(3) = \Phi(1) - \Phi(-1) = 0.683,\; k(2) = k(4) = \Phi(-1) - \Phi(-3) = 0.157$$

   The numbers of the worked example. The kernel is a short list: shifts more than three standard deviations from the mean have negligible weight and can be dropped.

How coarse may the grid be?

Rounding a position to the nearest cell centre adds an error that is uniform over a width $\Delta$, with variance $\Delta^2/12$ (the quantization noise of the sensor lesson). It adds in quadrature to the physical noise:

1. $$\sigma_{\text{grid}} = \sqrt{\sigma^2 + \Delta^2/12} = \sigma\sqrt{1 + \dfrac{1}{12}\Bigl(\dfrac{\Delta}{\sigma}\Bigr)^2}$$

   Independent errors combine in quadrature (the variance lesson).

2. $$\Delta = \sigma \;\Rightarrow\; \sigma_{\text{grid}} = 1.04\,\sigma, \qquad \Delta = 2\sigma \;\Rightarrow\; \sigma_{\text{grid}} = 1.15\,\sigma$$

   A cell as wide as the noise inflates the uncertainty by 4%; one twice as wide by 15%. A cell width of the order of the noise, or somewhat smaller, is a good rule.

Cost

With $d$ state variables and cells of size $\Delta_i$ over a range $L_i$, the number of cells is $N = \prod_i L_i/\Delta_i$. Each prediction touches every cell and every kernel entry, $N\prod_i K_i$ multiply-adds with $K_i \approx 6\sigma_i/\Delta_i + 1$ entries per dimension. The cost grows exponentially with the dimension: this is why the grid filter is used for one or two dimensions (a corridor, a floor plan) and rarely for a full 3D pose.

A histogram filter is exact up to the cell size and costs one number per cell. Choose a cell width of about one noise standard deviation, and expect the cost to explode with every added dimension.

A worked example

The 60-cell corridor: doors at 2 m, 5 m and 7 m, a door detector that is wrong 5% of the time, a command of 0.6 m (3 cells, ±1). Rover starts at cell 3 (0.6 m) with a flat belief. The program simulates it, records the belief, and tests recovery.

1. Global localization. At step 1 the belief is almost flat: 9.7% of the mass is within two cells of the truth, entropy 5.58 bits (the flat value is $\log_2 60 = 5.9$). At step 3, after the first door, the most likely cell is wrong (cell 26, the second door) and only 26% is near the truth: three hypotheses. At step 8 the filter has chosen the right one (65%); by step 12, after 3 doors, 93%. In the last steps the entropy hovers around 2.4 bits: the position along the corridor is known to about a cell, but the belief keeps a small spread as the motion noise accumulates between informative readings.
2. The kernel. From $N(0.6\text{ m}, 0.1\text{ m})$ on 0.2 m cells: shifts 2, 3, 4 get 0.157, 0.683, 0.157 and shifts 1 and 5 get 0.0013. The example above used a simpler kernel (0.1, 0.8, 0.1), which corresponds to a slightly sloppier robot.
3. Kidnapped. At step 12 the robot is picked up and put down at cell 5. With a detector wrong 5% of the time the belief recovers in 17 steps whether or not extra uniform probability is mixed in, because the model itself never says “impossible”. With a detector modelled as never wrong, the very first contradictory reading kills the belief at the new position, and the filter ends with every cell ruled out. Mixing in 2% of a uniform belief each step gives that same model a recovery in 7 steps.
4. Cost. The 1D corridor needs 60 cells and 300 multiply-adds per prediction. A 20 × 20 m floor at 10 cm has 40 000 cells and a million operations per step. A full pose (x, y, heading at 2°) has 7.2 million cells and 900 million operations per prediction: too slow for a robot that must update ten times a second.

import math
from robotprob import Rng, hist_predict, hist_update, entropy, gaussian_cdf

N = 60                                              # a 12 m corridor in cells of 0.2 m
door_cells = set(list(range(9, 12)) + list(range(24, 27)) + list(range(34, 37)))   # doors at 2 m, 5 m and 7 m
kernel = {2: 0.1, 3: 0.8, 4: 0.1}                   # a command of 0.6 m is 3 cells, give or take one

def like(z, p_err):                                 # P(reading | cell): the detector is wrong with probability p_err
    out = []
    for i in range(N):
        door = i in door_cells
        out.append(((1 - p_err) if door else p_err) if z == 'door' else (p_err if door else (1 - p_err)))
    return out

def localize(p_err, mix=0.0, teleport_at=None, steps=20):
    rng = Rng(5)
    true, bel, log, recovered = 3, [1.0 / N] * N, [], None
    for t in range(1, steps + 1):
        true = min(N - 1, true + rng.discrete([0.1, 0.8, 0.1]) + 2)
        if t == teleport_at:
            true = 5                                # the robot is picked up and put down at cell 5
        bel = hist_predict(bel, kernel)             # prediction: blur with the motion kernel
        z = 'door' if rng.random() < (1 - p_err if true in door_cells else p_err) else 'wall'
        try:
            bel = hist_update(bel, like(z, p_err))  # correction: multiply and normalize
        except ValueError:
            return log, 'every cell was ruled out'
        if mix > 0:                                 # leave room for the model to be wrong
            bel = [(1 - mix) * b + mix / N for b in bel]
        best = max(range(N), key=lambda i: bel[i])
        near = sum(bel[max(0, true - 2):true + 3])
        log.append((t, true, z, best, near, entropy(bel)))
        if teleport_at and t >= teleport_at and recovered is None and abs(best - true) <= 2 and bel[best] > 0.3:
            recovered = t - teleport_at
    return log, recovered

log, _ = localize(0.05)
for t, tr, z, best, near, h in log:
    if t in (1, 3, 5, 8, 10, 12, 14, 18):
        print('step', str(t).rjust(2), ' true cell', str(tr).rjust(2), ' read', z.ljust(4), ' most likely cell', str(best).rjust(2), ' mass within 2 cells of the truth', round(near, 3), ' entropy', round(h, 2), 'bits')

for p_err, mix in ((0.0, 0.0), (0.0, 0.02), (0.05, 0.0), (0.05, 0.02)):
    _, rec = localize(p_err, mix, teleport_at=12, steps=45)
    print('picked up at step 12; sensor error rate', p_err, ', uniform mixing', mix, '-> steps to recover:', rec)

cell, mu, sigma = 0.2, 0.6, 0.1                     # a Gaussian motion discretized into cell shifts
shift_p = {s: gaussian_cdf((s + 0.5) * cell, mu, sigma) - gaussian_cdf((s - 0.5) * cell, mu, sigma) for s in range(0, 7)}
print('kernel from N(0.6 m, 0.1 m):', {s: round(p, 4) for s, p in shift_p.items() if p > 1e-3}, ' total', round(sum(shift_p.values()), 4))

for name, cells, kernel_size in (('1D, 12 m at 0.2 m', 60, 5), ('2D, 20 x 20 m at 0.1 m', 200 * 200, 25), ('3D (x, y, heading), 20 x 20 m at 0.1 m and 2 deg', 200 * 200 * 180, 125)):
    print(name.ljust(52), 'cells', str(cells).rjust(9), ' multiply-adds per prediction', cells * kernel_size)

step  1  true cell  6  read wall  most likely cell 59  mass within 2 cells of the truth 0.097  entropy 5.58 bits
step  3  true cell 11  read door  most likely cell 26  mass within 2 cells of the truth 0.265  entropy 4.44 bits
step  5  true cell 18  read wall  most likely cell 32  mass within 2 cells of the truth 0.287  entropy 4.34 bits
step  8  true cell 27  read wall  most likely cell 27  mass within 2 cells of the truth 0.651  entropy 3.09 bits
step 10  true cell 33  read wall  most likely cell 33  mass within 2 cells of the truth 0.463  entropy 3.22 bits
step 12  true cell 39  read wall  most likely cell 39  mass within 2 cells of the truth 0.934  entropy 1.89 bits
step 14  true cell 45  read wall  most likely cell 45  mass within 2 cells of the truth 0.923  entropy 2.31 bits
step 18  true cell 57  read wall  most likely cell 57  mass within 2 cells of the truth 0.96  entropy 2.42 bits
picked up at step 12; sensor error rate 0.0 , uniform mixing 0.0 -> steps to recover: every cell was ruled out
picked up at step 12; sensor error rate 0.0 , uniform mixing 0.02 -> steps to recover: 7
picked up at step 12; sensor error rate 0.05 , uniform mixing 0.0 -> steps to recover: 17
picked up at step 12; sensor error rate 0.05 , uniform mixing 0.02 -> steps to recover: 19
kernel from N(0.6 m, 0.1 m): {1: 0.0013, 2: 0.1573, 3: 0.6827, 4: 0.1573, 5: 0.0013}  total 1.0
1D, 12 m at 0.2 m                                    cells        60  multiply-adds per prediction 300
2D, 20 x 20 m at 0.1 m                               cells     40000  multiply-adds per prediction 1000000
3D (x, y, heading), 20 x 20 m at 0.1 m and 2 deg     cells   7200000  multiply-adds per prediction 900000000

In robotics

Where grids are the right tool

A grid filter is the natural choice when the state is one- or two-dimensional or genuinely discrete: a position along a corridor or a rail, a room in a building (a topological map), a lane on a road, a discrete operating mode of a machine (“nominal”, “sensor stuck”, “motor degraded”), or a Wi-Fi fingerprint cell. A hidden Markov model is exactly this filter. The Robot Foundations course lets you build one in Where Am I?, and the SLAM course covers grid localization in Bayesian Estimation.

Grids for the full pose

Markov localization with a 3D grid (x, y, heading) was the first successful global localizer for indoor robots, run on fast computers with coarse cells and sparse updates. It has since been replaced by particle filters (module 9), which place their computation where the probability is, instead of everywhere. The table above gives the reason: 7 million cells to update when only a few hundred matter.

Efficient implementations

Several tricks keep grids practical: updating only the cells that carry significant probability (a sparse grid), working with a coarse grid and refining near the peak, using an FFT for large convolutions, and computing the sensor likelihood once per cell from a precomputed map (the likelihood-field idea of M5-T3: Range, Bearing and Beam Observation Models).

The bounded belief as a safety net

Whatever the filter, a floor on the belief (or a small mixture with a uniform distribution) is cheap insurance against an overconfident model, a map that has changed, or a robot that has been moved. It costs a little accuracy in the normal case. Practical localizers go further and add random hypotheses on purpose (M9-T4: Monte Carlo Localization in Practice).

Resolution is a decision

The cell size should follow the noise: finer than the noise wastes computation, coarser degrades accuracy by Δ²/12 in variance. For a corridor with 10 cm of motion noise, 10 cm cells are about right, and 1 cm cells would cost ten times as much for no benefit.

Common misconceptions

Key takeaways

1. A histogram filter keeps one probability per cell. Prediction spreads each cell’s mass by the motion kernel (a convolution if the motion is the same everywhere); correction multiplies by the likelihood and normalizes.
2. The kernel of a Gaussian motion is the area of the Gaussian over each cell shift: k(s) = Φ((s + ½)Δ − μ)/σ) − Φ((s − ½)Δ − μ)/σ).
3. The grid represents any belief, including several peaks, which suits global localization: hypotheses survive until the readings contradict them.
4. Cell width adds Δ²/12 to the variance: about one noise standard deviation is a good size. The cost is the number of cells, which grows exponentially with the dimension.
5. Never allow exact zeros. A sensor model that allows no errors, or a belief with no floor, can lose the truth forever after a kidnapping.

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 4.1 (the histogram filter) and chapter 7 (Markov localization). Paid book.
• C. Stachniss, “Bayes Filter” (video lecture) , discrete Bayes filter and the corridor example. Free to watch.
• S. Thrun, Artificial Intelligence for Robotics (Udacity CS373) , the localization lessons implement exactly this filter. Free course.
• D. Fox, W. Burgard and S. Thrun, “Markov localization for mobile robots in dynamic environments”, Journal of Artificial Intelligence Research 11, 1999. The grid localizer, with the kidnapped-robot discussion. Free to read as an open-access journal article.

Knowledge check

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

What’s next

A grid stores the belief by brute force. The next lesson compares the three ways of storing it (grid, Gaussian, samples) and shows what each can and cannot represent.

Related content

Nothing below is required to finish this lesson.

• Before
  M6-T1: The Bayes Filter

  Derive the prediction and correction steps of the Bayes filter and state which assumption each line of the derivation uses.

• Lab
  M6-L1: Histogram Localization

  Rover is switched on somewhere in a corridor with three identical doors. Implement the predict and update steps and watch three hypotheses collapse into one.

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

• SLAM M2-T5: Bayesian Estimation
• Robot Foundations 05: Where Am I?