Skip to content

Add probabilistic motion models (velocity and odometry) from Probabilistic Robotics, chapter 5 - #1524

Open
Lorite wants to merge 1 commit into
AtsushiSakai:masterfrom
Lorite:add-probabilistic-motion-models
Open

Lorite wants to merge 1 commit into
AtsushiSakai:masterfrom
Lorite:add-probabilistic-motion-models

Conversation

@Lorite

@Lorite Lorite commented Nov 20, 2026

Copy link
Copy Markdown

Reference issue

Closes #1516

What does this implement/fix?

A new localization example with the two probabilistic motion models of Thrun, Burgard and Fox, Probabilistic Robotics (2005), chapter 5:

  • Velocity motion model: closed form (Table 5.1) and sampler (Table 5.3), control (v, w), noise parameters alpha1 to alpha6.
  • Odometry motion model: closed form (Table 5.5) and sampler (Table 5.6), control = two odometry poses, noise parameters alpha1 to alpha4.

Files:

  • Localization/probabilistic_motion_model/probabilistic_motion_model.py: the four model functions with docstrings and units, plain functions only (NumPy and Matplotlib). The closed forms accept one pose or a whole grid of poses. main() shows straight motion, a left turn and a right turn, with the sample cloud and the contour of the closed-form density for both models. Plotting is guarded by show_animation.
  • tests/test_probabilistic_motion_model.py: 9 tests (main without animation, straight motion, forward right turn, backward motion, angle wrapping across ±pi, pure rotation in odometry, agreement between samples and closed form for both models, and the odometry density over the pose).
  • docs/modules/2_localization/probabilistic_motion_model/probabilistic_motion_model_main.rst, added to the localization toctree.

As described in the issue, the docs page explains two details of the book:

  1. Line 7 of Table 5.1 uses the unsigned radius, so the recovered speed has the wrong sign when v and w have opposite signs. The code uses the signed radius of eq. 5.17. For a noise-free forward right turn, the true end pose now gets the same (highest) density as the mirrored left turn (test_forward_right_turn).
  2. The closed forms return a density over the noise variables, not over the pose. For the odometry model, dividing by the Jacobian delta_trans gives a density over the pose that sums to about one (test_odometry_density_over_pose_integrates_to_one).

While writing the tests I found a third, smaller point, also on the docs page: Table 5.6 takes the noise variance from the measured odometry, Table 5.5 from the candidate pose. So about 93 % of the odometry samples fall in the 90 % region of the closed form. The velocity model gives 90 %.

Additional information

  • The GIF is in AtsushiSakai/PythonRoboticsGifs#21. The docs image link works after that PR is merged.
  • I wrote this example with the help of Claude Code (AI assistant). I reviewed the code, checked the numbers against my own notebook from a reading group on the book, and ran the tests and the docs build locally.

Local results (Python 3.13, requirements/requirements.txt):

  • pytest tests -l -Werror: 165 passed (including the 9 new tests)
  • ruff check and mypy on the new files: no issues.
  • Docs build (sphinx -b html): no warnings for the new page. The equations, the code links and the toctree entry render correctly in the HTML.

CheckList

  • Did you add an unittest for your new example or defect fix?
  • Did you add documents for your new example?
  • All CIs are green? (You can check it after submitting)

🤖 Generated with Claude Code

Velocity and odometry motion models of Probabilistic Robotics, chapter 5,
each as a closed-form density and as a sampler, with tests and a docs page.
The velocity model uses the signed radius, so a forward right turn gets the
correct speed sign (fix AtsushiSakai#1516).

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Add probabilistic motion models (velocity and odometry) from Probabilistic Robotics, chapter 5

1 participant