Chapters 10 and 11 assumed a map was already available. In practice, a robot deployed in an unknown environment must build the map while navigating — the simultaneous localisation and mapping (SLAM) problem. This chapter explains how to build an occupancy grid from sensor data and introduces the conceptual architecture of SLAM without implementing a full probabilistic back-end.
The occupancy grid introduced in Chapter 10 stores the probability that each cell is occupied. The log-odds representation is more convenient for updating:
$l_{ij} = \log \frac{P(occ_{ij})}{1 - P(occ_{ij})}$
Converting back: $P(occ) = 1 / (1 + e^{-l})$.
The log-odds update rule for a new observation $z$:
| $l_{ij} \leftarrow l_{ij} + l_{sensor}(z | occ_{ij}) - l_0$ |
where $l_0 = 0$ (prior probability 0.5), and $l_{sensor}$ is the sensor model.
For the HC-SR04, the inverse sensor model assigns log-odds based on the cell’s position relative to the measured range $d_{meas}$:
$\sigma_r = 0.05$ m accounts for sensor range noise.
def update_from_scan(grid, robot_pose, angle_deg, dist_m):
rx, ry, rtheta = robot_pose
beam_angle = rtheta + math.radians(angle_deg)
# Ray-cast from robot to measured distance
for d in np.arange(0.02, dist_m - 0.05, grid.res):
cx = rx + d * math.cos(beam_angle)
cy = ry + d * math.sin(beam_angle)
r, c = grid.world_to_cell(cx, cy)
if grid.in_bounds(r, c):
grid.log_odds[r, c] -= 0.4 # free
# Mark obstacle cell
ox = rx + dist_m * math.cos(beam_angle)
oy = ry + dist_m * math.sin(beam_angle)
r, c = grid.world_to_cell(ox, oy)
if grid.in_bounds(r, c):
grid.log_odds[r, c] += 0.9 # occupied
After many observations, cells converge to high log-odds (reliably occupied) or low log-odds (reliably free).
As the robot moves, it takes ultrasound scans at each pose and updates the grid. The robot pose comes from odometry (Chapter 5). Each update is a call to update_from_scan for each scan beam.
For a full 19-beam scan:
for angle, dist in scan.items():
update_from_scan(grid, odometry.get_pose(), angle, dist)
After exploring a room, the grid distinguishes walls (high log-odds) from open floor (low log-odds) and unvisited areas (near-zero log-odds).
The problem: the map update depends on knowing the robot’s pose, but pose estimation (odometry) drifts over time. If the robot returns to a previously visited area, the grid will show double walls because the accumulated pose error causes observations to be placed in wrong cells.
SLAM solves this by jointly estimating:
| The posterior is $P(\mathbf{x}_{1:t}, m | z_{1:t}, u_{1:t})$ where $z$ are sensor observations and $u$ are motion commands. |
The key event in SLAM is loop closure: recognising that the robot has returned to a previously visited location. This observation allows the back-end to correct the entire trajectory — reducing accumulated drift retroactively.
Scan matching. Compare the current ultrasound scan with previous scans stored at known locations in the grid. If the current scan matches a stored scan (using ICP — Iterative Closest Point — or correlation), the robot’s position relative to that stored scan can be estimated, providing a pose correction.
Visual loop closure. Using the camera, extract appearance descriptors from key frames (ORB or BRIEF features, or CNN embeddings on constrained hardware). Compare descriptors to a database of past frames. A high descriptor similarity triggers loop closure.
Graph SLAM represents the SLAM problem as a pose graph:
Each edge has an associated covariance expressing measurement uncertainty. The back-end optimises node positions to minimise the total constraint violation (a nonlinear least-squares problem, solved by g2o or GTSAM in production systems).
For the platform in this book, a simplified version is achievable:
This is not full SLAM, but it demonstrates the correction loop that SLAM formalises.
Once a map is available, the robot must localise within it. The particle filter (Monte Carlo Localisation, MCL) represents the pose distribution as a set of $N$ particles, each a hypothesis $(x, y, \theta)$:
With 200–500 particles and ultrasound observations, MCL converges to a reliable pose estimate in most indoor environments within 5–10 seconds.
SLAM and localisation are deep topics with extensive literature. This chapter provides the conceptual foundation needed for Chapter 15’s autonomous navigation pipeline. For production SLAM, see Thrun, Burgard and Fox (2005), Probabilistic Robotics.
Navigation: