Geometry, Mapping, and 3D/4D State › Simultaneous Localization and Mapping

This section covers how an agent builds and maintains geometric state over time: the coupling between localization and mapping, the classical EKF solution, occupancy grids for dense maps, and semantic extensions. The unifying theme is accumulation under uncertainty: single-frame estimates are noisy and partial, and the SLAM family of methods is about fusing many of them into a consistent whole without letting errors compound.

To build a map from a moving sensor, you need to know where each observation was taken. To know where you are, it helps to have a map to match against. This chicken-and-egg structure is the defining feature of SLAM: simultaneous localization and mapping. It is worth spelling out why neither half alone suffices. Odometry, the integration of motion measurements, gives localization without a map, but without an absolute or map-relative correction its uncertainty accumulates and dead-reckoning drift is generally unbounded over long horizons. Individual zero-mean errors can cancel on a particular realization; it is the growing uncertainty, and systematic bias when present, that makes uncorrected odometry unreliable. Matching against a map gives map-relative localization that can bound or correct accumulated odometric drift, though map, calibration, association, and environmental errors remain. In an unknown environment without an external absolute reference such as a surveyed map, GNSS, motion capture, or known landmarks, the SLAM solution is to estimate pose and map together and let each constrain the other. Formally, the agent wants the joint posterior:

$$p(\mathbf{x}_{1:t}, m \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})$$

over the entire trajectory $\mathbf{x}_{1:t}$ and the map $m$, given all observations and controls. Full-trajectory SLAM is a smoothing problem because today's map depends on where the robot was yesterday. EKF-SLAM, by contrast, is an online filtering formulation over the current pose and persistent map state. When the robot realizes, upon recognizing a landmark, that its pose from an hour ago was wrong, every observation taken since then was attached to the map at the wrong place and must be corrected retroactively. This is why SLAM is fundamentally harder than the filtering problems of Part II: the state whose history matters keeps growing. In Part II we met Bayesian filtering over a belief state; SLAM is what happens when the belief state grows to include a model of the world itself, and the state and the map become statistically coupled through every shared observation. The connection to world models is direct: SLAM is the classical instance of maintaining a persistent, action-conditioned belief over scene structure, updated by every observation and corrected by every act of re-observation.

The extended Kalman filter approach makes the belief explicit. For the planar example below, the robot pose is $\mathbf{x}=(x,y,\theta)$ and each landmark $\mathbf{l}_j$ is a point in $\mathbb{R}^2$. The state vector stacks that pose and all landmark positions:

$$\mu = \begin{bmatrix} \mathbf{x} \\ \mathbf{l}_1 \\ \vdots \\ \mathbf{l}_M \end{bmatrix}, \qquad \Sigma \in \mathbb{R}^{(3+2M) \times (3+2M)}$$

The key structural fact is that $\Sigma$ is dense: observing a landmark from a new pose correlates the landmark estimate with the pose estimate and, through the pose, with every other landmark. This density is not an accident or an implementation nuisance; it is the mathematical expression of the chicken-and-egg coupling. If the pose is uncertain, then a landmark's inferred location inherits that uncertainty, and so does every other landmark placed relative to the same pose. Each measurement update tightens the whole web. Algorithmically, each step is the familiar predict-then-update cycle, with the motion model linearized in the pose block and the measurement model (range and bearing to a landmark) linearized jointly in the pose and landmark blocks. Linearization is needed because both models are nonlinear in the pose, through sines and cosines of the heading and through the arctangent in the bearing, and the EKF evaluates the local Jacobian at the current mean. As everywhere with extended Kalman filtering, the linearization is only trustworthy within a neighborhood, so large innovations or long un-observed stretches can push the estimate to where the linearization misleads.

Let's implement a compact EKF-SLAM. To keep the demo focused on the estimation machinery, we make three explicit simplifications: the robot follows a known circular path with noisy odometry, measurements are range-bearing to landmarks with known data association (we are told which landmark each measurement corresponds to; the real problem of deciding this is the association problem), and landmark initialization uses the first measurement directly. Each simplification hides a real difficulty. The association problem, deciding whether this observation is of a previously mapped landmark or a new one, is a discrete inference problem layered on top of the continuous one, and a single wrong association can corrupt the entire dense covariance. Initialization from a single range-bearing reading is possible only because the sensor directly measures both range and angle; with a camera, a first sighting constrains only a ray, and the landmark's depth stays unknown until parallax accumulates. The time schedule also matters: the filter consumes the measurements at $t=0$ without advancing the state, then performs one odometry prediction from $t-1$ to $t$ before applying the measurements indexed by $t$. Thus every stored estimate, measurement set, and ground-truth pose with the same index refers to the same simulation time. Because the demo has only relative range-bearing observations and unknown landmarks, its global $SE(2)$ frame is not identifiable from the measurements. The imperfect initial pose prior supplies the simulator-frame anchor, so the raw pose and map errors below are unaligned and depend on that gauge choice.

Recognition-based loop closure is itself an inference problem, often approached with learned or hand-crafted place descriptors followed by geometric or probabilistic verification. False loop closures are especially dangerous because a wrong long-range constraint can warp an otherwise consistent map.

Landmarks are sparse. For planning, agents often want a dense answer to a dense question: for each cell of space, what is the probability it is occupied? Collision checking and path planning must reason about every point the robot might sweep through; detected features alone are insufficient. A landmark map tells you where distinctive things are; an occupancy map estimates where space is free, occupied, or unknown and provides a key input to collision checking and path planning. Occupancy grids discretize space into cells and maintain, per cell, a Bernoulli belief. Two strong assumptions concern the map itself, and it is important to state them:

• Cell independence: the map posterior is approximated as factorizing across cells conditional on the pose and measurement history. This is clearly false at boundaries, where adjacent cells belong to the same surface, but it buys an enormous simplification: instead of one inference problem over the joint map, we get a grid of tiny independent problems.
• Static world: occupancy does not change over time, so evidence can be accumulated indefinitely. This is what allows the filter to integrate over the whole history, and it is also what breaks when furniture moves.

The standard recursive update also conditions on a known or estimated sensor pose and treats measurements as conditionally independent given the map, pose, and inverse sensor model. Correlated rays or pose errors violate that approximation and can make repeated evidence accumulate too confidently.

Under these map and measurement assumptions, each cell admits a small binary filter, and the standard trick is to run it in log-odds form. For occupancy probability $p$, define $l = \log \frac{p}{1-p}$. Bayes updates multiply odds, which become additions in log space; the update for cell $i$ given observation $\mathbf{z}$ is:

$$l_i \leftarrow l_i + \log \frac{p(m_i \mid \mathbf{z}_t)}{p(\lnot m_i \mid \mathbf{z}_t)} - \log \frac{p(m_i)}{p(\lnot m_i)}$$

The first term is the inverse sensor model: given "this ray passed through here and stopped there," how much does that raise or lower each cell's occupancy? Rays mark traversed cells as likely free and their endpoint as likely occupied. Everything before a valid endpoint is evidence against occupancy because the measurement indicates that the beam traversed those cells before returning from the endpoint. The endpoint is evidence for occupancy because something returned or stopped the beam there. The subtraction of the prior log-odds prevents repeatedly re-adding prior belief on every update; without it, a cell would drift toward the prior's preference every time any ray touched it, even a completely uninformative one. Clipping keeps probabilities from saturating at 0 or 1, preserving the ability to revise beliefs later. This last detail is easy to dismiss and important to keep: a cell that has saturated at "certainly free" cannot absorb future evidence that it is occupied, and in any environment where the map might be wrong or the world might change, certainty is a liability.

The deeper lesson for world-model builders is what this filter does with negative evidence. A cell being consistently observed as free is information, just as real as an obstacle. The ray that traveled through a cell and kept going has testified that nothing solid was there, and after enough such rays the cell's probability drops far below the prior. Learned world models must therefore be evaluated for whether they distinguish unknown space from sensor-supported free space. Occupancy grids are a reminder that an observed ray traversal is evidence of free space under the sensor model. Mere absence of a measurement is not automatically free-space evidence, because it may reflect occlusion, field of view, dropout, or missing coverage. A state representation that cannot distinguish "unknown" from "known empty" will mislead any planner that relies on it.

Let's build a small occupancy map from simulated lidar.

The reconstructed map records occupied boundary cells for all three obstacles, but notice the structure of what it does not know: obstacle interiors, cells behind obstacles, and regions no ray ever entered sit at or near the prior probability 0.5 unless another ray supplies evidence. This shadowing is not a bug. It is the honest representation of perspective dependence, and it is precisely what a naive "render the world from the last frame" world model loses. A rendered frame shows surfaces that happened to be visible; it has no way to say that the region behind the pillar is unverified, and a planner reading it has no way to ask. This distinction supports information-aware planning: a planner can prefer to gather information before committing when its objective explicitly penalizes uncertainty or rewards information gain. This is an early instance of a theme that will recur throughout the book: a state representation with honest uncertainty does not just support estimation, it actively shapes behavior, driving exploration and information gathering as rational responses to what the state admits it does not know.

Occupancy answers "is something here?" Most tasks also need "what is here?" A robot that knows a region is occupied but not whether it is a table, a person, or a pile of boxes can still choose a conservative detour or gather more information. Occupancy alone cannot choose well among all task-specific responses, such as whether waiting or interaction is appropriate. Semantic maps attach class distributions to geometric elements, either per cell (Bayesian kernel semantic fusion: each labeled observation casts a kernel of evidence into the grid) or per object in a landmark-style map. The per-cell version treats every classified pixel or point as a noisy categorical observation of the cell it lands in, and fuses those observations with the same Bayesian accumulation machinery as occupancy; the per-object version instead maintains discrete object hypotheses with class posteriors, which is more compact and more natural for manipulation but requires solving the object segmentation and data association problems first. A prominent representation in autonomous-driving research is 3D semantic occupancy: a volumetric grid where every cell carries both occupancy and class, effectively fusing detection and mapping into one representation. Its appeal is that it answers, in a single queryable structure, both the geometric question a planner asks and the semantic question a policy asks, without requiring objects to have been detected as objects at all. Semantic maps introduce new failure modes that pure geometry avoids: class priors can override evidence (the map "knows" tables do not float), and semantic errors accumulate through categorical, often correlated likelihoods rather than exactly like geometric errors. Multi-view semantic fusion can improve a class estimate, but it lacks the same direct metric constraint that triangulation provides for depth. Two cameras seeing the same surface from different angles can cross-validate a depth estimate; classifiers seeing the same object from different angles may share biases and agree on a wrong label with high confidence. Semantic disagreement is therefore often harder to diagnose than geometric inconsistency.