Robot Perception, Localization, Mapping, and Planning

A mobile robot never observes its own position directly; it must infer state such as position, heading, and velocity from a stream of noisy, indirect sensor measurements combined with an imperfect model of how its actions change that state. Thrun, Burgard, and Fox's textbook Probabilistic Robotics frames the general solution to this problem as the Bayes filter: rather than committing to a single best-guess state, the robot maintains a full probability distribution, called its belief, over every state it might plausibly be in, and recursively refines that belief in two alternating steps. The prediction step propagates the belief forward whenever the robot acts, using a motion model to describe how the previous belief should spread out under the uncertainty of that action; the correction step then incorporates a new sensor measurement via Bayes' rule, sharpening the belief in the directions the new evidence supports and dampening the directions it contradicts. Nearly every localization and mapping algorithm used in mobile robotics, from Kalman filters to particle filters, is best understood as a specific way of representing and updating this same predict-then-correct belief cycle, differing mainly in what mathematical form the belief distribution is allowed to take.

Rudolf Kalman's 1960 paper "A New Approach to Linear Filtering and Prediction Problems" derives the optimal recursive estimator for exactly one such form: a single Gaussian distribution, summarized compactly by a mean and covariance, under the assumption that both the motion model and the sensor model are linear functions corrupted by Gaussian noise. This compactness makes the Kalman filter extremely efficient, but real robot kinematics are rarely linear, which is why mobile robotics typically uses the Extended Kalman Filter, approximating a nonlinear motion or sensor model with a local, first-order linearization around the current state estimate at every step. Even a well-calibrated, unbiased motion model accumulates error this way: consider a robot navigating by dead reckoning, integrating wheel-encoder-based velocity estimates over time, where each of 25 successive position updates contributes an independent error with variance 4 cm². Because the errors are independent, their variances simply add, giving a total accumulated variance of 25 × 4 = 100 cm² and therefore an accumulated standard deviation of √100 = 10 cm after just 25 steps. Uncertainty compounds with the square root of the number of steps even when nothing is going wrong at any individual step, which is precisely why dead reckoning alone is never sufficient for long-term navigation and must be corrected with external, exteroceptive sensing.

A single Gaussian belief has a structural limitation that becomes critical in the "kidnapped robot" problem, where a robot has no prior idea where it is on a known map, or must recover after being moved without warning: a Gaussian can only express one hypothesis about the robot's location at a time, no matter how it is parameterized. Dellaert, Fox, Burgard, and Thrun's 1999 paper "Monte Carlo Localization: Efficient Position Estimation for Mobile Robots" addresses this by representing belief instead as a set of weighted random samples, or particles, each one a concrete hypothesis about the robot's full state. Each particle is propagated forward under the motion model with independently sampled noise during prediction, then reweighted according to how well its hypothesized state matches the newly received sensor measurement during correction, and particles are periodically resampled in proportion to their weights so that implausible hypotheses are pruned away while promising ones are replicated. Because a particle set can spread across many disconnected regions of the state space simultaneously, a particle filter can represent genuinely multimodal beliefs, starting spread nearly uniformly across an entire map with no prior position information and progressively concentrating onto the correct location, or locations, purely as sensor evidence rules out inconsistent hypotheses — a capability no single-Gaussian Kalman filter has.

As Durrant-Whyte and Bailey's SLAM tutorial describes, simultaneous localization and mapping is circular by construction: accurate localization normally presupposes a known map, and accurate mapping normally presupposes a known location, so a robot entering unmapped territory has neither on its own. Thrun, Burgard, and Fox's Probabilistic Robotics resolves this circularity the same way single-robot state estimation does, by treating it as a Bayes filter problem, except that the state being tracked now includes both the robot's own trajectory and the positions of everything it has observed in the environment, all updated jointly as new sensor data arrives. One of the two things this joint state must represent is the map itself, and how that map is represented has direct consequences for how tractable the resulting estimation problem is.

Occupancy grid mapping, as presented in Probabilistic Robotics, represents the map as a fixed grid of cells, each holding an independent estimate of the probability that it is occupied by an obstacle, updated with a binary Bayes filter every time a sensor reading passes through or terminates in that cell. Because directly multiplying many small probabilities together during repeated updates is numerically unstable, occupancy grid implementations typically track each cell's log-odds value, l = log(p / (1 − p)), which turns each multiplicative Bayesian update into a simple addition. Concretely, take a cell that starts with no prior information, at probability p = 0.5, so its log-odds is l₀ = log(0.5/0.5) = 0. Suppose the sensor model assigns an occupied reading a 0.7 likelihood of correctness, so each "occupied" observation contributes a log-odds increment of log(0.7/0.3) ≈ 0.847. After two independent, consistent "occupied" readings of the same cell, its log-odds becomes 0 + 0.847 + 0.847 ≈ 1.694, which converts back to a probability of 1 / (1 + e^−1.694) ≈ 1 / 1.184 ≈ 0.845. Two agreeing readings move the cell from complete uncertainty to roughly 84.5% confident it is occupied, through nothing more than repeated addition, while a later "free" reading would simply subtract from the same running total, illustrating why log-odds is the standard implementation choice for this kind of repeated Bayesian evidence accumulation.

Representing the entire joint posterior over a robot's trajectory and every landmark's position with one large Extended Kalman Filter becomes computationally expensive as the number of landmarks grows, because the filter's covariance matrix scales with the total number of state variables being tracked jointly. Montemerlo, Thrun, Koller, and Wegbreit's 2002 paper "FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem" avoids this cost with a key factorization: conditioned on knowing the robot's exact trajectory, the positions of different landmarks become statistically independent of one another, since any correlation between two landmark estimates in a standard SLAM filter arises entirely from shared uncertainty about the robot's own path. FastSLAM exploits this by using a particle filter, exactly the Monte Carlo localization technique from the previous lesson, to represent the distribution over possible robot trajectories, while each individual particle carries its own small set of independent, per-landmark Extended Kalman Filters conditioned on that particle's specific trajectory hypothesis. This Rao-Blackwellized factorization, particles for the trajectory, tiny per-landmark Kalman filters for the map, lets FastSLAM scale far more efficiently in the number of landmarks than a single monolithic joint filter, directly combining the two state-estimation tools introduced earlier in this module into one working SLAM solution.

Once a robot has an estimate of its own state and a map to act within, from the previous two lessons, it still must decide how to move through that map, the path planning problem. A common formulation discretizes the environment into a graph, such as a grid of cells connected to their neighbors, and searches that graph for a minimum-cost path from the robot's current cell to a goal cell. Hart, Nilsson, and Raphael's 1968 paper "A Formal Basis for the Heuristic Determination of Minimum Cost Paths" introduced A* search, which extends uninformed graph search (Dijkstra's algorithm is the special case of A* with the heuristic fixed at zero everywhere) with a heuristic function estimating the remaining cost from any given node to the goal, and repeatedly expands whichever node currently has the lowest estimated total cost f = g + h, the true cost already paid plus the heuristic's estimate of what remains. Hart, Nilsson, and Raphael's key result is that as long as the heuristic never overestimates the true remaining cost, a property called admissibility, A* is still guaranteed to find an optimal path, while typically expanding far fewer nodes than an uninformed search would.

Consider a small 3×3 grid where a robot can move only up, down, left, or right between adjacent cells at a cost of 1 per move, starting at the top-left cell (0,0) and trying to reach the bottom-right cell (2,2), with a single obstacle at the center cell (1,1). The Manhattan-distance heuristic estimates the remaining cost from any cell (r,c) as |2−r| + |2−c|, which evaluates to 4 at the start; since every move changes r or c by exactly 1, no unobstructed path can possibly cost less than 4, so this heuristic never overestimates the true remaining cost and is admissible here. Enumerating the six possible shortest paths that move only right or down shows that exactly two of them, right-right-down-down and down-down-right-right, never pass through the blocked center cell, so the obstacle eliminates four of the six optimal-length paths without raising the true optimal cost above 4. Because its heuristic is admissible, A* is guaranteed to find one of the two surviving cost-4 paths, and it does so while prioritizing exactly the candidate cells whose f-value points most directly toward the goal, rather than exhaustively exploring every direction the way an uninformed search would.

Grid-based search works well for a mobile robot navigating a 2D floor plan, but it breaks down for a robot arm, whose configuration space, the space of all possible joint-angle combinations, has one dimension per degree of freedom; even a modest six-jointed arm has a six-dimensional configuration space, and discretizing a space that large into a grid at any useful resolution requires a number of cells that grows exponentially with the number of dimensions. LaValle's 1998 technical report "Rapidly-Exploring Random Trees: A New Tool for Path Planning" sidesteps this by never discretizing the configuration space at all: an RRT incrementally builds a tree rooted at the start configuration by repeatedly sampling a random point in the configuration space, finding the tree's nearest existing node to that sample, and extending the tree a small step toward it, checking for collisions along the way, so the tree's growth is naturally biased toward unexplored regions of the space. LaValle designed RRTs to be probabilistically complete, meaning that if a valid path exists, the probability that the algorithm eventually finds one approaches 1 the longer it runs, but unlike A* with an admissible heuristic, a basic RRT carries no guarantee that the path it finds is the shortest one, trading formal optimality for the ability to scale to continuous, high-dimensional spaces that grid search cannot handle at all.