Localization and Mapping — Robotics

TL;DR

A robot never knows its pose for certain; it keeps a belief, a probability for every possible pose, and improves it in a loop of predict (move, become less sure) and update (measure, become more sure).
The Kalman filter does this for one bell-shaped guess: it blends prediction and measurement in proportion to how much each can be trusted, which makes it fast and, for linear systems with Gaussian noise, optimal.
A particle filter keeps hundreds or thousands of guesses at once, so it can hold several possibilities (two identical corridors) until the sensors settle the question.
An occupancy grid is a map of cells, each with a probability of being occupied, updated with every scan; log-odds make that update a simple addition.
SLAM builds the map and localizes in it at the same time; loop closure, recognizing a place seen before, is what removes the drift that has built up along the way.

The localization problem

Robot localization is working out a robot's pose (its position and heading) relative to a map, from its motion and its sensor readings. Wheel odometry alone cannot do it for long: it drifts without limit (see Mobile Robots and Odometry). The robot has to look around, recognize something whose position is known, and correct itself, much as you would on a walk in an unfamiliar town by spotting the church tower on your map.

Three versions of the problem are usually distinguished:

Position tracking: the start pose is known, and the robot only has to keep its estimate from drifting. The easiest case.
Global localization: the robot is switched on somewhere on a known map and has no idea where. It must consider every place at once.
The kidnapped robot: a well-localized robot is picked up and carried elsewhere (or its wheels slip badly), and it must notice that its confident belief is now wrong. A good test of any method, and not hypothetical: people move warehouse robots by hand.

Belief: a probability for every pose

Because sensors are noisy and wheels slip, a robot never knows its pose for certain. Localization methods therefore keep a belief: a probability distribution over all poses the robot could be in. The belief is updated in a loop of two steps that every method on this page shares:

The loop behind every filter on this page. They differ only in how they store the belief: as a table, as one bell curve, or as a cloud of samples.

The update step is Bayes' theorem: the new belief in a pose x is the old belief times the probability of the reading z if the robot really were at x, scaled so everything adds up to 1. The predict step moves the belief along with the motion u, spreading it out because the motion is uncertain:

\overline{\text{bel}}(x) = \sum_{x'} p(x \mid x', u)\, \text{bel}(x') \qquad \text{bel}(x) \propto p(z \mid x)\, \overline{\text{bel}}(x)

The bar marks the predicted belief, before the reading; the sign \propto means "proportional to" (divide by the total afterward). The vertical bar is read "given", as in conditional probability. This recipe is called the Bayes filter.

A corridor with doors

The smallest useful example: a robot in a corridor that loops around a building, split into 10 cells, with doors in cells 0, 3 and 7. Its only sensor tells door from wall, and is right 80% of the time. It starts with no idea where it is (every cell 10%), reads, moves one cell (sometimes zero or two), reads again, and so on. This version of the Bayes filter, with a table of probabilities, is called a histogram or grid filter:

# A robot in a ring-shaped corridor of 10 cells. Doors at cells 0, 3 and 7.
DOORS = {0, 3, 7}
CELLS = 10
P_CORRECT = 0.8   # the door detector is right 80% of the time

def sense(belief, saw_door):
    """Update: weight every cell by how well it explains the reading (Bayes)."""
    weighted = []
    for cell, p in enumerate(belief):
        is_door = cell in DOORS
        likelihood = P_CORRECT if is_door == saw_door else 1 - P_CORRECT
        weighted.append(p * likelihood)
    total = sum(weighted)
    return [w / total for w in weighted]

def move(belief):
    """Predict: the robot tries to move one cell right; 10% it stays, 10% it overshoots."""
    new = [0.0] * CELLS
    for cell, p in enumerate(belief):
        new[(cell + 1) % CELLS] += 0.8 * p
        new[cell] += 0.1 * p
        new[(cell + 2) % CELLS] += 0.1 * p
    return new

belief = [1 / CELLS] * CELLS          # no idea where we are: uniform
readings = [True, False, False, False, True]     # door, wall, wall, wall, door
for step, saw_door in enumerate(readings):
    if step:
        belief = move(belief)
    belief = sense(belief, saw_door)
    best = max(range(CELLS), key=lambda c: belief[c])
    print(f"{'door' if saw_door else 'wall'}: " + " ".join(f"{p:.2f}" for p in belief)
          + f"  -> cell {best} ({belief[best]:.0%})")

door: 0.21 0.05 0.05 0.21 0.05 0.05 0.05 0.21 0.05 0.05  -> cell 0 (21%)
wall: 0.02 0.21 0.08 0.02 0.21 0.08 0.06 0.02 0.21 0.08  -> cell 1 (21%)
wall: 0.03 0.06 0.22 0.03 0.06 0.22 0.11 0.02 0.05 0.22  -> cell 2 (22%)
wall: 0.07 0.08 0.11 0.07 0.08 0.11 0.30 0.04 0.05 0.10  -> cell 6 (30%)
door: 0.16 0.03 0.03 0.17 0.03 0.03 0.05 0.43 0.03 0.02  -> cell 7 (43%)

The last line of the output as a picture. The other two doors keep some belief, because a door reading could still come from them; one more step with a wall reading would move the peak to cell 8 (41%), following the robot.

After the first reading all three doors are equally likely, and the program simply reports the first of them. The pattern does the work: only one place in the corridor has a door, three walls and a door in a row, so after five readings cell 7 stands out. Nothing here is certain: 43% is a belief, not a fix, and a robot would act on it carefully. Notice also that the robot did not need to know its start; that is global localization.

A table works for 10 cells. A real floor of 50 × 50 m at 5 cm resolution with 360 headings has 360 million cells, and every prediction must touch them all. The next two methods store the belief far more cheaply.

The Kalman filter

The Kalman filter is a Bayes filter that stores the belief as a single bell curve (a Gaussian or normal distribution): a best estimate x and its variance P, the square of its standard deviation (see spread). R. E. Kalman published it in 1960, in "A New Approach to Linear Filtering and Prediction Problems" in the Journal of Basic Engineering, and it has been used in navigation ever since.

In one dimension, the two steps are four lines. Predict with a motion u whose own variance is Q; update with a reading z whose variance is R:

\begin{aligned} \text{predict:}\quad x &\leftarrow x + u, & P &\leftarrow P + Q \\ \text{update:}\quad K &= \frac{P}{P + R}, & x &\leftarrow x + K\,(z - x), \quad P \leftarrow (1 - K)\,P \end{aligned}

Everything rests on the Kalman gain K, a number between 0 and 1. If the estimate is very uncertain (large P) and the sensor is good (small R), K is close to 1 and the estimate jumps to the reading. If the estimate is already good and the sensor poor, K is small and the reading only nudges it. The difference z - x is called the innovation: what the reading says that the estimate did not expect.

Worked example: standing still

A robot stands still 10 m from a wall and measures the distance five times with a sensor whose noise has a standard deviation of 2 m, so R = 4. Its first guess is 0 m, but with a huge variance of 100, meaning "I really don't know". Nothing moves, so Q = 0 and only the update runs. The first step by hand: K = 100 / 104 = 0.9615, so the estimate jumps almost all the way to the reading: 0 + 0.9615 \times (10.2 - 0) = 9.8077.

# A 1D Kalman filter: the robot stands still 10 m from a wall and measures the
# distance with a sensor whose noise has a standard deviation of 2 m (R = 4).
x, P = 0.0, 100.0      # first guess: 0 m, but very unsure (variance 100)
R = 4.0                # measurement variance
for z in [10.2, 9.6, 10.5, 9.9, 10.1]:
    K = P / (P + R)                # Kalman gain: how much to trust the reading
    x = x + K * (z - x)            # move the estimate toward the reading
    P = (1 - K) * P                # and become more certain
    print(f"z={z:4}  K={K:.4f}  x={x:.4f}  P={P:.4f}")
print(f"check: 1 / (1/100 + 5/4) = {1 / (1 / 100 + 5 / 4):.4f}")

z=10.2  K=0.9615  x=9.8077  P=3.8462
z= 9.6  K=0.4902  x=9.7059  P=1.9608
z=10.5  K=0.3289  x=9.9671  P=1.3158
z= 9.9  K=0.2475  x=9.9505  P=0.9901
z=10.1  K=0.1984  x=9.9802  P=0.7937
check: 1 / (1/100 + 5/4) = 0.7937

The gain falls with every reading: each new measurement counts for less, because the estimate already rests on the earlier ones. With Q = 0 the variance after n readings follows a simple formula, and the program's last line checks it:

P_n = \frac{1}{1/P_0 + n/R}

Here the filter is simply computing a weighted average of the readings and the first guess, which is what you would do by hand. The point of the filter is that it gets there one reading at a time, without storing the readings, and that it keeps working when the robot moves.

Moving: prediction and correction

Now the robot drives toward the wall. Each step, odometry reports "moved 0.5 m" with a standard deviation of 0.2 m (Q = 0.04), and a range sensor with a standard deviation of 0.5 m (R = 0.25) measures the distance. The same robot with odometry alone is computed alongside for comparison:

# The robot now drives toward the wall. Each step odometry says "moved 0.5 m"
# with variance Q = 0.04 (sd 0.2 m); a range sensor measures the distance to the
# wall with variance R = 0.25 (sd 0.5 m).
Q, R = 0.04, 0.25

def predict(x, P, moved):
    return x - moved, P + Q          # closer to the wall, and less sure

def update(x, P, z):
    K = P / (P + R)
    return x + K * (z - x), (1 - K) * P

x, P = 10.0, 0.01                    # we start exactly 10 m from the wall
readings = [9.4, 9.1, 8.3, 8.2, 7.4, 7.1]
x_odo, P_odo = x, P                  # the same robot with odometry only
for z in readings:
    x, P = predict(x, P, 0.5)
    x, P = update(x, P, z)
    x_odo, P_odo = predict(x_odo, P_odo, 0.5)
    print(f"z={z}  filter x={x:.3f} sd={P ** 0.5:.3f}   odometry only x={x_odo:.3f} sd={P_odo ** 0.5:.3f}")

z=9.4  filter x=9.483 sd=0.204   odometry only x=9.500 sd=0.224
z=9.1  filter x=9.012 sd=0.248   odometry only x=9.000 sd=0.300
z=8.3  filter x=8.451 sd=0.269   odometry only x=8.500 sd=0.361
z=8.2  filter x=8.028 sd=0.278   odometry only x=8.000 sd=0.412
z=7.4  filter x=7.487 sd=0.283   odometry only x=7.500 sd=0.458
z=7.1  filter x=7.024 sd=0.285   odometry only x=7.000 sd=0.500

With odometry alone the uncertainty keeps growing, as dead reckoning always does. With the filter it levels off near 0.29 m, better than either source on its own: the sensor alone is good to 0.5 m, and odometry would soon be worse than that. Two imperfect sources, combined according to their uncertainty, beat both. This is sensor fusion in its simplest form.

Beyond straight lines

The Kalman filter is optimal when the motion and the measurements are linear and the noise is Gaussian. A robot's world is not linear: its x-position depends on the cosine of its heading, and a range reading to a landmark depends on a square root. Two common extensions handle that. The extended Kalman filter (EKF) replaces the curves with straight-line approximations around the current estimate, using derivatives; the unscented Kalman filter (UKF) pushes a few carefully chosen sample points through the real curves instead. Both still keep one bell curve, so both fail when the belief should have two peaks, as in the corridor above. In many robots an EKF fuses wheel odometry with the gyroscope of an inertial measurement unit, because both are smooth, frequent and roughly Gaussian.

Particle filters

A particle filter stores the belief as a cloud of samples, called particles: each particle is one guess of the robot's pose. Where particles are dense, the robot probably is. One round of the loop works on the whole cloud:

Predict: move every particle by the odometry, plus a bit of random noise of its own, so the cloud spreads like the real uncertainty.
Weight: give each particle a weight: how likely the actual sensor reading would be from that particle's pose.
Resample: draw a new cloud of the same size from the old one, picking heavy particles more often. Unlikely guesses die out; likely ones multiply.

Applied to localization on a map, this is Monte Carlo localization (MCL), introduced by Dellaert, Fox, Burgard and Thrun at the IEEE International Conference on Robotics and Automation (ICRA) in 1999. Its strength is that the cloud can take any shape: spread over the whole map at the start, split into two clusters in two identical corridors, and shrinking to one when the robot sees what tells them apart. The name you will often meet in ROS navigation is AMCL, short for adaptive Monte Carlo localization.

The example puts 1,000 particles anywhere in a 20 m corridor that loops around a building, with doors at 2, 7 and 15 m. The robot starts at 6 m, drives 1 m per step and reads its door detector (right 90% of the time). The randomness is seeded, so the run repeats exactly:

import math
import random

# A corridor that loops around a building: a ring of 20 m, doors centered at
# 2, 7 and 15 m. The robot's door detector sees a door when it is within
# 0.5 m of one, and is right 90% of the time.
LENGTH, DOORS = 20.0, (2.0, 7.0, 15.0)

def near_door(x):
    return any(abs((x - d + LENGTH / 2) % LENGTH - LENGTH / 2) <= 0.5 for d in DOORS)

def likelihood(x, saw_door):
    return 0.9 if near_door(x) == saw_door else 0.1

random.seed(42)
N = 1000
particles = [random.uniform(0, LENGTH) for _ in range(N)]   # anywhere at all

true_x = 6.0
for step in range(10):
    # Predict: the robot drives 1 m; each particle moves 1 m plus its own noise.
    true_x = (true_x + 1.0) % LENGTH
    particles = [(p + 1.0 + random.gauss(0, 0.1)) % LENGTH for p in particles]
    # Update: weight each particle by how well it explains the reading.
    saw_door = near_door(true_x)            # this simulated sensor happens to be right
    weights = [likelihood(p, saw_door) for p in particles]
    # Resample: draw a new set, favoring heavy particles.
    particles = random.choices(particles, weights=weights, k=N)
    close = sum(abs((p - true_x + LENGTH / 2) % LENGTH - LENGTH / 2) <= 0.5 for p in particles)
    print(f"step {step + 1}: true x={true_x:4.1f} m, {'door' if saw_door else 'wall'}, "
          f"{close / N:4.0%} of particles within 0.5 m")

step 1: true x= 7.0 m, door,  21% of particles within 0.5 m
step 2: true x= 8.0 m, wall,  22% of particles within 0.5 m
step 3: true x= 9.0 m, wall,  21% of particles within 0.5 m
step 4: true x=10.0 m, wall,  23% of particles within 0.5 m
step 5: true x=11.0 m, wall,  25% of particles within 0.5 m
step 6: true x=12.0 m, wall,  33% of particles within 0.5 m
step 7: true x=13.0 m, wall,  38% of particles within 0.5 m
step 8: true x=14.0 m, wall,  58% of particles within 0.5 m
step 9: true x=15.0 m, door,  86% of particles within 0.5 m
step 10: true x=16.0 m, wall,  89% of particles within 0.5 m

The first door reading leaves three clusters, one at each door. A long run of walls slowly removes the clusters that should have met a door by now, and the second door settles it: almost 90% of the particles now sit within half a meter of the truth. The (… + LENGTH / 2) % LENGTH - LENGTH / 2 pattern is the distance around a ring, so 19.8 m and 0.1 m count as close.

Occupancy grids

Localization needs a map. The most common map for mobile robots is an occupancy grid: the floor divided into small square cells (often 5 cm), each holding the probability that something occupies it. Hans Moravec and Alberto Elfes introduced the idea in 1985, to build maps from the wide, fuzzy beams of sonar sensors ("High resolution maps from wide angle sonar", ICRA 1985). Today the input is usually a lidar scan, but the principle is the same: a beam that ends at a cell is evidence that the cell is occupied, and a beam that passes through a cell is evidence that it is free. Cells no beam has reached stay at 0.5, "unknown".

Updating with log-odds

Updating a probability with Bayes' theorem on every scan means multiplying and dividing. A trick makes it an addition: store each cell as its log-odds, the natural logarithm of the odds that it is occupied:

l = \ln \frac{p}{1 - p} \qquad l \leftarrow l + l_{\text{reading}} \qquad p = 1 - \frac{1}{1 + e^{l}}

An unknown cell, p = 0.5, has log-odds 0. Each reading adds a fixed amount: positive for a hit, negative for a miss. Conflicting readings cancel, repeated readings add up, and a moving person who blocked a beam once is soon outvoted by later scans. Converting back to a probability is only needed when the map is used or drawn. The example uses a sensor model in which a hit means "70% occupied" and a pass-through means "40% occupied":

import math

def logit(p):
    return math.log(p / (1 - p))

def prob(l):
    return 1 - 1 / (1 + math.exp(l))

L_HIT = logit(0.7)    # a beam ended in this cell: evidence for "occupied"
L_MISS = logit(0.4)   # a beam passed through it: evidence for "free"
print(f"L_HIT={L_HIT:.4f}  L_MISS={L_MISS:.4f}")

# One cell, watched by several scans; it starts unknown (p = 0.5, log-odds 0).
l = 0.0
for event in ["hit", "hit", "miss", "hit", "hit"]:
    l += L_HIT if event == "hit" else L_MISS
    print(f"{event:4}: log-odds {l:+.3f}  p(occupied)={prob(l):.3f}")

# A row of 8 cells and a lidar in cell 0 that measures a wall in cell 5, twice.
row = [0.0] * 8
for scan in range(2):
    for cell in range(1, 5):
        row[cell] += L_MISS          # the beam passed through cells 1-4
    row[5] += L_HIT                  # and ended in cell 5
print("row:", " ".join(f"{prob(v):.2f}" for v in row))
print("map:", "".join("#" if prob(v) > 0.65 else "." if prob(v) < 0.35 else "?" for v in row))

L_HIT=0.8473  L_MISS=-0.4055
hit : log-odds +0.847  p(occupied)=0.700
hit : log-odds +1.695  p(occupied)=0.845
miss: log-odds +1.289  p(occupied)=0.784
hit : log-odds +2.136  p(occupied)=0.894
hit : log-odds +2.984  p(occupied)=0.952
row: 0.50 0.31 0.31 0.31 0.31 0.84 0.50 0.50
map: ?....#??

The map line shows the three states a planner cares about: free (.), occupied (#) and unknown (?): the robot's own cell and everything behind the wall. Planners treat unknown space with care, and exploring robots drive toward the border between free and unknown to fill in the map. How a planner finds a route through such a grid is on Path Planning.

SLAM

Localization needs a map, and building a map needs to know where each scan was taken. In a new building a robot has neither. Doing both at once is SLAM, simultaneous localization and mapping: the robot estimates its own trajectory and the map together, each improving the other. It sounds circular, and early on the errors in pose and map are indeed tangled: a wrong pose puts a wall in the wrong place, and that wall then misleads the next pose estimate.

In a survey in IEEE Robotics & Automation Magazine (2006), Hugh Durrant-Whyte and Tim Bailey trace the "genesis" of the probabilistic SLAM problem to the 1986 IEEE Robotics and Automation Conference; Randall Smith and Peter Cheeseman published a key paper on representing spatial uncertainty that year in the International Journal of Robotics Research. The acronym SLAM itself was first presented in a paper at the International Symposium on Robotics Research (ISRR) in 1995.

A modern SLAM system usually has two halves:

The front end processes the raw sensor data: it matches each lidar scan against the previous ones, or tracks visual features from frame to frame, and turns that into estimates of how the robot moved between poses.
The back end takes all those pose-to-pose estimates, each with its uncertainty, and finds the trajectory and map that agree with them best. Many systems store this as a graph of poses joined by measured constraints and solve it as a large least-squares problem.

SLAM with lidar is common on indoor robots and warehouse vehicles; visual SLAM with cameras is used on drones, phones (augmented reality) and low-cost robots; many systems fuse both with an IMU. Cameras, markers and depth are on Robot Vision and the sensors themselves on Sensors for Robots.

Loop closure

While a robot explores, its pose estimate drifts, just as odometry does, only more slowly. The decisive moment comes when it returns to a place it has seen before and recognizes it. Loop closure is that recognition, plus the correction that follows: the robot now knows that its current pose must match a pose from long ago, and the whole trajectory in between can be adjusted to agree. A loop closure can move a wall in the map by a meter in one go.

The toy below drives a 10 × 10 m loop, recording a pose every meter with a little random odometry error, and then "recognizes" the start. It spreads the gap between the last pose and the start evenly along the trajectory. Real back ends solve an optimization over every constraint, rotations included, but the effect is the same in spirit:

import math
import random
import statistics

# A robot drives around a 10 m x 10 m block and back to its start, recording a
# pose every meter. Each meter, odometry adds a small random position error.
random.seed(3)
truth, odometry = [(0.0, 0.0)], [(0.0, 0.0)]
for dx, dy in [(1, 0)] * 10 + [(0, 1)] * 10 + [(-1, 0)] * 10 + [(0, -1)] * 10:
    tx, ty = truth[-1]
    ox, oy = odometry[-1]
    truth.append((tx + dx, ty + dy))
    odometry.append((ox + dx + random.gauss(0, 0.05), oy + dy + random.gauss(0, 0.05)))

def mean_error(poses):
    return statistics.mean(math.dist(p, t) for p, t in zip(poses, truth))

end = odometry[-1]
print(f"odometry ends {math.dist(end, odometry[0]):.3f} m from where it started")

# Loop closure: the robot RECOGNIZES its starting point, so the last pose must
# equal the first. Toy correction: spread the gap over the whole trajectory,
# moving each pose in proportion to how far along the loop it is.
gap_x, gap_y = odometry[0][0] - end[0], odometry[0][1] - end[1]
n = len(odometry) - 1
corrected = [(x + gap_x * i / n, y + gap_y * i / n) for i, (x, y) in enumerate(odometry)]

print(f"mean error of all {n + 1} poses: before {mean_error(odometry):.3f} m, "
      f"after {mean_error(corrected):.3f} m")
print(f"worst pose: before {max(math.dist(p, t) for p, t in zip(odometry, truth)):.3f} m, "
      f"after {max(math.dist(p, t) for p, t in zip(corrected, truth)):.3f} m")

odometry ends 0.782 m from where it started
mean error of all 41 poses: before 0.390 m, after 0.178 m
worst pose: before 0.863 m, after 0.389 m

One recognized place cuts the average error by more than half here. With other seeds the gain is smaller, and occasionally this crude straight-line spreading even makes a few poses worse, which is exactly why real systems solve the full optimization instead.

Localization in practice

A typical indoor robot builds its map once with SLAM, while someone drives it around (or it explores by itself), saves the occupancy grid, and then localizes on that fixed map with a particle filter during normal work. In ROS 2 that work is done by Nav2, which describes itself as a "production-grade autonomy framework" and covers localization, planning and control (as of October 2026 it is actively supported for the Lyrical and Jazzy releases; see ROS 2). Outdoors, satellite positioning gives a rough absolute fix that never drifts, and is fused with odometry and an IMU.

Things that make localization hard in real buildings:

Symmetry: long identical corridors, rows of shelves, empty halls. The sensors cannot tell places apart.
Change: pallets, parked carts, open or closed doors and crowds. The saved map slowly stops matching the world; good systems tolerate readings that the map does not explain.
Glass and mirrors: lidar beams pass through glass or bounce off mirrors, so walls go missing or appear in the wrong place.
Bad starts: a robot switched on in the wrong place, or with a wrong initial pose typed in. Many systems let an operator set the start pose on the map by hand, which is often the first thing to check when a robot "gets lost".