Repository navigation
Conversation
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
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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:
(v, w), noise parameters alpha1 to alpha6.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 byshow_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:
vandwhave 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).delta_transgives 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
Local results (Python 3.13,
requirements/requirements.txt):pytest tests -l -Werror: 165 passed (including the 9 new tests)ruff checkandmypyon the new files: no issues.sphinx -b html): no warnings for the new page. The equations, the code links and the toctree entry render correctly in the HTML.CheckList
🤖 Generated with Claude Code