Skip to content

feat(localization): add probabilistic motion models from chapter 5 - #1523

Open
NTbankey1 wants to merge 2 commits into
AtsushiSakai:masterfrom
NTbankey1:NTbankey1/add-probabilistic-motion-models-velocity-and-odo
Open

NTbankey1 wants to merge 2 commits into
AtsushiSakai:masterfrom
NTbankey1:NTbankey1/add-probabilistic-motion-models-velocity-and-odo

Conversation

@NTbankey1

Copy link
Copy Markdown

Implement the velocity and odometry motion models of Thrun, Burgard and Fox, Probabilistic Robotics, chapter 5.

  • velocity model: closed form of Table 5.1 and sampler of Table 5.3, with the signed radius of equation 5.17 and wrapped heading differences
  • odometry model: closed form of Table 5.5 and sampler of Table 5.6, with frame-invariant handling of a zero translation
  • tests for straight and backward motion, angle wrapping, the forward right turn, frame invariance, the pure-rotation and large-turn limits, sampler/density agreement, and the missing Jacobian
  • documentation page with the equations, the two book findings, and the identifiability limits

Refs: #1516

Reference issue

What does this implement/fix?

Additional information

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)

Implement the velocity and odometry motion models of Thrun, Burgard and
Fox, Probabilistic Robotics, chapter 5.

- velocity model: closed form of Table 5.1 and sampler of Table 5.3, with
  the signed radius of equation 5.17 and wrapped heading differences
- odometry model: closed form of Table 5.5 and sampler of Table 5.6, with
  frame-invariant handling of a zero translation
- tests for straight and backward motion, angle wrapping, the forward
  right turn, frame invariance, the pure-rotation and large-turn limits,
  sampler/density agreement, and the missing Jacobian
- documentation page with the equations, the two book findings, and the
  identifiability limits

Refs: AtsushiSakai#1516

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

Copy link
Copy Markdown
Author

Hi @AtsushiSakai, the GitHub Actions checks on this PR (Linux_CI, MacOS_CI, Windows_CI, Code scanning) are all showing action_required — they are waiting for a maintainer to approve the workflow runs because this is my first contribution from a fork.

The change has been verified locally with the same environment as CI (Python 3.14 + pinned requirements/requirements.txt):

  • full suite (pytest tests -l -Werror): 172 passed
  • ruff check on the changed files: no issues
  • CircleCI build_doc and AppVeyor are already green

Could you approve the workflow runs when you have a moment? Thanks!

Tables 5.1 and 5.3 of Probabilistic Robotics specify the velocity noise
variances as alpha1 v^2 + alpha2 omega^2, and Tables 5.5 and 5.6 specify
the odometry variances as alpha1 d_rot1^2 + alpha2 d_trans^2, and so on.
The implementation used absolute values instead, and the odometry
translation variance grouped the rotations as
alpha4 |d_rot1 + d_rot2| instead of alpha4 d_rot1^2 + alpha4 d_rot2^2.

Update the module, its tests and the documentation to match the book,
and refresh the numbers quoted in the docs.

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.

1 participant