Chapter 11 · Estimate robot state, localize honestly, and measure uncertainty
Today in the field story
One problem, then the next
After the IMU event, small pose errors accumulate until the lidar walls no longer align. Compare scan matching against the previous scan or map and record when repetitive aisle geometry creates a plausible wrong fit. Update occupancy only from framed, timed rays, and keep unknown cells unknown during dropout. The route’s growing drift is now evidence to explain, not an aesthetic defect to smooth away.
- Why now
Local fusion must connect to geometric localization and mapping without hiding accumulated error.
- Ignore today
Ignore building a new scan matcher; diagnose initialization, overlap, residuals, and map updates.
- Unlocks next
Measured drift and registration evidence ready for global correction.
Understand
Build the physical picture first
Scan matching lays two imperfect range drawings over each other, occupancy mapping records what rays crossed and struck, and every small alignment mistake can accumulate into localization drift.
Scan matching estimates relative motion by finding a transform that makes a current lidar scan agree with a previous scan, local submap, or map. An optimizer scores candidate transforms using point or likelihood agreement. The lowest residual found is only the best result inside that search and model. Repeated walls, low overlap, moving people, glass, bad timestamps, and a poor starting guess can produce a convincing but physically wrong alignment.
Occupancy mapping asks a different question: given a pose and range observation, which cells were probably free and which endpoint was probably occupied? A simple inverse sensor model lowers occupancy belief along the ray and raises it near a valid obstacle return. Unknown is not free, maximum-range readings need declared handling, and repeated evidence should update belief without becoming impossible certainty after one scan.
Drift is accumulating disagreement between estimated and true pose. Wheel-scale error, scan-match bias, wrong lidar extrinsics, unsynchronized motion, and repeated local minima can each add a small error every cycle. A map may still look tidy because nearby scans agree while the entire route bends or stretches. Measure drift against simulation truth or surveyed references after transforming pose and truth into the same frame and time.
Mapping and localization reuse some observations but estimate different unknowns. During mapping, scan alignment helps estimate trajectory while occupancy and pose-graph state are built. During localization in an approved static map, the map is held fixed and pose changes. Keep raw scans, odometry prior, chosen match, residuals, map settings, transforms, and truth separate enough to diagnose whether a bad wall came from sensing, pose, or occupancy update.
Words you need
Name each idea precisely
- Scan matching
Estimating relative pose by aligning current range observations with another scan, submap, or map.
Physical example:Two lidar outlines of the same corner align after a 0.12 m translation and 0.03 rad rotation.
- Correspondence
A proposed relationship between observed geometry in two data sets or between a scan and map.
Physical example:A current wall segment is paired with the earlier wall segment believed to represent the same surface.
- Residual
The disagreement remaining between transformed observations and the model being matched.
Physical example:After alignment, point-to-wall distances average 0.04 m under the selected score.
- Inverse sensor model
A rule for updating map-cell belief from a sensor reading and an assumed sensor pose.
Physical example:Cells crossed by a valid lidar ray become more likely free, while the hit cell becomes more likely occupied.
- Occupancy grid
A map of equal cells whose values encode occupied, free, or unknown belief.
Physical example:At 0.05 m resolution, a 1 m-wide obstacle spans about 20 cells.
- Localization drift
Accumulated difference between estimated pose and independently referenced pose over travel or time.
Physical example:Ten slightly short one-metre estimates end 0.10 m behind the surveyed finish mark.
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.
With a systematic error of per move over moves, the accumulated drift is approximately
Measure how a tiny systematic match error becomes drift
A rover truly advances 1.00 m on each of ten straight segments. Its scan-matching pipeline systematically estimates each segment as 0.99 m.
Calculate per-segment signed error: estimated minus true = 0.99 - 1.00 = -0.01 m.
Sum true travel: 10 × 1.00 = 10.00 m.
Sum estimated travel: 10 × 0.99 = 9.90 m.
Calculate final signed position error: 9.90 - 10.00 = -0.10 m.
Report drift magnitude as 0.10 m over 10.00 m, or 1%, while retaining the negative sign separately to show the estimate is short.
Reject the claim that every match was correct merely because -0.01 m looked small; the repeated systematic direction is the causal signature.
A one-centimetre error per one-metre match accumulates into a ten-centimetre final error after ten segments.
Small independent noise may partly cancel, but a repeated bias integrates; plot signed error over distance instead of inspecting only local residuals.
Physical examples
Where this appears in real life
Transparent hallway scan cards
Two transparent sheets carry range points drawn from slightly different positions in a corridor. Sliding and rotating them can align both parallel walls at several longitudinal offsets.
A visually low residual does not choose the correct offset in symmetric geometry; an odometry prior or distinctive end wall provides missing information.
Flashlight ray across a square grid
A paper ray starts at a known sensor cell, crosses empty squares, and ends on a cardboard box, while areas outside the ray remain unobserved.
Traversed cells support free belief, the endpoint supports occupied belief, and untouched cells remain unknown rather than being painted free.
Hands-on exercise
Make the idea observable
Replay a saved Gazebo lidar-and-odometry route with ground truth. Use SLAM Toolbox or the existing scan-matching path; do not drive physical hardware.
Freeze one pair of scans with source timestamps and transforms, overlay them at the odometry prior, and record overlap plus the initial residual or score.
Run the matcher, save the selected transform and residual, then compare estimated relative motion with time-aligned Gazebo relative motion.
Repeat on an open corner and a symmetric corridor, preserving alternative candidate transforms instead of saving only the winner.
Build the occupancy map from the same route and inspect valid hit endpoints, free ray paths, unknown cells, maximum-range handling, resolution, and lidar extrinsics.
Inject one documented lidar timestamp offset or extrinsic yaw error, rerun the unchanged route, and plot signed position and heading error versus travelled distance.
Restore the correct value and produce a table separating scan residual, relative-pose error, final drift, and visible map symptom for nominal and faulty runs.
The symmetric scene can accept a wrong low-residual transform, while a timing or extrinsic fault creates directional drift and duplicated or bent map structure.
The nominal and faulty maps are reproducible, match residual is never presented as pose truth, and the report traces one map defect to a timestamp, transform, or alignment cause.
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 comparison table for “Scan matching, occupancy mapping, and localization drift” contains the test condition, metric, result, and justified engineering decision.
Common mistakes
Catch the wrong mental model
Using low scan-matching residual as proof of correct global pose.
Compare with motion priors, distinctive geometry, alternative candidates, transform and timing checks, and independent pose reference.
Marking every cell beyond or beside a lidar ray as free.
Update only cells supported by the declared inverse sensor model; unobserved space remains unknown.
Tuning map resolution until the map looks sharp without measuring pose error.
Freeze the route and compare map settings using localization drift, residual distributions, compute cost, and reference geometry.
Job connection
How this becomes employable evidence
Diagnose an AMR map that bends after long aisles by correlating scan-match candidates, odometry priors, lidar timing and extrinsics, occupancy updates, and ground-truth drift.
Relevant target roles
- Robotics Software Engineer — ROS 2 / AMR
- Robotics Application / ROS 2 Integration Engineer
- Robotics Deployment, Integration & Validation Engineer
Chapter 11 interview drill
Interview questions: Scan matching, occupancy mapping, and localization drift
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
A scan matcher reports a low residual but the completed warehouse loop misses its start by 0.4 m. Explain symmetry, initialization, correspondences, timing, extrinsics, occupancy evidence, and decisive tests.
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
Q1Why can a symmetric corridor produce a wrong low-residual scan match?
Several longitudinal shifts can align similar parallel walls, so geometry alone does not uniquely determine pose.
Q2What does a valid lidar ray usually say about cells before its hit?
They receive evidence of free space; the valid endpoint receives occupied evidence and untouched cells remain unknown.
Q3What distinguishes residual from localization error?
Residual measures fit to the matcher model, while localization error compares estimated pose with an independent reference in common frame and time.