0.4 Part 0 · Introduction · deeper into the basics

State estimation and planning

In lesson 0.3 the controller knew the exact position of the arm, and the planner had a map of the walls. A real robot never has it that easy: sensors are noisy, and there is no map of an unfamiliar home. Before guiding a robot to a goal, you need to answer two questions: where is it now, and which way should it go. Classical robotics answers them with filters, maps and path search. We'll go through them in order, from a filter that merges the readings of two sensors to path search on a map of the home, and find where each method is reliable and where it stops working. If the topic is familiar, you can skip this lesson.

Ada is a home humanoid on a wheeled base; the course's labs are built around her. More about her in lesson 0.1.

≈ 50 minutes5 labs and 4 interactives17 missions with a goal

KALMANWhere Ada is in the corridor: merge the wheel and rangefinder readings so the error stays under 4 cm and the uncertainty band matches the real spread

PARTICLESWhich room Ada is in: find a spot where two identical bedrooms are easy to confuse, and find Ada after she has been carried to another room without warning

MAPWhere the map comes from: watch how an error in the wheel-based distance count closes the kitchen door on the map, then fix the map

PATHWhich way to go: make the grid path search expand more than 150 cells, and find a narrow gap where random-tree search is slow

Start the lesson ↓

1 Two questions

Where Ada is and where to go

To guide a robot to a goal, the controller needs two answers: where the robot is now and which way to go. In the classical stack from lesson 0.1, two modules between perception and control provide them.

State estimation answers the question "where am I and how am I moving". No sensor can say this exactly, so the robot keeps not a single number but a distribution: where it most likely is and how sure it is. This distribution is called the belief. It is updated in a two-step loop: predict from the motion model, correct from the measurement.

Planning answers the question "which way to go". From the map you build a graph or a set of sampled points and search it for a path that avoids obstacles and is as short as possible.

Blue marks classical modules. Every method in this lesson is classical; at the end of the lesson we'll see where neural networks replace or back them up today.

Let's start with state estimation in the simplest case: Ada drives along a straight corridor. How do you know where she is, when each sensor errs in its own way?

🔬 Lab 1

A noisy sensor and the Kalman filter

Ada drives along a corridor to the wall and back, and she has two imprecise sensors. The wheels count revolutions: over a short stretch this is accurate, but the error accumulates, and on carpet the wheels slip. The rangefinder measures the distance to the wall 10 times a second: its error doesn't accumulate, but every reading wanders by about 10 cm.

In lesson 0.2 we fused a similar pair, a gyroscope that accumulates drift and a noisy accelerometer, with a complementary filter. At every step it nudges the estimate toward the measurement by a fixed fraction $1 - \alpha$ . The weight $\alpha$ is tuned by hand, and it is the same in every situation: at the start, when the estimate is still rough, and after the sensor has gone silent.

The Kalman filter decides for itself how much to trust the measurement. It computes the weight from the sensor noise and from how confident it is in its current estimate. It shows its uncertainty as a $\pm 2\sigma$ band, where $\sigma$ (sigma) is the typical size of the error. If the filter is tuned correctly, the true position is almost always inside the band.

Three Adas drive side by side with the same sensors. The first trusts only the rangefinder, the second a complementary filter with the same $\alpha = 0.98$ , the third a Kalman filter.

Top view of the corridor. The three lanes are the same drive seen three ways. Ada drives to the wall on the right and back; the pale Ada is where she really is, the colored dot is where the estimate sees her. The dashed line from Ada to the wall is the rangefinder beam.

Horizontal axis: time, s. Vertical axis: position estimate error, cm.

model σ1 cm rangefinder σ10 cm compl. α0.98 carpet rangefinder silent 3 s

The Kalman filter keeps two numbers: the position estimate and its variance P, how unsure it is. Every step has two parts. Prediction: shift the estimate by the wheel readings; the uncertainty grows by $Q$ . Correction: compare the estimate with the measurement and move toward it by a fraction $K = P/(P + R)$ . A noisy sensor (large $R$ ) moves the estimate only slightly; an unsure filter (large $P$ ) moves it a lot. The engineer sets $Q$ and $R$ ; the filter computes $K$ itself.

This is how real robots work. On MIT's legged robots a Kalman filter estimates the position and velocity of the body from the IMU and the leg angles 500–1,000 times a second. The PX4 drone autopilot by default runs the prediction step of an extended Kalman filter 100 times a second and feeds in GPS, barometer, magnetometer and other sensors as their measurements arrive.

Under the hood: the equations, the complementary filter and nonlinear versions

Prediction:

x^- = x + u\,\Delta t, \qquad P^- = P + Q.

Correction:

\begin{aligned} K &= \frac{P^-}{P^- + R}, \\ x &= x^- + K\,(z - x^-), \\ P &= (1 - K)\,P^-. \end{aligned}

$u$ is the speed from the wheels, $z$ the rangefinder measurement, $\Delta t = 0.1$ s. The knobs set $\sigma$ , not variances: $Q$ is the model $\sigma$ squared, $R$ the rangefinder $\sigma$ squared. While there are no measurements, the filter only predicts, and $P$ grows by $Q$ every step.

In the lab the wheels overestimate speed by 4% and are noisy with $\sigma = 6$ cm/s; on the carpet (from 2.5 to 3.5 m) they overestimate by another 30%. Rangefinder noise is 10 cm. All three estimates start from the first rangefinder reading, the Kalman filter with $P = R$ . The randomness is seeded, so every run repeats exactly.

The complementary filter is a special case. Its formula $x = \alpha\,(x + u\,\Delta t) + (1 - \alpha)\,z$ coincides with a Kalman step if you take a constant $K = 1 - \alpha$ . The difference is that the Kalman filter computes $K$ itself from $Q$ , $R$ and the current uncertainty, so it converges quickly at the start and after a measurement dropout. In steady state $K$ stops changing, and the Kalman filter behaves like a complementary filter with the best $\alpha$ for these noise levels.

The multidimensional case. The state is a vector (say, position and velocity), $P$ and $Q$ are covariance matrices, the model is given by matrices $A$ and $H$ :

\begin{aligned} x^- &= A x + B u, \\ P^- &= A P A^{\mathsf T} + Q, \\ K &= P^- H^{\mathsf T} \bigl(H P^- H^{\mathsf T} + R\bigr)^{-1}. \end{aligned}

The filter is optimal for a linear model with Gaussian noise. For nonlinear models there is the extended Kalman filter (EKF): it linearizes the model with a Jacobian at the current estimate. The unscented Kalman filter (UKF) pushes several specially chosen points through the nonlinearity and does without Jacobians.

The Kalman filter errs by 3.2 cm on average, the complementary filter by 7.5 cm, the rangefinder by 10 cm. The Kalman filter picks the measurement weight itself and shows its uncertainty: while there are no measurements, the band grows. But the engineer sets the noise levels Q and R, and Q has to be chosen with a margin.

As long as Ada roughly knows where she is, one estimate with an uncertainty band is enough. But what if she has just powered on and doesn't even know which of two identical bedrooms she is standing in?

🔬 Lab 2

Where is Ada: the particle filter

Ada has powered on in one of two identical bedrooms and doesn't know which. There are two places she could be, right from the start.

Why won't the Kalman filter cope? It keeps a single hypothesis, a bell curve around one place. It would have to pick one bedroom and be confidently wrong.

A particle filter keeps hundreds of hypotheses at once. Each particle says "Ada is here, facing that way". One filter step: shift all particles by the odometry, compare each particle's expected lidar scan with the real one, multiply the ones that match and drop the ones that don't.

Ada has a map of the home, and a lidar with 24 beams reaching 2.5 m. The two bedrooms on the left are deliberately identical. Let's see how the filter finds Ada, where it gets confused, and what happens if Ada is quietly moved.

inject random particles when the scan stops matching the map Particles600

Under the hood: Monte Carlo localization

A particle filter used for localization is called Monte Carlo localization (MCL). One step:

for each particle

i

move by odometry with noise:

x_i \leftarrow \text{move}(x_i, u) + \text{noise}

weight by the scan:

w_i \leftarrow p(z \mid x_i, \text{map})

normalize the weights; if fewer than half the particles have noticeable weight:

resample: new particles are drawn with probability

w_i

Scan likelihood. For a particle we compute which distances the 24 beams would see if Ada stood there. The mismatch on each beam is scored with a Gaussian with $\sigma = 25$ cm plus a 20% uniform component for unexpected objects; beams are treated as independent. The product over the beams is too sharp, so we multiply its logarithm by 0.35, a common trick to keep the filter from becoming confident too early. Particles inside walls get zero weight.

Motion noise is 10% of the distance plus 0.6 cm, and 10% of the turn plus 2° per step (0.25 s). Resampling is low-variance: one random point and an equal stride along the cumulative weights.

Injecting random particles (Augmented MCL from the book "Probabilistic Robotics"). The filter tracks the average scan likelihood with two moving averages: a fast one (coefficient 0.5) and a slow one (0.05). If the fast one drops below the slow one, the scan has suddenly stopped matching expectations, and a fraction $\max(0,\ 1 - \text{fast}/\text{slow})$ of random particles, but no more than half, is scattered over the free space of the home. While everything matches, there are no random particles.

The number of particles can also change on the fly: KLD-sampling uses many particles while the cloud is wide and few once it has collapsed. For global localization it needed about 3,000 particles where a filter with a fixed number needed 50,000. The first paper on MCL (Dellaert et al., 1999) used 20,000 particles for global localization and 5,000 for tracking, and in 75 minutes the robot drove more than 2.2 km through the Smithsonian museum without ever losing itself. In the ROS 2 navigation stack (Nav2), localization is done by default by the same kind of adaptive particle filter, AMCL.

The particle filter holds both bedrooms at once and picks the right one when the lidar peeks through the door. While Ada stands still, identical places cannot be told apart: only motion helps. After a kidnapping, random particles save the filter: without them the error is 2.7 m after 10 s, with them Ada is found in 1.25 s.

All this time the filter had a ready map of the home: it compared every scan against it. Where does the map come from if Ada is in the home for the first time?

🔬 Lab 3

A map from lidar and SLAM

Ada is in the home for the first time and has no map. Without a map, the particle filter has nothing to compare the scan with.

So the map has to be built with the lidar. Ada drives around the home and records each beam: empty along the beam, an obstacle at its end. The result is an occupancy grid: 5 cm cells, each storing how much it looks like a wall.

Why is this hard? To put a beam on the map, you need to know where Ada was looking from. And to know where Ada is, you need a map. Doing both at once is SLAM, simultaneous localization and mapping.

First we'll give the map builder Ada's true pose, then the pose from the wheels, and then we'll teach Ada to correct her pose using the map itself.

Heading drift1°/m match scans to the map

Under the hood: log-odds, scan matching and loop closure

Occupancy grid. A cell stores the log-odds $l = \ln\dfrac{p}{1 - p}$ : 0 is "don't know", positive is "probably a wall", negative is "probably empty". Each beam adds −0.4 to every cell it passed through and +0.85 to the cell where it hit an obstacle. Values are clamped to ±4. Adding log-odds is the same Bayesian update as in the filters, just done for each cell separately, assuming the cells are independent.

Lidar and odometry. 90 beams reaching 3.5 m with 1.5 cm noise, 4 scans a second; Ada drives at 0.5 m/s along a 27 m route through every room. Odometry overestimates the distance by 1%, is noisy, and drifts the heading by the set number of degrees per meter traveled.

Matching the scan to the map. We predict the pose from odometry, then try shifts of up to ±12 cm and rotations of up to ±6° and pick the pose at which the beam endpoints land most strongly on already known walls. First coarsely, in steps of 4 cm and 2°, then finer. The map used for comparison is slightly blurred so that the score changes smoothly. This is a simple SLAM: it keeps the error from accumulating while Ada sees familiar walls.

What's missing here. In a large building a small error still accumulates. When the robot returns to a familiar place, real SLAM notices it (loop closure) and corrects the whole trajectory at once: poses are the nodes of a graph, matches are spring-like edges, and optimization stretches the graph. That is how Cartographer works for lidars and ORB-SLAM for cameras, for example.

With the exact pose the map is clean. Odometry that drifts the heading by just 1° per meter doubles the walls and closes the kitchen door on the map. If every scan is aligned to the map built so far, the pose error doesn't accumulate, and the map is drivable again.

Now Ada has a map and knows where she is. Which way should she go from the bedroom to the kitchen?

🔬 Lab 4

Dijkstra and A* on the same map

We need a path from the bedroom to the kitchen: the shortest one that doesn't touch walls or furniture.

Paths are searched on a graph. A grid of 20 cm cells is a graph: neighboring free cells are joined by edges of length 1 straight and $\sqrt{2}$ diagonally. To expand a cell is to check its neighbors and record the shortest path to each of them. Dijkstra's algorithm expands cells in order of distance traveled $g$ , as a wave in all directions. A* expands them by $g + h$ , where $h$ is the distance to the goal ignoring walls. Such a hint about how far is left to go is called a heuristic.

Dijkstra on the left, A* on the right, the same task: from the upper bedroom to the kitchen.

Dijkstraexpands by distance traveled g

A*expands by

g + h

Heuristic weight ε1.0

Under the hood: why A* finds the shortest path

open list

\leftarrow \{\text{start}\}

g(\text{start}) = 0

while the list is not empty:

take the node

v

with the smallest

f = g(v) + \varepsilon\, h(v)

v

is the goal: path found

for each neighbor

u

: if

g(v) + \text{length}(v, u) < g(u)

, update

g(u)

and record the parent

With $\varepsilon = 0$ this is Dijkstra's algorithm. The heuristic $h$ is admissible if it never overestimates the remaining path. Then A* finds the shortest path. If $h$ is also consistent ( $h(v) \le \text{length}(v, u) + h(u)$ ), each node is expanded at most once. In the lab $h$ is the grid distance ignoring walls, with diagonals of $\sqrt{2}$ : it is admissible and consistent. Cutting diagonally past the corner of a wall is not allowed. On equal $f$ , the node with the larger $g$ is expanded first, so A* switches less often between equally good cells.

With $\varepsilon > 1$ the heuristic already overestimates the path, and the guarantee weakens: the path is at most $\varepsilon$ times longer than the shortest, but usually fewer nodes are expanded. This is weighted A*.

With a binary heap, Dijkstra runs in $O(E \log V)$ , where $V$ is the number of nodes and $E$ the number of edges. In the worst case A* is no better: if the heuristic gives no useful hint (traps, dead ends), it expands almost everything.

On an open map A* expands 24 cells versus 314 for Dijkstra, and finds the same path. In a trap the heuristic leads it into a dead end, and the gain shrinks to a factor of two: 160 cells versus 332. Weighted A* expands fewer, but its path can be longer than the shortest.

The grid suits Ada on wheels well: she has only two or three degrees of freedom. Does a grid work for an arm with six or seven joints?

2 Dimensionality

Why a grid won't do for an arm

For an arm the grid is built not over the floor but over the joint angles. To the planner, the robot is a point in configuration space: for Ada on wheels that is x, y and heading; for the arm from lesson 0.3, two angles. On the main page, in the "Planning" tab, there is an interactive where A* searches for a path of such an arm on a 96 × 96 grid over the plane of the two angles.

A real arm has 6–7 joints, and each one adds another axis to the grid. Work out how many cells the grid will have then.

Degrees of freedom2 Cells per axis50

Logarithmic scale: each tick is 1,000 times the previous one.

Every new joint multiplies the number of cells by the number of cells per axis: with 50 cells per axis, by 50. The Lynch and Park textbook "Modern Robotics" says outright that grid methods are suitable only for low-dimensional spaces: with 100 cells per axis, an arm with seven joints has 10¹⁴ cells.

The way out is not to cover the whole space with cells but to throw random points into it and connect those between which you can travel. That is how sampling-based planners work. PRM (probabilistic roadmap, 1996) builds such a graph in advance for many queries. RRT (1998) grows a tree from the start for a single query.

Random points don't cover the space completely. Will a random tree find a path, and how short will it be?

🔬 Lab 5

RRT and RRT*: random trees

On a grid, A* with an admissible heuristic is guaranteed to find the shortest path. A random tree has no such guarantees: you can't tell in advance whether it will find a path, after how many iterations, or how short the path will be.

RRT grows a tree from the start: it takes a random point, finds the nearest node of the tree and takes a 30 cm step toward the point if there is no obstacle in the way. Every twentieth point is the goal itself. RRT* does the same, but for each new node it picks the best parent among the neighbors and rewires the neighbors if the new node gives them a shorter path.

To make the trees visible, we grow them on a plan of the living room rather than in the space of arm angles: the algorithm is the same there. Both trees get the same random points. The living room is divided by a partition made of two sliding panels.

RRTa step toward a random point

RRT*plus parent choice and rewiring

Horizontal axis: iterations. Vertical axis: length of the best path found, m.

Under the hood: RRT, RRT* and what "probabilistically complete" means

RRT:

x_{\text{rand}} \leftarrow

a random point (with probability 5%, the goal)

x_{\text{near}} \leftarrow

the nearest node of the tree

x_{\text{new}} \leftarrow

a 30 cm step from

x_{\text{near}}

toward

x_{\text{rand}}

if the segment

x_{\text{near}} \to x_{\text{new}}

is free: add

x_{\text{new}}

with parent

x_{\text{near}}

RRT*: the same, but

the parent of

x_{\text{new}}

is the neighbor with the shortest path to

x_{\text{new}}

;

neighbors whose path is shorter through

x_{\text{new}}

get

x_{\text{new}}

as their parent

Neighbors are the nodes within a radius $r = \min\bigl(75\ \text{cm},\ \gamma\sqrt{\ln n / n}\bigr)$ , where $n$ is the number of nodes. This is how the radius shrinks in the paper by Karaman and Frazzoli (2011): in the plane as $\sqrt{\ln n / n}$ , in a space of dimension $d$ as $(\ln n / n)^{1/d}$ . With this radius RRT* is asymptotically optimal, and a new node still has only a few neighbors. In the lab $\gamma = 650$ cm. Ada is a point, and the obstacles are inflated by her radius of 15 cm.

Properties. RRT is probabilistically complete: if a path exists, the probability of finding it tends to one as the number of iterations grows. How many iterations it will take is not guaranteed: in a narrow passage, a great many. RRT almost surely converges to a suboptimal path. RRT* is asymptotically optimal: the length of its path tends to the shortest. Their nodes are the same, and the neighbor search and rewiring, as Karaman and Frazzoli showed, make RRT* more expensive than RRT only by a constant factor.

Trees grown from both ends, RRT-Connect, are the default planner in MoveIt, the ROS motion planning library for manipulators: it takes the planner from the OMPL library. For mobile robots a grid is usually enough: in Nav2 the default path search is a wavefront Dijkstra algorithm.

Narrow passages are a well-known weakness of sampling: a point lands in a passage with a probability equal to its share of the area. What helps is sampling right next to obstacles (Gaussian sampling: of two nearby points, keep the free one if the other hit an obstacle), the "bridge" (bridge test: both points are inside obstacles while the midpoint between them is free, and that is exactly a passage), and trees grown from both ends, RRT-Connect. For comparison, the shortest path in the lab is computed exactly, on the visibility graph between the corners of the obstacles.

Both trees find their first path on the same iteration. If you keep growing them, RRT* straightens its path almost to the shortest, while the RRT path stays 18% longer. In a narrow passage you have to wait a long time for the first path: a random point rarely lands in the passage.

Every method in this lesson relies on models written by a human: sensor noise, the map, the shape of obstacles. Where do such models stop working, and what do people do then?

3 Classics next to neural networks

How these methods live on today

Hand-written models work in a known, unchanging world. There, filters and planners give guarantees, an estimate of their uncertainty and explainable errors. In the open world you can't write such models: no formula tells you which floor is wet.

The classics are rarely thrown out entirely. Learned parts are built into them or placed alongside. Pick a system and see what in it is learned and what has stayed classical.

🧩 Task

Which method for which task

Sort the tasks by method: drag a card, or tap it and then a bin. The last bin is for tasks where you can't write a classical module and need a learned model.

4 Check yourself

Eight common misconceptions

5 Takeaways

Lesson takeaways

The robot keeps not a point but a distribution, and updates it in a loop: "predict from the model → correct from the measurement".
The Kalman filter picks the measurement weight itself from the noise levels Q and R. Q hides everything the model doesn't know, so it is chosen with a margin, and you check whether the uncertainty band is calibrated.
The particle filter holds many hypotheses and survives ambiguity. After a kidnapping, only random particles save it.
The map and the pose depend on each other. Without pose correction the walls double, hence SLAM.
A* with an admissible heuristic finds the shortest path and usually expands fewer cells than Dijkstra, but in traps almost as many.
In high-dimensional spaces a grid doesn't fit in memory, and random sampling does the job: RRT finds a path, RRT* straightens it over time. Narrow passages are hard for them.
The classics rely on models written by a human. Where the world can't be described that way, they are complemented by learned models.

Open questions

Why do drones and cars still estimate their position with a Kalman filter, even though neural networks see better?

The filter needs little computation, runs at the sensor rate and outputs an estimate together with its uncertainty, which the other modules understand. Its errors are explainable, and its behavior when a sensor fails is predictable. Neural networks are more often placed at the filter's input: they provide measurements or their uncertainty, for example by estimating displacement from the IMU, and the filter fuses it all together.

In an apartment the chairs get moved every day. What will break first in the classical stack, and how do you back it up?

The map: it will show chairs that are no longer there and miss the new ones. Localization is hardly affected as long as the walls are visible: the particle filter survives extra obstacles thanks to the uniform component in the scan model. The planner will drive around ghosts. What helps is forgetting old observations on the map, a separate layer for movable objects, and frequent replanning from a fresh scan.

Why is A* on a grid enough for a robot vacuum, while an arm with seven joints needs RRT?

A vacuum's configuration space is two- or three-dimensional: a grid of thousands of cells is searched instantly. An arm has seven degrees of freedom: even with 50 cells per axis the grid has almost $10^{12}$ cells. Random trees don't cover the whole space but explore it by sampling, so their cost hardly depends on the dimension directly.

Paper guide

S. Thrun, W. Burgard, D. Fox, "Probabilistic Robotics", MIT Press, 2005.

Read closelyChapters 2–4: the Bayes filter, Kalman filters (with EKF and UKF) and particle filters. Chapter 8: Monte Carlo localization; Augmented MCL is Table 8.3. Chapter 9: occupancy grids.
Can skipChapters 11–16: GraphSLAM, sparse information filters, POMDPs. Worth coming back to when you need SLAM in large buildings or planning under uncertainty.
Question along the wayWhy is the scan model in MCL deliberately made cruder than the real lidar?

S. LaValle, "Rapidly-Exploring Random Trees: A New Tool for Path Planning", Technical Report 98-11, Iowa State University, 1998.

Read closelyThe algorithm itself and the figure of the tree growing into unexplored regions.
Question along the wayWhy are nodes at the edge of the tree extended more often than those in the middle?

Resources

How a Kalman filter works, in picturesT. Babb — the Kalman filter in pictures, without heavy math. Russian translation on Habr
Kalman and Bayesian Filters in PythonR. Labbe — a free book in Jupyter notebooks, from the g-h filter to the UKF and the particle filter
Introduction to the A* AlgorithmRed Blob Games — an interactive explanation of grid search: breadth-first search, Dijkstra, greedy search, A*
Planning AlgorithmsS. LaValle, 2006 — a free book on planning, including sampling-based methods
SLAM CourseC. Stachniss — lectures on filters, occupancy grids and SLAM on YouTube
Mobile RobotsK. Krinkin, CS Club at PDMI, 2019 — a SLAM course in Russian
PythonRoboticsShort Python implementations of filters, SLAM and planners

✅

Check yourself on all of Part 0. 13 questions from lessons 0.1–0.4, shuffled, plus an exercise on a live scene: that shows what has stuck. At the end, a score per lesson and the sections worth revisiting.

Part 0 check →

➡️

What's next: learning from examples. The classics are strong in a known world, where noise, the map and obstacles can be described. In the open world the rules can't be written down: no formula says what a "wet floor" or a "folded T-shirt" is. If you can't write the rule, you can show the behavior you want: the operator guides the robot, and the network learns to repeat it. This is where the main path of the course begins: lesson 1.1, "Behavior Cloning".

Was the lesson clear?

Found a mistake or inaccuracy? Report it on GitHub

Found this lesson useful? Say thanks — that is how the author knows the course is being read.