fix(ekf): plot_error in SLAM, against time, honouring bgcolor, with correct bounds

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)
 

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])

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