Add Monte Carlo Localization with kidnapped-robot recovery (navigation/38)

[rsasaki0109]

What

A new example, examples/navigation/38_monte_carlo_localization.py: a self-contained Augmented Monte Carlo Localization particle filter that recovers from the kidnapped-robot problem. The repo had no particle filter, and localization is one of the canonical "what happens after it fails" stories.

The lesson

Three acts on one run: global localization from a uniform prior → a scheduled kidnap (teleport) → recovery.

Plain MCL converges beautifully and then fails the kidnapped-robot problem: once the cloud collapses onto the true pose, a teleport leaves no particles anywhere to recover with. Augmented MCL (Thrun et al., Probabilistic Robotics, Table 8.3) is the fix in one trick — track a fast and a slow running average of the measurement likelihood, and when the fast one drops below the slow one (the signature of a failure) inject random particles in proportion to the drop. A few land near the new pose and the cloud re-collapses there.

The contrast is built into the same file via --no-augment, and it shows up directly in the failure log:

augmented:  kidnapped ×1                          (re-localizes in ~1 step)
plain MCL:  kidnapped ×1 + localization_lost ×30   (stays lost the whole run)

Contents

• A self-contained MCLWorld (point landmarks, range + bearing sensing, one scheduled kidnap) and an AugmentedMCL filter: systematic (low-variance) resampling, w_slow/w_fast injection, and a weighted-mean pose estimate so a handful of freshly injected particles on the sharp likelihood peak snap the estimate to the recovered pose.
• Two smoke tests: augmented global-localizes, detects the kidnap (inject_ratio > 0), and re-localizes; plain MCL never injects, stays unsuccessful, and emits localization_lost while augmented on the same seed recovers without ever reporting itself lost.
• examples/README.md row + a full examples/navigation/README.md section; example count 40→41 and test count 113→115 in README.md / docs/status.md.

Why it works (design notes)

Three subtleties that took iterating to get right, all documented in-code:

• The augmented signal must use the absolute average likelihood (not rescaled by its own max), or a kidnap is invisible to w_fast/w_slow.
• Range + bearing (not range-only) makes the likelihood sharply peaked, so a stranded cloud has no smooth cross-map gradient to creep along — isolating recovery to injection.
• The pose estimate is the weighted particle mean, so a few injected particles on the peak dominate instead of being dragged toward the map center.

Verification

• Seeds 0–9: augmented recovers 10/10 (final error < 0.21), plain MCL 0/10; ~0.06 s per run.
• Full suite green (128 passed); matplotlib render path confirmed under Agg.

References

• S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press 2005 — Augmented_MCL (Table 8.3).
• D. Fox, W. Burgard, F. Dellaert, S. Thrun, "Monte Carlo Localization for Mobile Robots," ICRA 1999. The ROS amcl package implements this adaptive recovery.

🤖 Generated with Claude Code