Skip to content

feat(localization): add OdometryFusion (E1) and LandmarkLocalization (E3) - #39

Merged
gabrielfrasantos merged 5 commits into
mainfrom
claude/determined-einstein-bznlpi
Nov 5, 2026
Merged

gabrielfrasantos merged 5 commits into
mainfrom
claude/determined-einstein-bznlpi

Conversation

@gabrielfrasantos

Copy link
Copy Markdown
Contributor

Bumps numerical-toolbox to b93d6e0, which brings in ResidualKalmanUpdate (NT74, embedded-pro/numerical-toolbox-cpp#367). Then it ships the two localization specs that NT74 unblocked.

Commit Item
f7b15dc + c91acfc Pin bump for NT74. The first commit pinned the upstream PR's commit; the second repoints the pin to the merge commit b93d6e0.
f4d8d37 E1 OdometryFusion: wheel and gyroscope EKF on SE(2) with a gyro-bias state.
3b64372 E3 LandmarkLocalization: range–bearing EKF against a known landmark map.

Each item commit includes the header, coverage .cpp, tests (the spec's 6 cases each), doc, index rows, and removal of the spec and its roadmap rows. robotics.localization now links numerical.filters.active and robotics.mobile.

Highlights

  • E1:
    • The mean follows the exact SE(2) arc; the covariance uses midpoint Jacobians, speed-scaled wheel noise and a bias random walk.
    • The wheel yaw-rate, heading and position updates all go through NT74. A χ² gate on the wheel update catches slip, and heading residuals are wrapped.
  • E3:
    • Body-frame increment noise comes from F1's IncrementDeviation, rotated into the world frame.
    • Each observation is matched to a landmark by id, or by smallest Mahalanobis distance. Matches are χ²-gated and the bearing residual is wrapped.

Deviations from the specs

  • E3 tests:
    • The association test also asserts the spec's reference NIS values against the other landmarks.
    • outlier_is_gated also checks two of the spec's edge cases, which keeps coverage at 100%: an unknown id returns Singular with no landmark, and a map larger than MaxLandmarks is rejected.
  • E3 association is split into FindById and Nearest helpers.
  • E3 coverage instantiates the test size <float, 8> alongside the spec's <float, 16>.
  • Docs and roadmap:
    • The roadmap/README.md canonical example now points to localization/MonteCarloLocalization.
    • The NT74 link now points to the upstream doc.
    • The README overview lists localization.

Verification

  • Debug and coverage ctest: 7/7.
  • 100% merged line coverage on OdometryFusion.hpp and LandmarkLocalization.hpp.
  • Clang -Wall -Wextra -Werror -Wshadow with -DROBOTICS_TOOLBOX_COVERAGE_BUILD, with dependency headers treated as system headers.
  • clang-format.
  • validate-docs OK; no doc line over 400 characters; tables aligned.
  • Not run locally: Qt preset, MSVC, cppcheck, SonarCloud.

Generated by Claude Code

Wheel and gyroscope EKF on SE(2) with state (x, y, heading, gyro bias):
the mean follows the exact SE(2) arc, the covariance uses midpoint
Jacobians with speed-scaled wheel noise and a bias random walk, and the
wheel yaw-rate, heading and position updates go through the upstream
residual-form Kalman update, so slip is caught by the chi-square gate
and heading residuals are wrapped. The roadmap canonical example now
points at MonteCarloLocalization.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN
Range-bearing EKF against a known landmark map: body-frame odometry
increments composed onto the pose with the F1 increment noise rotated
into the world, observations matched by identity or by minimum
Mahalanobis distance, chi-square gated and applied through the upstream
residual-form Kalman update with the bearing residual wrapped.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN
@github-actions

github-actions Bot commented Nov 5, 2026 •

Copy link
Copy Markdown

✅⚠️MegaLinter analysis: Success with warnings

Descriptor Linter Files Fixed Errors Max errors Warnings Elapsed time
✅ ACTION actionlint 7 0 0 0.52s
✅ CPP clang-format 201 0 0 0 1.71s
✅ CPP cppcheck 201 0 0 3.11s
✅ DOCKERFILE hadolint 1 0 0 0.28s
✅ JSON jsonlint 8 0 0 0.09s
✅ JSON prettier 8 2 0 0 0.28s
✅ MARKDOWN markdownlint 90 0 0 0 1.77s
✅ MARKDOWN markdown-table-formatter 90 0 0 0 0.18s
⚠️ SPELL lychee 137 2 0 2.14s
✅ YAML prettier 12 0 0 0 0.4s
✅ YAML yamllint 12 0 0 0.45s

Detailed Issues

⚠️ SPELL / lychee - 2 errors
[ERROR] failed to verify TLS certificate: invalid peer certificate: UnknownIssuer
📝 Summary
---------------------
🔍 Total..........357
🔗 Unique.........127
✅ Successful.....345
⏳ Timeouts.........0
🔀 Redirected.......5
👻 Excluded.........9
❓ Unknown..........0
🚫 Errors...........2
⛔ Unsupported......2

Errors in .github/workflows/static-analysis.yml
[403] https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-$ (at 37:21) | Rejected status code: 403 Forbidden

Errors in doc/kinematics/InverseKinematics.md
[ERROR] https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf (at 125:169) | SSL certificate not trusted. Use --insecure if site is trusted

Hint: Followed 5 redirects. You might want to consider replacing redirecting URLs with the resolved URLs. Use verbose mode (`-v`/`-vv`) to see redirection details.
Hint: You can configure accepted/rejected response codes with `-a` or `--accept`

Notices

⚠️ Your configuration references items that have been removed from MegaLinter and are ignored: REPOSITORY_GITLEAKS, REPOSITORY_KICS. See Removed linters to find their replacements.

See detailed reports in MegaLinter artifacts

Your project could benefit from a custom flavor, which would allow you to run only the linters you need, and thus improve runtime performances. (Skip this info by defining FLAVOR_SUGGESTIONS: false)

  • Documentation: Custom Flavors
  • Command: npx mega-linter-runner@10.1.0 --custom-flavor-setup --custom-flavor-linters ACTION_ACTIONLINT,CPP_CPPCHECK,CPP_CLANG_FORMAT,DOCKERFILE_HADOLINT,JSON_JSONLINT,JSON_PRETTIER,MARKDOWN_MARKDOWNLINT,MARKDOWN_MARKDOWN_TABLE_FORMATTER,SPELL_LYCHEE,YAML_PRETTIER,YAML_YAMLLINT

MegaLinter is provided by OX Security
Show us your support by starring ⭐ the repository

…claration and definition

The declaration named the position covariance 'covariance', which also
shadowed the filter's covariance member.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN
@sonarqubecloud

sonarqubecloud Bot commented Nov 5, 2026

Copy link
Copy Markdown

@gabrielfrasantos
gabrielfrasantos merged commit 33d39fd into main Nov 5, 2026
12 checks passed
@gabrielfrasantos
gabrielfrasantos deleted the claude/determined-einstein-bznlpi branch November 5, 2026 19:56
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.

2 participants