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 57141948d..2afc93953 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)` @@ -1168,22 +1172,44 @@ 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.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 + 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: + 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 +1230,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 d8b642f58..57d76594f 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