From 03dab5bea06cff019069fd7573dfc3019eb374c6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 15:56:48 +1000 Subject: [PATCH 1/2] fix(particlefilter): get_xyt returns (x, y, theta) as documented The docstring and the name say (x, y, theta), as does EKF.get_xyt(), but the result had only the (x, y) columns. plot_xy() is the only caller in the toolbox and uses the first two. xyt[:, :2] is the previous result. Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/mobile/ParticleFilter.py | 7 ++++++- tests/test_vehicle_estimators.py | 11 +++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/roboticstoolbox/mobile/ParticleFilter.py b/src/roboticstoolbox/mobile/ParticleFilter.py index 5e5a6856a..ec1589e0f 100644 --- a/src/roboticstoolbox/mobile/ParticleFilter.py +++ b/src/roboticstoolbox/mobile/ParticleFilter.py @@ -654,9 +654,14 @@ def get_xyt(self): :return: vehicle trajectory where each row is configuration :math:`(x, y, \theta)` :rtype: ndarray(n,3) + .. versionchanged:: 1.5.0 + Returns the heading as well as the position, as documented and as + :meth:`EKF.get_xyt` does. Previously the result had only the + :math:`(x, y)` columns, ``xyt[:, :2]`` is that. + :seealso: :meth:`plot_xy` :meth:`run` :meth:`history` """ - return np.array([h.xest[:2] for h in self._history]) + return np.array([h.xest[:3] for h in self._history]) def get_std(self): r""" diff --git a/tests/test_vehicle_estimators.py b/tests/test_vehicle_estimators.py index 452aded3e..35783553f 100644 --- a/tests/test_vehicle_estimators.py +++ b/tests/test_vehicle_estimators.py @@ -397,7 +397,7 @@ def run_filter(self, nparticles: int = 500, T: float = 6, seed: int = 0): def test_converges_to_the_truth(self) -> None: pf, robot = self.run_filter() - error = np.linalg.norm(pf.get_xyt()[-1] - robot.x[:2]) + error = np.linalg.norm(pf.get_xyt()[-1, :2] - robot.x[:2]) self.assertLess(error, 1.0) def test_uncertainty_shrinks(self) -> None: @@ -409,10 +409,17 @@ def test_result_shapes(self) -> None: pf, robot = self.run_filter(nparticles=100, T=2) steps = round(2 / robot.dt) self.assertEqual(pf.get_t().shape, (steps,)) - self.assertEqual(pf.get_xyt().shape, (steps, 2)) + self.assertEqual(pf.get_xyt().shape, (steps, 3)) self.assertEqual(pf.get_std().shape, (steps, 3)) self.assertEqual(pf.x.shape, (100, 3)) + def test_xyt_includes_heading(self) -> None: + pf, _ = self.run_filter(nparticles=100, T=2) + xyt = pf.get_xyt() + # heading is the estimate in the history, within the wrapped range + nt.assert_array_equal(xyt[:, 2], [h.xest[2] for h in pf.history]) + self.assertTrue(np.all(np.abs(xyt[:, 2]) <= np.pi + 1e-9)) + def test_same_seed_same_estimate(self) -> None: a, _ = self.run_filter(nparticles=100, T=2, seed=5) b, _ = self.run_filter(nparticles=100, T=2, seed=5) From cb2811e695cb5972ba332660b4e7f88ed411141b Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 16:07:34 +1000 Subject: [PATCH 2/2] docs: the next release is 1.4.5, not 1.5.0 Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/mobile/ParticleFilter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roboticstoolbox/mobile/ParticleFilter.py b/src/roboticstoolbox/mobile/ParticleFilter.py index ec1589e0f..949e36a9a 100644 --- a/src/roboticstoolbox/mobile/ParticleFilter.py +++ b/src/roboticstoolbox/mobile/ParticleFilter.py @@ -654,7 +654,7 @@ def get_xyt(self): :return: vehicle trajectory where each row is configuration :math:`(x, y, \theta)` :rtype: ndarray(n,3) - .. versionchanged:: 1.5.0 + .. versionchanged:: 1.4.5 Returns the heading as well as the position, as documented and as :meth:`EKF.get_xyt` does. Previously the result had only the :math:`(x, y)` columns, ``xyt[:, :2]`` is that.