M7-T7 — Localization Loss and the Kidnapped Robot

Learning objectives

After this lesson, you should be able to:

• List signals that a pose estimate is failing: covariance growth, a map-to-odom jump, a falling match score and inconsistent innovations.
• Compute a jump test and a normalised-innovation test for a pose update.
• Describe the response ladder (slow, stop, relocalize, verify, ask) and why the robot must stop first.
• Explain global localization and augmented Monte Carlo localization, and why the kidnapped robot is hard.

The intuition

You wake up in a hotel and for a moment do not know which way the bathroom is. You do not walk quickly towards where it was in your last hotel. You stop, look for a landmark, the window or the door, and only when what you see matches what you expect do you move. Everything between “something is wrong” and “now I know” is the part a robot needs to write down.

The difficulty is that the localizer does not shout. A filter that has locked onto the wrong place reports a small covariance and a plausible pose, because it is internally consistent. Its failure shows in other ways: the pose jumps by more than the robot could have driven; the scan stops lining up with the map; the corrections the filter has to make each step are bigger than its own noise model says they should be.

The kidnapped robot is the extreme version of this. A filter that has concentrated its belief on one place has no mass left anywhere else, so it cannot recover by itself. Algorithms for that case deliberately keep a little belief everywhere, and the safe system around them decides when to use it. The figure shows the states of that system.

The mathematics

Signals

The jump test

If the transform from map to odom changes by Δ between two updates a time δt apart, a robot with top speed v_max cannot have moved more than v_max δt. A change much larger than that is not motion:

The innovation test

A filter predicts a measurement and compares it with the real one. The innovation is ν = z − ẑ, with covariance S (from the filter). If the filter is right, the normalised innovation squared is chi-square distributed with as many degrees of freedom as the measurement:

A run of NIS values above the gate means the filter’s model of itself is wrong, which is exactly what happens when the pose is wrong.

Recovering

• Local search around the last good pose: a wider scan match or a rotation in place if there is room. Works for drift, not for a kidnap.
• Global localization: spread hypotheses over the whole map and let measurements select. In AMCL this is the global-localization service, which re-initialises the particles uniformly over free space.
• Augmented MCL: keep short-term and long-term averages of the measurement likelihood, w_fast and w_slow, and inject random particles with probability max(0, 1 − w_fast / w_slow). When the likelihood drops, the filter adds hypotheses by itself.
• Landmarks: a docking station, a coded marker or a known door gives a pose with a small error and a known frame.
• A person: the operator places the robot on the map.

Verify before you move

A relocalization attempt ends with a test: the covariance below a limit, the match score above a limit, and these holding for a minimum time or distance. Until then the robot is not allowed to resume its mission.

A worked example

The jump. Scout drives at 0.6 m/s towards a goal. The localizer updates every 0.1 s. At one update the map-to-odom transform changes by 1.48 m (the case of lab M1-L1, incident 3). In 0.1 s Scout can have moved 0.6 · 0.1 = 0.06 m.

The correction is 25 times what motion could explain. With a threshold of, say, 5, this flags immediately. The covariance, meanwhile, grew from 0.02 to 0.85 m²: a second signal. A rule that accepted the new pose silently produced the failure of that lab, a robot that stopped well past its goal and reported success.

The innovation. A position measurement has innovation ν = (0.5, 0.3) m and the filter predicts S = diag(0.04, 0.04) m².

The 95% gate for two degrees of freedom is 5.99 and the 99% gate is 9.21. This one fails the first and passes the second. A single such value is not alarming (5% of good ones fail the 95% gate); five in a row are.

Time is distance. At 0.6 m/s a detector that needs three consecutive bad updates at 5 Hz takes 0.6 s, during which Scout travels 0.36 m on a pose that may be wrong. Stopping then needs 0.6 · 0.3 + 0.6²/(2 · 0.8) = 0.405 m (lesson M7-T1). The robot has therefore moved about 0.77 m before it is still. The choices are a faster detector, a lower speed in places where a mistake is costly, or a larger margin.

Augmented MCL. The long-term likelihood average is w_slow = 0.8 and, after the robot is moved, the short-term average falls to w_fast = 0.5. The probability of drawing a random particle at the next resampling is 1 − 0.5/0.8 = 0.375. If the likelihood keeps falling that probability rises, and the random particles are what let the filter find the true pose.

Engineering implications and limitations

Stop first, then think

While the pose is in doubt, the margins, the speed limits and the recoveries are all drawn around a position that may be wrong. Spinning or backing up on a wrong pose is more dangerous than standing still. Stop (a controlled stop within the deceleration limit), publish a clear status, and only then relocalize.

Several independent checks

No single signal is enough. Use at least one that does not depend on the filter, such as a landmark or a scan-to-map score computed separately. Require that a bad pose is rejected by any one of them, and a good pose accepted only by all.

Plan where you can relocalize

If the robot must restart from a dock or a marked place, say so in the map, and design the recovery to drive there (slowly, when the pose is still plausible) or to ask for a person to carry it there. In a bare corridor a global search can return several equally good answers. Do not accept an ambiguous one.

Do not hide jumps

Some stacks smooth a map-to-odom correction. That is good for a small drift and bad for a large one, because the robot drives in the wrong direction while the estimate catches up. Treat a large correction as an event, not as noise.

Scope

The logic here belongs to navigation. A robot that works around people and in places with hazards also needs protective functions that do not depend on the pose at all (lesson M7-T2 and the standards named there). This track does not replace a safety case.

Common misconceptions

Engineering takeaways

1. Signs of a failing pose are covariance growth, a map-to-odom jump, a low match score and inconsistent innovations; none is enough alone.
2. A jump is suspicious when it is far larger than v_max · δt; an innovation is suspicious when NIS exceeds its chi-square gate for several steps.
3. Respond in steps: slow, stop, relocalize, verify, and ask a person if the pose cannot be verified.
4. Global localization and random-particle injection let a filter recover from a kidnap; landmarks and people give independent fixes.
5. Detection takes time, and time is distance travelled on a possibly wrong pose.

References and further reading

Probabilistic Robotics covers the kidnapped robot and augmented MCL; the other sources are the original Monte Carlo localization papers.

• S. Thrun, W. Burgard and D. Fox, “Probabilistic Robotics”, MIT Press, 2005 (global localization, the kidnapped robot problem and augmented MCL, chapter 8).
• D. Fox, W. Burgard, F. Dellaert and S. Thrun, “Monte Carlo Localization: Efficient Position Estimation for Mobile Robots”, AAAI, 1999.
• S. Thrun, D. Fox, W. Burgard and F. Dellaert, “Robust Monte Carlo Localization for Mobile Robots”, Artificial Intelligence, 128, 2001.
• Nav2 documentation, “AMCL” (including the global localization service and the recovery_alpha parameters), docs.nav2.org.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: beyond a single trip, ordering stops, covering a floor and sharing space with other robots.

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T6: Recovery Behaviors and Stuck Detection

  Detect a stuck robot from progress over time, and order recoveries (wait, clear, back up, spin) by cost and risk.

• Lab
  M7-L4: Detect and Escape a Stuck Robot

  Scout rocks back and forth for ten minutes without noticing: detect no-progress and break out.