Skip to content

fix(mobile): use circular particle heading moments - #735

Open
Afloat16 wants to merge 3 commits into
petercorke:mainfrom
Afloat16:fix-particle-heading-moments-20261011
Open

Afloat16 wants to merge 3 commits into
petercorke:mainfrom
Afloat16:fix-particle-heading-moments-20261011

Conversation

@Afloat16

Copy link
Copy Markdown
Contributor

Particle headings are wrapped to [-pi, pi], but the particle-filter estimate averages them linearly. A cloud around pi can therefore report the opposite heading near zero. The angular spread also divides a square-root sum by N - 1, so replicating identical particles changes the reported uncertainty and a singleton produces NaN.

Use the circular heading mean and RMS wrapped residuals of the current equally weighted cloud, after native resampling when an observation is available. This matches the population-moment convention already used for Cartesian coordinates. Document the heading-spread units, the (n, 3) public result, and the undefined zero-resultant centre.

Validation: the same 13 new cases on a separately built pristine baseline produce 12 failures and one passing control; the complete matched baseline has 845 passes and the same 3 skips. The patched configured suite and CI coverage run each pass 857 tests, with 17 passing subtests. Both compiled extensions and Swift/collision/QP dependencies are present. The configured Sphinx HTML build passes with no Sphinx warnings. Formatting, new-test lint, and diff checks pass; source lint has the same four existing diagnostics on base and patch. Other operating systems, Python versions, and physical hardware were not tested.

@codacy-production

codacy-production Bot commented Oct 11, 2026 •

Copy link
Copy Markdown

Up to standards ✅

🟢 Issues 0 issues

Results:
0 new issues

View in Codacy

🟢 Metrics 8 complexity · 0 duplication

Metric Results
Complexity 8
Duplication 0

View in Codacy

NEW Get contextual insights on your PRs based on Codacy's metrics, along with PR and Jira context, without leaving GitHub. Enable AI reviewer
TIP This summary will be updated as you push new changes.

@codecov

codecov Bot commented Oct 13, 2026 •

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 0% with 3 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (acc6842) to head (13fa7a5).
⚠️ Report is 24 commits behind head on main.

Files with missing lines Patch % Lines
src/roboticstoolbox/mobile/ParticleFilter.py 0.00% 3 Missing ⚠️
Additional details and impacted files
@@          Coverage Diff          @@
##            main    #735   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        143     143           
  Lines      14269   14366   +97     
=====================================
- Misses     14269   14366   +97     

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@@ -509,12 +517,11 @@ def _step(self):

# our estimate is simply the mean of the particles
x_est = self.x.mean(axis=0)

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

computing the mean of column 2, and then overwriting it in next line is wasteful

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in 13fa7a5: the linear mean is now computed only over self.x[:, :2], so the heading column is no longer averaged and overwritten.


# our estimate is simply the mean of the particles
x_est = self.x.mean(axis=0)
x_est[2] = sp.stats.circmean(self.x[:, 2], low=-np.pi, high=np.pi)

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use spatialmath angle_mean()

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in 13fa7a5 to use spatialmath.base.angle_mean(self.x[:, 2]). The wrap-boundary and particle-replication regressions pass in the current CI run.

std_est[2] = np.sqrt(np.sum(smb.angdiff(self.x[:, 2], x_est[2]) ** 2)) / (
self.nparticles - 1
)
std_est[2] = np.sqrt(np.mean(smb.angdiff(self.x[:, 2], x_est[2]) ** 2))

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use spatialmath angle_std()

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in 13fa7a5 to use spatialmath.base.angle_std() on the centred heading residuals. This avoids round-off for replicated identical headings; the tests also cover singleton and near-zero-resultant clouds. The docstring now describes circular standard deviation and its unbounded spread near a zero resultant.

petercorke added a commit that referenced this pull request Oct 14, 2026
…es (#768)

Annotates the tools, demo, bin, models, ets and robot packages and holds them to disallow_untyped_defs. BaseETS is generic over its element type (BaseETS[ET], BaseETS[ET2]), replacing the self-type overloads. 68 unused pyright ignores removed, module/class name clashes made explicit. Left on the mypy to-do list: tools/trajectory.py (open #736), mobile/ParticleFilter.py (open #735), the two unused DHFactor modules and a stale test in tools/urdf/tests. Behaviour unchanged except on paths that already raised.
petercorke added a commit that referenced this pull request Oct 18, 2026
… cross-references, zero warnings (#833)

* docs(ets): ETS gets its own section, with 3D and 2D pages, tables and ET constructors

ets.rst replaces arm_ets.rst: corrected line references and the fkine
return type, and ETS is now a top-level item. ets_3d.rst and ets_2d.rst
document ET/ETS and ET2/ETS2 with Information, Modification and Kinematics
tables, the constructors, joint descriptor strings with examples, and the
three inverse kinematics mechanisms.

Fixes docstring examples that failed when built (ET2/ETS2 copied from the
3D case, split, swap, merge), docutils warnings, and adds missing
docstrings for plot, teach, ETS2.jacob0, qlim and s.

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

* docs: remove the page-level Jesse Haviland credit, he is first author in the footer

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

* docs(ets): overview table lists the page, the sequence class and the element class

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

* feat(ets): fkine and eval take no argument for a sequence without joints

q is now optional on ETS and ETS2 fkine and eval, and means an empty joint
vector. A sequence with joints still raises a ValueError that names the
lengths when q is missing or too short; tests pin both behaviours.

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

* docs(ets): say angle or distance, not amount

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

* docs(ets): public class names, constructor arguments, joint descriptor rules as text, IK examples

Documents ET, ETS, ET2 and ETS2 under their public names, drops the module
prefix from signatures (add_module_names = False), lists the constructor
arguments, moves the joint descriptor rules out of the BaseET class box,
adds fixjoint to the Modification tables, and rewrites the opening examples
and the IK section.

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

* docs: fix typos in docstrings and pages, and the warnings the docs build printed

Typos found with codespell in rendered docstrings and docs/source (ParticleFilter,
DHFactor, Dynamixel and ROS left alone). A docstring with an escaped asterisk
is now raw. The quintic example no longer calls plot() in a runblock, where
there is no display. Robot.manipulability(method='asada') uses eigvalsh,
since eig returned a complex array for a matrix symmetric to rounding error,
which printed a ComplexWarning.

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

* docs: correct the introduction, IK, mobile and blocks pages after a read-through

Stale line references and a named-tuple claim in the introduction, a nonexistent
ETSLink and manipm, a misplaced History section, URDF models described as DH,
the planner tables, a wrong seg-fault claim for the C++ IK solvers, dead
placeholders (:py:class:`Class`), and the broken bdsim logo URL.

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

* fix(ik): a q0 of the wrong length is an error, in the C++ and the Python solvers

The C++ solvers (ik_LM, ik_NR, ik_GN) ignored a q0 of the wrong length and
started from a random vector, so the caller got a solution for a different
start than the one they asked for. The Python solvers raised an unhelpful
NumPy broadcast error. Both now raise ValueError naming the length expected,
through one helper, check_q0.

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

* fix(ik): a mask that is not 6 elements is an error

The C++ solvers read six elements from the mask, so a shorter one was read
past its end; the Python solvers failed later with a matrix shape error.

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

* docs: describe the q0 and mask checks of the C++ IK solvers; explain eigvalsh

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

* fix(ets): swap accepts transforms that commute, merge rejects a joint, ET2.s is a 3-vector

swap only accepted two transforms of the same kind although it is documented
for commutative ones: it now accepts translations along different axes and a
rotation with a translation along its axis, and names the pair when it refuses.
merge raised TypeError for a joint and a constant, now ValueError. ET2.s
returned a 4-vector, a copy of the 3D case, now the 3-vector twist. Two
seealso references to SerialLink.jtraj now point at jtraj.

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

* docs: resolve the cross-references that the strict build reports

Fix at the source the references that named a short or a wrong target (ETS, IKSolver.solve,
plot_bg, spatialmath.SE3, Link.*, DHRobot.*), turn names that were never
objects into literals, index the methods of the IK solver classes so they can
be linked, document URDFRobot and NoPathError, and list in nitpick_ignore_regex
the few kinds that can never resolve. The strict build (sphinx-build -n) went
from 1650 warnings to 5, three of them in ParticleFilter, left for #735.

Also the partial_fkine0 example prints the shape of the result, not the matrix.

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

* docs: two more cross-reference fixes (hessian0, a missing colon in a :math: role)

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

---------

Co-authored-by: Claude Sonnet 5.5 <noreply@anthropic.com>
@petercorke
petercorke enabled auto-merge (squash) October 19, 2026 04:22
@petercorke

Copy link
Copy Markdown
Owner

Thank you for your contribution to RTB, and for the careful fix to the particle filter's heading estimate. The circular mean and spread are exactly right, the regression tests around ±π and for replicated particles are a welcome addition, and thank you for addressing the review comments so thoroughly. It is approved and will be merged as soon as the checks pass.

One request for the future: if an AI tool helped with a change, please say so in the PR description, as the repository's AGENTS.md asks. It helps the reviewer know what to look for.

@petercorke

Copy link
Copy Markdown
Owner

Thank you again. One small request so that we can merge: could you update this branch with main, using the "Update branch" button or a merge or rebase? The branch has to be up to date before it can merge, and I can't push to your fork. Once it is, it will go in straight away.

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.

2 participants