Skip to content

Fix EKF SLAM covariance prediction Jacobian orientation - #1475

Draft
yunyv wants to merge 3 commits into
AtsushiSakai:masterfrom
yunyv:fix/ekf-slam-covariance-prediction
Draft

yunyv wants to merge 3 commits into
AtsushiSakai:masterfrom
yunyv:fix/ekf-slam-covariance-prediction

Conversation

@yunyv

@yunyv yunyv commented Oct 26, 2026

Copy link
Copy Markdown

Reference issue

No existing open issue. This corrects the prediction transpose order in the existing EKF SLAM example; it is separate from the full-state covariance dimensions fixed in #973.

What does this implement/fix?

jacob_motion returns the Jacobian of the column-vector state transition. The prediction covariance must therefore be G @ PEst @ G.T, not G.T @ PEst @ G.

For example, with zero heading, velocity 2 m/s, DT=0.1, and only heading variance initially nonzero (diag(0, 0, 1)), the motion should contribute 0.04 to the y variance and 0.2 to the y/heading covariance. The current multiplication order leaves both at zero.

This changes the EKF's numerical covariance predictions and, consequently, its updates and data associations. It does not change the motion model, noise settings, default parameters, or public API.

  • Correct the single prediction expression.
  • Derive an independent test Jacobian by central finite differences of motion_model, covering three headings, zero/one/two landmarks, dense pose/map cross-covariances, and moving/stationary controls.
  • Keep the official EKF SLAM explanation, equation, and code excerpt consistent.

Additional information

Validation on Linux with the repository's pinned requirements:

  • Baseline, Python 3.14.8: 9 failed, 9 passed (stationary controls), with the existing simulation deselected.
  • Fixed EKF SLAM tests including the unanimated default simulation, Python 3.14.8: 19 passed. The same tests also passed on Python 3.13.5.
  • Ruff checks for both changed Python files and git diff --check: passed.
  • bash runtests.sh (including mypy, changed-file Ruff, and warnings-as-errors): 174 passed in 338.11s with BLAS/OMP threads limited to one.
  • make html: passed; generated HTML includes the corrected equation/code and remains linked from the SLAM parent page. Browser visual inspection was not available because the local browser's Unix socket was restricted by the execution sandbox.

CheckList

  • Did you add an unittest for your new example or defect fix?
  • Did you add documents for your new example? (Updated the existing official page.)
  • All CIs are green? (Upstream CI results are pending.)

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