Chapter 11 · Estimate robot state, localize honestly, and measure uncertainty
Today in the field story
One problem, then the next
Courier-3 recognizes the loading-bay landmark near its starting point. Add a loop-closure constraint and inspect which poses move, then distinguish that graph correction from AMCL’s particles on an existing map. Track the resulting map-to-odom adjustment so local odometry remains continuous while global belief changes. A large correction requires evidence and downstream awareness; it is not automatically a triumphant snap to truth.
- Why now
The mission needs global recovery mechanisms after local drift becomes visible.
- Ignore today
Ignore multi-robot map merging and lifelong mapping; compare one loop closure and one localization recovery.
- Unlocks next
An explainable global correction that preserves local control continuity.
Understand
Build the physical picture first
A pose graph is a route held by measured elastic links, loop closure adds a long link back to a known place, and AMCL tests many robot cut-outs against a finished map.
A pose graph represents selected robot poses as nodes and relative observations as edges. Consecutive edges can come from odometry or scan matching; a loop edge connects poses believed to show the same place at different times. Each edge carries a measured relative transform and uncertainty. Graph optimization adjusts node poses to reduce weighted constraint disagreement, distributing correction instead of pretending the final node alone caused all drift.
Loop closure is a data-association decision before it is an optimization result. A true closure can straighten a bent route and align repeated walls. A false closure tells the optimizer that two different places are the same, potentially warping the whole map while achieving a low mathematical cost. Require appearance or geometric agreement, a plausible relative transform, consistency with other constraints, and before/after residual inspection rather than accepting every proposed revisit.
AMCL solves localization in an existing static map with a particle filter. Each particle is a pose hypothesis. The motion model moves particles according to odometry plus noise; the laser model weights particles by how well predicted ranges agree with the real scan; resampling concentrates computation on stronger hypotheses while retaining policy for recovery. A particle cloud can represent several locations, unlike one Gaussian mean and covariance.
Global correction belongs in the frame chain, not in a teleporting local controller signal. Smooth local odometry owns odom→base_link; SLAM during mapping or AMCL during saved-map localization normally supplies map→odom. When loop closure or localization changes the global belief, map→odom can change while the odometry history stays continuous. Only one active component should own that transform in each operating mode.
Mapping and saved-map localization are separate operating modes. Build and optimize with SLAM Toolbox, save the map and pose-graph provenance, stop the mapping authority, then launch the approved static map with AMCL. Initialize with an honest pose distribution, not a magic exact point. Test nominal start, broad initial uncertainty, a repeated corridor, and a controlled localization disturbance before navigation trusts the pose.
Words you need
Name each idea precisely
- Pose graph
A graph whose nodes are robot poses and whose edges are uncertain relative-pose constraints.
Physical example:Ten route poses are pins connected by bands representing odometry and scan matches.
- Constraint
A measured relationship between states, paired with uncertainty that determines its optimization weight.
Physical example:An edge says pose B is 1.0 m ahead of pose A with a stated covariance.
- Loop closure
A constraint asserting that a current observation revisits a place represented earlier in the graph.
Physical example:The final scan at the loading dock is matched to the starting-dock scan after circling the warehouse.
- Constraint residual
The disagreement between a measured graph edge and the relationship implied by current node poses.
Physical example:The optimized start-to-finish relationship still disagrees with the closure measurement by 0.03 m.
- Particle
One weighted sample representing a possible robot pose in a particle-filter belief.
Physical example:One AMCL particle proposes that the rover is beside bay A facing east.
- map→odom correction
The global transform that aligns smooth drifting odometry with the current map-based pose belief.
Physical example:AMCL shifts the odometry trajectory 0.25 m in map without rewriting past wheel increments.
Math, one line at a time
Work through today’s relationship
Prerequisite rescue · optionalProbability, variance, and Kalman weighting
State estimation combines predictions and measurements according to uncertainty.
- μ
- best current estimateUnit: state unit
- σ²
- variance, or squared uncertainty spreadUnit: state unit squared
- K
- Kalman gain, the measurement weightUnit: unitless
Prediction is 10 m. Measurement is 12 m. Let K = 0.25.
Innovation is 12 − 10 = 2 m.
Updated estimate = 10 + 0.25×2 = 10.5 m; the lower-trust measurement only shifts the estimate partway.
It is a weighted merge like resolving two data sources, but the weights come from modeled uncertainty.
Prediction 5 m, measurement 7 m, K = 0.5. What is the update?
5 + 0.5×(7−5) = 6 m.
The loop-closure position error is the Euclidean norm
Quantify a square-route closure before accepting its loop
A square route should return to (0.00, 0.00) m in map, but odometry ends at (0.25, -0.10) m. After adding a verified loop and optimizing, the end is (0.04, -0.02) m.
Calculate pre-closure displacement error: √(0.25² + (-0.10)²) = √0.0725 ≈ 0.269 m.
Keep signed components as evidence: +0.25 m in x and -0.10 m in y can reveal directional bias hidden by one magnitude.
Calculate post-optimization displacement error: √(0.04² + (-0.02)²) = √0.0020 ≈ 0.0447 m.
Calculate magnitude reduction: (0.269 - 0.0447) / 0.269 ≈ 0.834, or about 83.4%.
Inspect residuals on all odometry, scan, and loop edges; a smaller endpoint error alone could hide distortion elsewhere.
Verify the closure's place identity and compare the optimized trajectory with independent truth or surveyed landmarks before accepting the corrected map.
Endpoint closure magnitude falls from about 0.269 m to 0.0447 m, but acceptance still depends on correct data association and distributed residual and reference checks.
Optimization can reduce a number even for a bad constraint; validate the loop's identity and the whole map, not only the final gap.
Physical examples
Where this appears in real life
Pins and elastic route bands
Pins mark poses on a board and elastic bands mark noisy relative constraints. The route nearly closes, so a verified final-to-start band gently redistributes the gap.
A correct loop improves agreement across many edges; attaching the band to the wrong similar-looking pin distorts the entire route.
Transparent robot hypotheses on a map
Several clear robot cut-outs sit on a printed floor plan with range rays drawn toward walls. After a motion and scan, poorly matching cut-outs are removed or copied less often.
The collection, not one cut-out, is the belief; repeated rooms can retain separate groups until a distinctive observation arrives.
Hands-on exercise
Make the idea observable
Use the existing Gazebo route, lidar and odometry bags, SLAM Toolbox, a saved map, AMCL, RViz, and simulation truth.
Run mapping mode on a fixed loop, save raw bag, map, serialized pose graph, transforms, candidate loop evidence, and trajectory before and after optimization.
Calculate closure error, inspect graph residual changes, and compare the optimized trajectory with time-aligned Gazebo truth at several points rather than only the endpoint.
Record one ambiguous or rejected loop candidate and the geometric, transform, timing, and residual evidence that prevented automatic acceptance.
Stop the mapping authority, load the approved static map, launch AMCL as the sole
map→odompublisher, and initialize with a deliberately broad but plausible pose distribution.Run a distinctive-area and repeated-corridor trial, preserving particle clouds, scan agreement, estimated pose, covariance, transform freshness, and truth error.
Inject a controlled bad initial pose or temporary scan loss, apply the declared relocalization policy, and measure whether pose error returns within its bound without competing transforms or unsafe commands.
A verified loop distributes global correction while local odometry stays smooth; AMCL particles may remain split in repeated geometry and concentrate after a distinctive scan.
Mapping and localization launches have one global transform authority each, closure and AMCL claims are backed by independent error, and the ambiguous and recovery cases remain in the evidence set.
Build today
Use the frozen Week 8 artifacts to fuse odometry and IMU, explain scan matching, loop closure, and AMCL, then rerun the unchanged scenarios and compare error, uncertainty coverage, transform age, dropout recovery, and navigation success.
Evidence to save
DONE when a 60–120 second uncut “Pose graphs, loop closure, AMCL, and map→odom correction” demo links to its command, logs or plots, result count, and honest failure note.
Common mistakes
Catch the wrong mental model
Accepting a loop because optimization reduced total cost or endpoint gap.
Verify place identity, relative transform plausibility, graph-wide residuals, map geometry, and independent trajectory references.
Running SLAM mapping and AMCL as simultaneous map→odom publishers.
Define separate mapping and saved-map localization modes and assert one intended transform authority before movement.
Calling a tight AMCL particle cloud proof of accurate localization.
Check scan-to-map agreement, timestamps, transforms, initial-pose policy, covariance consistency, and independent pose error.
Job connection
How this becomes employable evidence
Commission a warehouse map and saved-map localization stack, reject a false loop candidate, enforce single map→odom ownership, and qualify AMCL initialization and relocalization across repeated aisles.
Relevant target roles
- Robotics Application / ROS 2 Integration Engineer
- Robotics Software Engineer — ROS 2 / AMR
- Robotics Deployment, Integration & Validation Engineer
Chapter 11 interview drill
Interview questions: Pose graphs, loop closure, AMCL, and map→odom correction
Practise a 60–90 second answer: define the idea, connect it to a physical robot, state assumptions, frames, and units when relevant, then finish with the failure signal or evidence you would inspect.
Primary interview scenario
Explain how pose-graph loop closure differs from AMCL localization, why a false loop is dangerous, how map→odom preserves smooth local motion, and how you would prove recovery.
Answer shape: clarify the situation → trace the physical and software path → test the most likely boundaries → name the evidence that would confirm the result.
Technical follow-up questions
Q1What do pose-graph nodes and edges represent?
Nodes represent selected poses; edges represent uncertain relative-pose constraints from motion, observations, or loop closures.
Q2Why can a false loop closure damage more than a missed loop?
A false global constraint can warp many poses and the map, while a missed loop usually leaves existing drift visible.
Q3Which transform should AMCL normally publish in saved-map mode?
map→odom, while the local odometry source continues to provide smooth odom→base_link.