From 7f3497dec51c35aab3d22f2c973d4e695be5b6b2 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 12:15:13 +1000 Subject: [PATCH 1/3] fix(ekf): plot_error in SLAM, against time, honouring bgcolor, with correct bounds - the error subtracted the whole state vector from the 3-element true pose, which raised for SLAM where the state also holds the landmarks - the error was plotted against sample index on axes limited to time, so only the first part of the run was visible - bgcolor was documented but ignored (the polygon was always red) - the bounds used chi-squared with 2 degrees of freedom for a scalar error, so the shaded region was 2.45 sigma rather than the 1.96 sigma of a 95% interval - confidence=None computed chi2.ppf(None) and failed - raise a clear ValueError for a map-only filter, which has no pose estimate - document the ax parameter, fix a stray backtick Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/mobile/EKF.py | 32 ++++++++++----- tests/test_vehicle_estimators.py | 67 ++++++++++++++++++++++++++++--- 2 files changed, 83 insertions(+), 16 deletions(-) diff --git a/src/roboticstoolbox/mobile/EKF.py b/src/roboticstoolbox/mobile/EKF.py index 57141948d..746fc1c4d 100644 --- a/src/roboticstoolbox/mobile/EKF.py +++ b/src/roboticstoolbox/mobile/EKF.py @@ -1149,15 +1149,19 @@ def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs r""" Plot error with uncertainty bounds - :param bgcolor: background color, defaults to 'r' + :param bgcolor: color of the confidence bound polygons, defaults to 'r' :type bgcolor: str, optional - :param confidence: confidence interval, defaults to 0.95 + :param confidence: confidence interval, defaults to 0.95, None for no bounds :type confidence: float, optional + :param ax: axes to plot into, one for each of :math:`(x, y, \theta)`, + defaults to a new figure with three stacked axes + :type ax: array_like(3) of Axes, optional :param block: hold plot until figure is closed, defaults to None :type block: bool, optional + :raises ValueError: if the vehicle pose is not part of the estimated state Plot the error between actual and estimated vehicle - path :math:`(x, y, \theta)`` versus time as three stacked plots. + path :math:`(x, y, \theta)` versus time as three stacked plots. Heading error is wrapped into the range :math:`[-\pi,\pi)` @@ -1170,20 +1174,28 @@ def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs :seealso: :meth:`get_P` :meth:`run` :meth:`history` """ + if not self._est_vehicle: + raise ValueError("the vehicle pose is not part of the estimated state") + error = [] bounds = [] - ppf = chi2.ppf(confidence, df=2) + # the bound on a scalar error is the interval that contains + # ``confidence`` of a 1-dimensional Gaussian, ie. chi-squared with one + # degree of freedom + ppf = chi2.ppf(confidence, df=1) if confidence is not None else None x_gt = self.robot.x_hist for k in range(len(self.history)): hk = self.history[k] - # error is true - estimated - e = x_gt[k, :] - hk.xest + # error is true - estimated, the vehicle pose is the first three + # elements of the state which, for SLAM, is followed by the landmarks + e = x_gt[k, :3] - hk.xest[:3] e[2] = base.wrap_mpi_pi(e[2]) error.append(e) - P = np.diag(hk.P) - bounds.append(np.sqrt(ppf * P[:3])) + if ppf is not None: + P = np.diag(hk.P) + bounds.append(np.sqrt(ppf * P[:3])) error = np.array(error) bounds = np.array(bounds) @@ -1204,10 +1216,10 @@ def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs ] ) polygon = plt.Polygon( - edge.T, closed=True, facecolor="r", edgecolor="none", alpha=0.3 + edge.T, closed=True, facecolor=bgcolor, edgecolor="none", alpha=0.3 ) ax.add_patch(polygon) - ax.plot(error[:, k], **kwargs) + ax.plot(t, error[:, k], **kwargs) ax.grid(True) ax.set_ylabel(labels[k] + " error") ax.set_xlim(0, t[-1]) diff --git a/tests/test_vehicle_estimators.py b/tests/test_vehicle_estimators.py index 452aded3e..17014514f 100644 --- a/tests/test_vehicle_estimators.py +++ b/tests/test_vehicle_estimators.py @@ -366,17 +366,72 @@ def test_plot_error_dead_reckoning(self) -> None: ekf.plot_error() plt.close("all") - @unittest.expectedFailure - def test_plot_error_slam(self) -> None: - # the error is computed against the whole state vector, which includes - # the landmarks in SLAM, so the shapes (3,) and (3+2N,) do not match + def slam(self, T: float = 5) -> tuple[EKF, Bicycle]: robot = self.robot() map = self.map() - ekf = EKF(robot=(robot, V), P0=P0, sensor=(self.sensor(robot, map), W), animate=False) - quiet(ekf.run, T=5) + ekf = EKF( + robot=(robot, V), P0=P0, sensor=(self.sensor(robot, map), W), animate=False + ) + quiet(ekf.run, T=T) + return ekf, robot + + def test_plot_error_slam(self) -> None: + # the state vector includes the landmarks, only the pose is compared + ekf, _ = self.slam() + ekf.plot_error() + plt.close("all") + + def test_plot_error_is_against_time(self) -> None: + ekf, _ = self.dead_reckoning(T=5) ekf.plot_error() + axes = plt.gcf().axes + self.assertEqual(len(axes), 3) + for ax in axes: + line = ax.get_lines()[0] + nt.assert_allclose(line.get_xdata(), ekf.get_t()) + self.assertEqual(len(line.get_ydata()), len(ekf.history)) + plt.close("all") + + def test_plot_error_error_and_bounds(self) -> None: + from scipy.stats import norm + + ekf, robot = self.slam() + ekf.plot_error(confidence=0.95) + ax = plt.gcf().axes[0] + # error is truth - estimate, for the x coordinate + expected = np.array([robot.x_hist[k, 0] - h.xest[0] for k, h in enumerate(ekf.history)]) + nt.assert_allclose(ax.get_lines()[0].get_ydata(), expected) + # a 95% interval on a scalar quantity is 1.96 sigma + sigma = np.array([np.sqrt(h.P[0, 0]) for h in ekf.history]) + vertices = ax.patches[0].get_xy() + upper = vertices[: len(ekf.history), 1] + nt.assert_allclose(upper, norm.ppf(0.975) * sigma, rtol=1e-9) plt.close("all") + def test_plot_error_bgcolor(self) -> None: + from matplotlib.colors import to_rgb + + ekf, _ = self.dead_reckoning(T=2) + ekf.plot_error(bgcolor="g") + for ax in plt.gcf().axes: + nt.assert_allclose(ax.patches[0].get_facecolor()[:3], to_rgb("g")) + plt.close("all") + + def test_plot_error_without_bounds(self) -> None: + ekf, _ = self.dead_reckoning(T=2) + ekf.plot_error(confidence=None) + for ax in plt.gcf().axes: + self.assertEqual(len(ax.patches), 0) + plt.close("all") + + def test_plot_error_needs_a_vehicle_estimate(self) -> None: + robot = self.robot(covar=None) + map = self.map() + ekf = EKF(robot=(robot, None), sensor=(self.sensor(robot, map), W), animate=False) + quiet(ekf.run, T=2) + with self.assertRaises(ValueError): + ekf.plot_error() + class TestParticleFilter(unittest.TestCase): R = np.diag([0.1, 0.1, np.radians(1)]) ** 2 From cfabe71c7ab77aa67e0a27b6da362284d9719e78 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 15:59:24 +1000 Subject: [PATCH 2/3] docs(ekf): note the change in plot_error behavior in the docstring and changelog The bounds were drawn at the radius of a two-dimensional confidence region, which is too wide for a scalar error; the figure in RVC3 (Fig 6.10) was made that way. Record the change, and that earlier plots differ, in a versionchanged note and in the changelog highlights. Co-Authored-By: Claude Sonnet 5.5 --- CHANGELOG.md | 9 +++++++++ src/roboticstoolbox/mobile/EKF.py | 14 ++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 32419b608..dd305cfac 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,15 @@ all 26 pages, stale/incorrect content fixed, ROS references removed, and new pages added — Panda model comparison, reactive control, inverse kinematics, and IK/RNE benchmark results with real numbers. +**Changed behavior: `EKF.plot_error()`.** The confidence bounds are now the +interval containing the requested fraction of a one-dimensional Gaussian +(±1.96σ for `confidence=0.95`); earlier versions drew ±2.45σ, which is the +95% radius of a two-dimensional region and was too wide for a scalar error. +The error is now plotted against time (it was plotted against sample number on +axes limited to time), `bgcolor` is honored, and SLAM no longer raises. +Figures regenerated with this version, for example Figure 6.10 in *Robotics, +Vision & Control* (3rd ed.), have narrower shaded bounds than the printed ones. + ## [1.4.4](https://github.com/petercorke/robotics-toolbox-python/compare/v1.4.3...v1.4.4) (2026-09-20) diff --git a/src/roboticstoolbox/mobile/EKF.py b/src/roboticstoolbox/mobile/EKF.py index 746fc1c4d..c8af6c911 100644 --- a/src/roboticstoolbox/mobile/EKF.py +++ b/src/roboticstoolbox/mobile/EKF.py @@ -1172,6 +1172,20 @@ def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs .. note:: Observations will decrease the uncertainty while periods of dead-reckoning increase it. + .. versionchanged:: 1.5.0 + - The ``confidence`` bounds are the interval that contains that + fraction of a one-dimensional Gaussian, :math:`\pm 1.96\sigma` + for 0.95. Earlier versions used the radius of a + two-dimensional confidence region, :math:`\pm 2.45\sigma` for + 0.95, which made the shaded region about 25% wider than its + label. Plots produced with earlier versions show wider bounds. + - The error is plotted against time. Earlier versions plotted it + against sample number on axes limited to the time range, so only + the start of a run was visible. + - ``bgcolor`` is used, it was previously ignored. + - Works for SLAM, where the state also holds the landmarks, and + raises ``ValueError`` if the vehicle pose is not estimated. + :seealso: :meth:`get_P` :meth:`run` :meth:`history` """ if not self._est_vehicle: From 641a83f0ad36054b509ad032863a1c1fc1227a5a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 16:07:29 +1000 Subject: [PATCH 3/3] docs: the next release is 1.4.5, not 1.5.0 Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/mobile/EKF.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roboticstoolbox/mobile/EKF.py b/src/roboticstoolbox/mobile/EKF.py index c8af6c911..2afc93953 100644 --- a/src/roboticstoolbox/mobile/EKF.py +++ b/src/roboticstoolbox/mobile/EKF.py @@ -1172,7 +1172,7 @@ def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs .. note:: Observations will decrease the uncertainty while periods of dead-reckoning increase it. - .. versionchanged:: 1.5.0 + .. versionchanged:: 1.4.5 - The ``confidence`` bounds are the interval that contains that fraction of a one-dimensional Gaussian, :math:`\pm 1.96\sigma` for 0.95. Earlier versions used the radius of a