Fixed-interval smoothing for PVAMEKF

src/smsfusion/__init__.py

@@ -4,6 +4,7 @@
     AMEKF,
     PVAMEKF,
     VAMEKF,
+    FixedIntervalSmoother,
     FixedNED,
     gravity,
 )
@@ -15,6 +16,7 @@
     "VAMEKF",
     "ConingScullingAlg",
     "ConingScullingAlgCalibrated",
+    "FixedIntervalSmoother",
     "FixedNED",
     "benchmark",
     "calibrate",

src/smsfusion/_ins/__init__.py

@@ -1,12 +1,14 @@
 from ._amekf import AMEKF
 from ._pvamekf import PVAMEKF
+from ._smoothing import FixedIntervalSmoother
 from ._utils import FixedNED, euler_from_acc, gravity
 from ._vamekf import VAMEKF
 
 __all__ = [
     "AMEKF",
     "PVAMEKF",
     "VAMEKF",
+    "FixedIntervalSmoother",
     "FixedNED",
     "euler_from_acc",
     "gravity",

src/smsfusion/_ins/_amekf.py

@@ -328,8 +328,8 @@ def update(
             A reference to the instance itself after the update.
         """
 
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
+        dvel = np.asarray(dvel).reshape(3)
+        dtheta = np.asarray(dtheta).reshape(3)
 
         if degrees:
             dtheta = (np.pi / 180.0) * dtheta

src/smsfusion/_ins/_pvamekf.py

@@ -209,7 +209,7 @@ def _reset(
         Attitude state estimate parameterized as a unit quaternion to be reset in place.
     bg_b : ndarray, shape (3,)
         Gyroscope bias state estimate to be reset in place.
-    dx : ndarray, shape (9,)
+    dx : ndarray, shape (12,)
         Error state vector containing the corrections to be applied to the state
         estimates. Will be reset to zero after applying the corrections.
     """
@@ -327,6 +327,9 @@ def __init__(
         self._bg_b = np.asarray_chkfinite(bg0).reshape(3).copy()
         self._P = np.asarray_chkfinite(P0).reshape(12, 12).copy()
         self._dx = np.zeros(12)
+        self._dx_before_reset = np.zeros(12)
+        self._dvel = np.zeros(3)
+        self._dtheta = np.zeros(3)
 
         # Discrete state-space model
         self._phi = _state_transition_matrix_init(
@@ -473,26 +476,28 @@ def update(
             A reference to the instance itself after the update.
         """
 
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
+        self._dvel[:] = np.asarray(dvel).reshape(3)
+        self._dtheta[:] = np.asarray(dtheta).reshape(3)
 
         if degrees:
-            dtheta = (np.pi / 180.0) * dtheta
+            self._dtheta[:] *= np.pi / 180.0
 
-        dtheta = dtheta - self._dt * self._bg_b
+        self._dtheta[:] = self._dtheta - self._dt * self._bg_b
 
         # Update state-space model
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)  # -> update phi
+        _state_transition_matrix_update(
+            self._phi, self._dvel, self._dtheta, R_nb
+        )  # -> update phi
 
         # Project (a priori) state estimates ahead
         _project_state_ahead(  # -> update p_n, v_n, q_nb (in place)
             self._p_n,
             self._v_n,
             self._q_nb,
             R_nb,
-            dvel,
-            dtheta,
+            self._dvel,
+            self._dtheta,
             self._dt,
             self._dvel_g_corr,
         )
@@ -544,7 +549,7 @@ def update(
                 self._P,
                 self._H[6:9],
                 vg_b,
-                dvel,
+                self._dvel,
                 np.asarray(gref_var),
             )
 
@@ -567,6 +572,7 @@ def update(
             )
 
         # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
+        self._dx_before_reset[:] = self._dx  # keep copy of dx estimate for smoothing
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self

src/smsfusion/_ins/_smoothing.py

@@ -0,0 +1,263 @@
+from typing import Any, Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import NDArray
+
+from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from ._common import _update_quaternion_with_gibbs2
+from ._pvamekf import PVAMEKF, _state_transition_matrix_update
+
+
+class FixedIntervalSmoother:
+    """
+    Fixed-interval smoothing for PVAMEKF.
+
+    This class wraps an instance of PVAMEKF, and maintains a time-ordered buffer
+    of state and error covariance estimates as measurements are processed via
+    the ``update()`` method. A backward sweep over the buffered data using the
+    Rauch-Tung-Striebel (RTS) algorithm [1] is performed to refine the filter
+    estimates.
+
+    Parameters
+    ----------
+    mekf : PVAMEKF
+        The underlying PVAMEKF instance used for forward filtering.
+    cov_smoothing : bool, optional
+        Whether to include the error covariance matrix, `P`, in the smoothing process.
+        Disabling the covariance smoothing has no effect on the smoothed state estimates,
+        and can reduce computation time if smoothed covariances are not required.
+
+    References
+    ----------
+    [1] R. G. Brown and P. Y. C. Hwang, "Random signals and applied Kalman
+        filtering with MATLAB exercises", 4th ed. Wiley, pp. 208-212, 2012.
+    """
+
+    def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None:
+        self._mekf = mekf
+        self._cov_smoothing = cov_smoothing
+        self.clear()
+
+    def clear(self) -> None:
+        """
+        Clear the internal buffers of state and covariance estimates. This resets
+        the smoother, and prepares it for a new interval of measurements.
+        """
+        # Buffers with estimates from the forward pass
+        self._p_buf: list[NDArray[np.float64]] = []
+        self._v_buf: list[NDArray[np.float64]] = []
+        self._q_buf: list[NDArray[np.float64]] = []
+        self._bg_buf: list[NDArray[np.float64]] = []
+        self._dx_buf: list[NDArray[np.float64]] = []
+        self._P_buf: list[NDArray[np.float64]] = []
+        self._dvel_buf: list[NDArray[np.float64]] = []
+        self._dtheta_buf: list[NDArray[np.float64]] = []
+
+        # Smoothed state and covariance estimates
+        self._p_n = np.empty((0, 3), dtype="float64")
+        self._v_n = np.empty((0, 3), dtype="float64")
+        self._q_nb = np.empty((0, 4), dtype="float64")
+        self._bg_b = np.empty((0, 3), dtype="float64")
+        self._P = np.empty((0, *self._mekf._P.shape), dtype="float64")
+
+    def update(self, *args: Any, **kwargs: Any) -> Self:
+        """
+        Update with IMU and aiding measurements, and buffer the resulting estimates
+        for smoothing.
+
+        The arguments are passed on to the underlying PVAMEKF instance unaltered.
+        See :meth:`smsfusion.PVAMEKF.update` for a full description of them.
+
+        Parameters
+        ----------
+        *args : tuple
+            Positional arguments passed on to ``PVAMEKF.update``.
+        **kwargs : dict
+            Keyword arguments passed on to ``PVAMEKF.update``.
+
+        Returns
+        -------
+        FixedIntervalSmoother
+            A reference to the instance itself after the update.
+
+        See Also
+        --------
+        smsfusion.PVAMEKF.update
+        """
+        self._mekf.update(*args, **kwargs)
+        self._p_buf.append(self._mekf.position())
+        self._v_buf.append(self._mekf.velocity())
+        self._q_buf.append(self._mekf.quaternion())
+        self._bg_buf.append(self._mekf.bias_gyro(degrees=False))
+        self._P_buf.append(self._mekf.P)
+        self._dx_buf.append(self._mekf._dx_before_reset.copy())
+        self._dvel_buf.append(self._mekf._dvel.copy())
+        self._dtheta_buf.append(self._mekf._dtheta.copy())
+        return self
+
+    def _smooth(self) -> None:
+        n_samples = len(self._q_buf)
+        if n_samples != len(self._p_n):
+            self._p_n, self._v_n, self._q_nb, self._bg_b, self._P = _rts_backward_sweep(
+                np.array(self._p_buf),
+                np.array(self._v_buf),
+                np.array(self._q_buf),
+                np.array(self._bg_buf),
+                np.array(self._P_buf),
+                np.array(self._dx_buf),
+                np.array(self._dvel_buf),
+                np.array(self._dtheta_buf),
+                self._mekf._phi.copy(),
+                self._mekf._Q,
+                self._cov_smoothing,
+            )
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Smoothed unit quaternion estimates.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 4)
+            Quaternion estimates for each of the N time steps where the smoother has
+            been updated with measurements.
+        """
+        self._smooth()
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Smoothed Euler angles estimates.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the Euler angles in degrees or radians. Defaults to radians.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 3)
+            Euler angles estimates for each of the N time steps where the smoother has
+            been updated with measurements.
+        """
+        self._smooth()
+        if self._q_nb.size == 0:
+            return np.empty((0, 3), dtype="float64")
+
+        theta = np.array([_euler_from_quaternion(q_i) for q_i in self._q_nb])
+
+        return np.degrees(theta) if degrees else theta
+
+    def position(self) -> NDArray[np.float64]:
+        """
+        Smoothed position estimates.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 3)
+            Position estimates for each of the N time steps where the smoother has
+            been updated with measurements.
+        """
+        self._smooth()
+        return self._p_n.copy()
+
+    def velocity(self) -> NDArray[np.float64]:
+        """
+        Smoothed velocity estimates.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 3)
+            Velocity estimates for each of the N time steps where the smoother has
+            been updated with measurements.
+        """
+        self._smooth()
+        return self._v_n.copy()
+
+    def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Smoothed gyroscope bias estimates.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 3)
+            Gyroscope bias estimates for each of the N time steps where the smoother has
+            been updated with measurements.
+        """
+        self._smooth()
+        bg_b = self._bg_b.copy()
+        return np.degrees(bg_b) if degrees else bg_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Error covariance matrix estimates.
+
+        If ``cov_smoothing=True``, smoothed error covariance estimates are returned.
+        Otherwise, the forward filter covariance estimates are returned.
+
+        Returns
+        -------
+        np.ndarray, shape (N, 12, 12)
+            Error covariance matrix estimates for each of the N time steps where
+            the smoother has been updated with measurements.
+        """
+        self._smooth()
+        return self._P.copy()
+
+
+@njit  # type: ignore[misc]
+def _rts_backward_sweep(
+    p_n: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    bg_b: NDArray[np.float64],
+    P: NDArray[np.float64],
+    dx: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    phi_k: NDArray[np.float64],
+    Q: NDArray[np.float64],
+    cov_smoothing: bool,
+) -> tuple[
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
+]:
+    """
+    Perform a backward sweep with the Rauch-Tung-Striebel (RTS) algorithm.
+    """
+
+    # Backward sweep
+    n = len(q_nb)
+    for k in range(n - 2, -1, -1):
+
+        # Update state space model for step k
+        R_nb_k = _rot_matrix_from_quaternion(q_nb[k])
+        _state_transition_matrix_update(phi_k, dvel[k + 1], dtheta[k + 1], R_nb_k)
+
+        # Calculate a priori error covariance matrix for step k + 1
+        P_prior_kp1 = phi_k @ P[k] @ phi_k.T + Q
+
+        # Smoothed error-state estimate and corresponding covariance
+        A = P[k] @ phi_k.T @ np.linalg.inv(P_prior_kp1)
+        ddx_k = A @ dx[k + 1]
+        dx[k] += ddx_k
+        if cov_smoothing:
+            P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T
+
+        # Smoothed state estimates
+        p_n[k] += ddx_k[0:3]
+        v_n[k] += ddx_k[3:6]
+        _update_quaternion_with_gibbs2(q_nb[k], ddx_k[6:9])
+        bg_b[k] += ddx_k[9:12]
+
+    return p_n, v_n, q_nb, bg_b, P

src/smsfusion/_ins/_vamekf.py

@@ -426,8 +426,8 @@ def update(
             A reference to the instance itself after the update.
         """
 
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
+        dvel = np.asarray(dvel).reshape(3)
+        dtheta = np.asarray(dtheta).reshape(3)
 
         if degrees:
             dtheta = (np.pi / 180.0) * dtheta