diff --git a/src/roboticstoolbox/mobile/ParticleFilter.py b/src/roboticstoolbox/mobile/ParticleFilter.py index 6eeec5509..574db3391 100644 --- a/src/roboticstoolbox/mobile/ParticleFilter.py +++ b/src/roboticstoolbox/mobile/ParticleFilter.py @@ -652,9 +652,14 @@ def get_xyt(self): :return: vehicle trajectory where each row is configuration :math:`(x, y, \theta)` :rtype: ndarray(n,3) + .. 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. + :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 57d76594f..3a0c5635c 100644 --- a/tests/test_vehicle_estimators.py +++ b/tests/test_vehicle_estimators.py @@ -456,7 +456,7 @@ def test_no_pause_unless_animating(self) -> None: 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: @@ -468,10 +468,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)