Localization, Mapping and Navigation

Kalman and particle filters, SLAM, occupancy grids, A* planning and the Nav2 stack, with a filter and a planner worked through.

On this page

The three problems: where am I, what is around me, how do I get there
The Kalman filter
Particle filters and AMCL
SLAM: mapping while localizing
Occupancy grids and costmaps
Global planning: Dijkstra and A*
Local planning and path following
The Nav2 stack
Planning for arms: configuration space and MoveIt
When it goes wrong: recovery and monitoring
FAQ

The three problems: where am I, what is around me, how do I get there

Problem	Question	Main tools
Localization	Where am I, in a known map?	Kalman filters, particle filters (AMCL), scan matching
Mapping	What does the world look like, given known poses?	Occupancy grids, point clouds, octrees
SLAM	Both at once, in an unknown place	SLAM Toolbox, Cartographer, RTAB-Map, visual SLAM
Planning	How do I get to the goal without hitting anything?	A, Dijkstra, Hybrid A, DWA, MPPI, TEB
Control	How do I follow the plan?	Pure pursuit, PID, MPC (see Motor Control, PID and Trajectories)

Every sensor is noisy and every motion is uncertain, so these problems are solved with probabilities: the robot keeps not a single pose but a belief (a mean and an uncertainty) and updates it as it moves and senses.

The Kalman filter

The Kalman filter keeps an estimate x and its variance P, and repeats two steps: predict (move the estimate by the motion model and grow the uncertainty by the process noise Q) and update (correct it with a measurement z of variance R, weighting the measurement by the Kalman gain K).

Predict: x ← x + motion; P ← P + Q Update: K = P ÷ (P + R); x ← x + K (z − x); P ← (1 − K) P

A Kalman filter estimating a constant from noisy measurements: the estimate converges and its uncertainty band narrows.

Worked example: one step and a fusion

One update. The estimate starts at x = 0 with a large variance P = 4. A measurement z = 5.4 arrives with variance R = 1: K = P ÷ (P + R) = 4 ÷ 5 = 0.80, x = 0 + 0.80 × (5.4 − 0) = 4.32, P = (1 − 0.80) × 4 = 0.80. The filter trusted the measurement because its own estimate was so uncertain.
Over time (the figure: true value 5, measurement noise σ = 1) the gain falls as confidence grows: after 10 measurements the estimate is 4.69 with σ = 0.31 and gain 0.098; after 40, 4.74 ± 0.16, and each new measurement moves it by only 2.6 % of its error.
Fusing two sensors. A wheel-based position of 10.2 m (σ = 0.4 m) and a lidar position of 9.8 m (σ = 0.2 m): weight each by 1 ÷ σ²: 6 and 25. The fused value is (6 × 10.2 + 25 × 9.8) ÷ 31 = 9.88 m with σ = 0.18 m: nearer the better sensor and better than either.

The extended Kalman filter (EKF) linearizes a nonlinear motion or measurement model at each step and is the workhorse of robot localization (robot_localization in ROS fuses odometry, IMU, GNSS and more). The unscented Kalman filter (UKF) handles stronger nonlinearities.
Tune Q and R honestly: R from the sensor’s measured noise, Q from how much the motion model can be wrong. A Q that is too small makes the filter stubborn and slow to follow; too large and it trusts noisy data.
Check consistency: the errors should look like the predicted σ (a few percent of measurements outside 2σ); reject outliers with a gate on the normalized innovation.
The filter assumes Gaussian noise; a ghost measurement from a reflection is not Gaussian: gate it out.
Time stamps matter: apply each measurement at the time it was taken, not when it arrived.

Particle filters and AMCL

A particle filter represents the belief by many sample poses (particles). Each step: move every particle by the motion model plus noise, weight each by how well the sensor reading fits the map from that pose, and resample (copy the likely particles, drop the unlikely). It can represent several hypotheses at once (the robot could be in either of two similar corridors), which a Kalman filter cannot. AMCL (adaptive Monte Carlo localization) is the standard way to localize a 2D lidar robot in a known map.

Worked example: effective sample size

Six particles with normalized weights 0.02, 0.05, 0.60, 0.20, 0.08, 0.05 have an effective sample size N_eff = 1 ÷ Σ w² = 2.4 out of 6: one particle dominates, so the filter resamples. Resample when N_eff falls below about half the number of particles; resampling too often loses diversity.

Cost: with 2 000 particles and 0.03 ms to evaluate one against a scan, an update takes 2 000 × 0.03 = 60 ms. AMCL adapts the number of particles between a minimum (while certain) and a maximum (while lost), saving computing when the pose is well known.

Give it a good initial pose (RViz “2D pose estimate”), or spread particles over the whole map for global localization.
The map must match the world; a changed layout (moved furniture, a long hall) reduces the match.
Outputs the transform map → odom, so the robot’s odom frame stays smooth while the map pose corrects slowly (see ROS 2 in Practice).

SLAM: mapping while localizing

SLAM (simultaneous localization and mapping) builds a map from sensor data while using it to find where the robot is. Modern systems build a pose graph: nodes are robot poses, edges are the relative motions measured by odometry and scan matching, and when the robot returns to a place it has seen (a loop closure) a new edge pulls the whole graph into consistency, removing the accumulated drift.

System	Sensor	Notes
SLAM Toolbox	2D lidar	The Nav2 default; online and offline mapping, map editing and lifelong mapping
Cartographer	2D and 3D lidar, IMU	Strong loop closure; Google; community maintained
RTAB-Map	RGB-D, stereo, lidar	Appearance-based loop closure; 3D maps
ORB-SLAM3, VINS-Fusion	Monocular, stereo, with IMU	Visual and visual-inertial SLAM
LIO-SAM, FAST-LIO	3D lidar and IMU	Accurate 3D lidar-inertial odometry for outdoors and large scenes

Drive slowly and smoothly while mapping, with loops (return to places) so that loop closures can correct drift; avoid long featureless corridors and glass walls.
Save the map and edit it (remove moving objects, close gaps); use that fixed map for later localization.
Quality check: straight walls stay straight, loops close without a double wall, and the map matches a measurement of the real building.

Occupancy grids and costmaps

An occupancy grid divides the plane into cells, each holding the probability that it is occupied. A lidar ray makes the cells it passes through more likely free and the cell where it ends more likely occupied. It is stored as log-odds so that each update is an addition: l = ln(p ÷ (1 − p)).

Worked example: updating a cell, and the memory of a map

Hits accumulate. A sensor that reports “occupied” with probability 0.70 adds ln(0.70 ÷ 0.30) = 0.85 to the log-odds of the cell. Starting from 0.5 (log-odds 0):

Hits	Log-odds	Probability occupied
1	0.85	0.70
2	1.69	0.84
3	2.54	0.93
4	3.39	0.97

A “free” observation with probability 0.40 subtracts 0.41: several free readings undo one hit, which is how moving objects fade out of the map. Clamp the log-odds so that a cell can change its mind.

Memory. A 50 × 50 m map at 0.05 m per cell has 1 000 000 cells: 1.0 MB at one byte per cell. At 0.025 m it has four times as many, 4.0 MB. Choose the resolution for the smallest gap the robot must pass, not for the prettiest picture.

A costmap turns the map and the live sensors into a cost for each cell: lethal (an obstacle), inscribed (the robot’s body would touch), and a smoothly decaying cost out to an inflation radius that keeps the robot away from walls.

Worked example: inflation

A robot with an inscribed radius of 0.20 m, an inflation radius of 0.55 m and a decay factor of 5.0: the cost at distance d from an obstacle is 253 at or inside 0.20 m, then 252 × e^{−5.0 (d − 0.20)} out to 0.55 m, and zero beyond:

Distance to the obstacle	Cost (0–253)
0.20 m	253
0.30 m	153
0.40 m	93
0.50 m	56
0.55 m	44
0.60 m	0

The gradient pushes paths towards the middle of a corridor. A larger decay factor lets the robot hug walls; a smaller one keeps it in the middle but may close a narrow door: a gap narrower than 2 × the inflation radius is blocked for the planner.

Global planning: Dijkstra and A*

A* on a small grid with walls: the heuristic steers the search towards the goal, so it examines fewer cells than Dijkstra.

A global planner finds a route over the whole map. Dijkstra’s algorithm expands the cheapest unexplored cell first and finds the shortest path; A* adds a heuristic h (an optimistic estimate of the remaining distance, such as the straight-line or Manhattan distance) and expands cells in order of g + h, so it heads for the goal.

Worked example: the grid in the figure

On the 96-cell map the shortest path from the green cell to the red is 18 steps (equal to the Manhattan distance 11 + 7, since a path of that length exists). A* examined 61 cells and Dijkstra 75. With a heuristic that is never too high (admissible) A* returns the same optimal path with less work; the gain is bigger in open space than in a maze.
Scale: a 50 × 50 m map at 0.05 m is 1 000 000 cells; at about 1 µs per cell a worst-case search takes 1.0 s, which is why planners use coarser grids, plan once and replan only when the map changes, and use a heuristic.
A weighted heuristic (h × 1.5) is faster but gives up optimality; Theta* and Hybrid A* (Nav2’s Smac planner) produce smoother, kinematically feasible paths for car-like robots.
Sampling planners (RRT, RRT*, PRM) search high-dimensional spaces such as arm configurations.

Local planning and path following

The global path is a rough plan; a local planner turns it into velocity commands every 50 ms while avoiding the obstacles the sensors see now. The classic dynamic window approach (DWA) samples velocity commands that the robot can reach within one control step, simulates each forward for about a second, and picks the best by a score (progress along the path, distance to obstacles, speed).

Worked example: the dynamic window

At v = 0.4 m/s with an acceleration limit of 0.8 m/s² and a control period of 0.1 s, the speeds reachable next step are 0.4 ± 0.8 × 0.1 = 0.32 to 0.48 m/s; with an angular acceleration of 1.5 rad/s² the turn rates are -0.15 to 0.15 rad/s.
Sampling 5 speeds × 7 turn rates gives 35 candidate trajectories; simulating each 1.5 s ahead at 0.1 ms per trajectory costs about 3.5 ms: cheap, so it runs at 20 Hz. The robot’s own acceleration limits keep the window small, which is why the planner needs a fast, accurate speed loop (see Motor Control, PID and Trajectories).
Other local controllers: TEB (optimizes a time-elastic band), MPPI (samples thousands of noisy trajectories and averages the best, in Nav2) and MPC.

Worked example: pure pursuit

Pure pursuit steers towards a point a fixed look-ahead distance L_d along the path. If that point is at an angle α to the robot’s heading, the robot drives a circle of curvature κ = 2 sin α ÷ L_d. With L_d = 0.6 m and α = 20°, κ = 2 × 0.342 ÷ 0.6 = 1.14 m⁻¹ (a radius of 0.88 m); at 0.4 m/s the command is ω = v κ = 0.46 rad/s. A short look-ahead follows the path closely but oscillates; a long one is smooth and cuts corners: scale L_d with speed.

The Nav2 stack

Component	Does	Typical rate
BT Navigator	Runs a behavior tree: compute a path, follow it, recover if stuck	Event-driven
Planner server	Global planner plugins (NavFn, Smac, Theta*)	On request (1 Hz or when blocked)
Controller server	Local controller plugins (DWB, MPPI, Regulated Pure Pursuit, TEB)	20 Hz
Costmap 2D	Global and local costmaps with static, obstacle and inflation layers	1–10 Hz
Behavior server	Recoveries: spin, back up, wait, clear costmap	On request
Smoother and velocity smoother	Shapes the path and limits accelerations	20 Hz
AMCL or SLAM Toolbox	Localization (map → odom)	10–30 Hz
Lifecycle manager	Starts and stops the servers in order	–

Everything is a plugin and a parameter: choose the planner and the controller in a YAML file (see ROS 2 in Practice).
Tune in this order: robot footprint and sensors correct, odometry and localization good, costmap and inflation sensible, then the controller’s speeds and the goal tolerances.
Test in simulation first with a realistic map (see Simulation and Digital Twins).

Planning for arms: configuration space and MoveIt

An arm’s planning space is its configuration space: one dimension per joint (six for a typical arm), in which a pose is a point and an obstacle in the workspace becomes a region that is hard to picture and easy to test with a collision checker. Grid searches are impractical in six dimensions (a hundred values per joint gives 10¹² cells), so sampling planners (RRT-Connect, PRM, in OMPL) grow a tree of random collision-free configurations towards the goal. MoveIt 2 wraps them with kinematics (Frames, Transforms and Arm Kinematics), collision checking against the URDF and the sensed scene, and time parameterization (Motor Control, PID and Trajectories).

Plans are not smooth or optimal: post-process (shortcutting, smoothing) and re-plan when the scene changes.
Cartesian paths (a straight line for the tool) need inverse kinematics at every point and fail near singularities and joint limits.
Planning time grows with clutter: keep the collision model simple, with margins.
Grasp planning chooses where and how to approach an object from its sensed shape; the rest is the same motion planning.

When it goes wrong: recovery and monitoring

Problem	Sign	Response
Localization lost	The covariance grows beyond 0.3 m; scans no longer match the map	Stop, spin to gather scans, re-localize globally, or ask for help
Kidnapped robot	The robot was carried to a new place	Global localization with particles spread over the map
Path blocked	No plan found, or the plan keeps changing	Wait, clear the costmap, back up, replan with a different planner, then report
Oscillation	The robot swings left and right at a gap	Increase the oscillation timeouts; reduce the inflation or the controller’s lookahead
Odometry fault	Wheel slip or encoder failure: jumps in the pose	Compare with the IMU and the lidar; use a gate on the filter
Dynamic obstacles	People and other robots move through the plan	A local costmap at 10 Hz, a prediction of their motion, and conservative speed near people

Monitor the filter’s uncertainty as a health signal and act on it: an uncertain robot should slow down.
Log everything (bags) so a navigation failure can be replayed and the cause found (see Building, Testing and Safety).
Keep a safe fallback: an independent obstacle-stop (a bumper, a safety lidar) that does not depend on the map or the planner.

Robotics Tools: the overview of this guide.
Perception: Cameras, Lidar and IMUs: the sensors these methods use.
Mobile Robot Kinematics and Odometry: odometry, the input to the filters.
Motor Control, PID and Trajectories: following the plan with PID and trajectories.
Numerical Methods and Machine Learning: filtering, optimization and learned navigation.

FAQ

Kalman filter or particle filter?

A Kalman (EKF) filter when the belief is a single hump and the models are nearly linear: sensor fusion of odometry, IMU and GNSS. A particle filter when several hypotheses are possible, as in localizing in a symmetric building (AMCL).

Why does my robot get lost in a long corridor?

A featureless corridor has no information along its length: the lidar cannot tell where the robot is along it. Add features, use wheel odometry and an IMU, or add landmarks (AprilTags).

How do I choose the grid resolution?

Smaller than a quarter of the narrowest gap the robot must pass, and no finer than the sensor accuracy: 0.05 m is typical for indoor robots. Finer grids cost memory and planning time.

A* or Dijkstra?

A* with an admissible heuristic: it finds the same shortest path with fewer expansions. Dijkstra is for when there is no useful heuristic or you need distances to every cell.

References

Thrun, S., Burgard, W. and Fox, D., Probabilistic Robotics; Kalman, R. E., “A new approach to linear filtering and prediction problems”, 1960.
Hart, P. E., Nilsson, N. J. and Raphael, B., “A formal basis for the heuristic determination of minimum cost paths” (A*), 1968; Fox, D., Burgard, W. and Thrun, S., “The dynamic window approach to collision avoidance”, 1997.
Macenski, S. et al., “The Marathon 2: A navigation system”, IROS, 2020 (Nav2); SLAM Toolbox, Cartographer, MoveIt 2 and OMPL documentation.

Last reviewed: October 2026. Filter and planner numbers are computed in the page builder for the small examples shown.