From c12b10f3f14b92339de227db984650a7b9d10747 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 12:56:39 +0100
Subject: [PATCH 001/217] init method

---
 src/smsfusion/_v2.py | 246 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 246 insertions(+)
 create mode 100644 src/smsfusion/_v2.py

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
new file mode 100644
index 00000000..7429ce28
--- /dev/null
+++ b/src/smsfusion/_v2.py
@@ -0,0 +1,246 @@
+from numba import njit
+import numpy as np
+from numpy.typing import ArrayLike, NDArray
+
+from ._ins import dhda_head
+from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
+
+
+def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
+    """
+    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
+
+    Parameters
+    ----------
+    g : float
+        Gravitational acceleration in m/s^2.
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame in which the gravity vector is expressed.
+
+    Returns
+    -------
+    ndarray, shape (3,)
+        Gravity vector expressed in the navigation frame.
+    """
+    if nav_frame.lower() == "ned":
+        g_n = np.array([0.0, 0.0, g])
+    elif nav_frame.lower() == "enu":
+        g_n = np.array([0.0, 0.0, -g])
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+    return g_n
+
+
+def _nz2vg(nav_frame: str) -> float:
+    """
+    Gravity direction along the navigation frame's z-axis.
+    """
+    if nav_frame == "ned":
+        return 1.0
+    elif nav_frame == "enu":
+        return -1.0
+    else:
+        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+
+@njit  # type: ignore[misc]
+def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
+    """
+    Gravity reference vector expressed in the body frame, computed from the attitude
+    quaternion, q_nb.
+
+    Parameters
+    ----------
+    q_nb : numpy.ndarray, shape (4,)
+        Unit quaternion.
+    nz2vg : float
+        Gravity direction along the navigation frame's z-axis. Should be +1 for
+        NED and -1 for ENU.
+    """
+    qw, qx, qy, qz = q_nb
+
+    x = 2.0 * (qx * qz - qw * qy)
+    y = 2.0 * (qy * qz + qw * qx)
+    z = 1.0 - 2.0 * (qx**2 + qy**2)
+
+    return nz2vg * np.array([x, y, z])
+
+
+def _state_transition(
+    dt: float, w_b: NDArray[np.float64], gbc: float
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    w_b : ndarray, shape (3,)
+        Angular rate measurement (bias corrected) in body frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (6, 6)
+        State transition matrix.
+    """
+    phi = np.eye(6)
+    phi[0:3, 0:3] -= dt * _skew_symmetric(w_b)  # NB! update each time step
+    phi[0:3, 3:6] -= dt * np.eye(3)
+    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition(
+    phi: NDArray[np.float64],
+    dt: float,
+    w_b: NDArray[np.float64],
+) -> None:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (6, 6)
+        State transition matrix to be updated in place.
+    dt : float
+        Time step.
+    w_b : ndarray, shape (3,)
+        Angular rate measurement (bias corrected) in body frame.
+    """
+    wx, wy, wz = w_b
+    phi[0, 1] = dt * wz
+    phi[0, 2] = -dt * wy
+    phi[1, 0] = -dt * wz
+    phi[1, 2] = dt * wx
+    phi[2, 0] = dt * wy
+    phi[2, 1] = -dt * wx
+
+
+def _process_noise_cov(
+    dt: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (6, 6)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((6, 6))
+    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
+    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+def _measurement_matrix(
+    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    vg_b : ndarray, shape (3,)
+        Gravity reference unit vector expressed in the body frame.
+
+    Returns
+    -------
+    ndarray, shape (4, 6)
+        Linearized measurement matrix.
+    """
+    dhdx = np.zeros((4, 6))
+    dhdx[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity ref vector
+    dhdx[3:4, 0:3] = dhda_head(q_nb)  # heading
+    return dhdx
+
+
+class VRU:
+    """
+    Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    q_nb : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg_b : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    w_b : array_like, shape (3,), optional
+        Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame.
+        Defaults to zero angular rate (stationary).
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix, **P**. If not
+        given, a small diagonal matrix will be used.
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+    _I: NDArray[np.float64] = np.eye(6)
+
+    def __init__(
+        self,
+        fs: float,
+        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg_b: ArrayLike = (0.0, 0.0, 0.0),
+        w_b: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = 1e-6 * np.eye(6),
+        gyro_noise_density: float = 0.0001,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+
+        # IMU noise parameters
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
+        self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
+        self._dx = np.zeros(6)
+
+        # Discrete state-space model
+        self._phi = _state_transition(self._dt, self._w_b, self._gbc)
+        self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
+        self._dhdx = _measurement_matrix(self._q_nb, self._vg_b)
+
+    @property
+    def _vg_b(self):
+        """Gravity reference vector (unit vector) expressed in the body frame."""
+        return _vg_b(self._q_nb, self._nz2vg)

From cc21eae8e05f171c7f5f8b07dbe5f6df9c7d6590 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 12:59:58 +0100
Subject: [PATCH 002/217] properties

---
 src/smsfusion/_v2.py | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 7429ce28..f543915d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -244,3 +244,28 @@ def __init__(
     def _vg_b(self):
         """Gravity reference vector (unit vector) expressed in the body frame."""
         return _vg_b(self._q_nb, self._nz2vg)
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Return a copy of the attitude quaternion.
+        """
+        return self._q_nb.copy()
+
+    def bias_gyro(self) -> NDArray[np.float64]:
+        """
+        Return a copy of the gyroscope bias estimate (rad/s) expressed in the body frame.
+        """
+        return self._bg_b.copy()
+
+    def angular_rate(self) -> NDArray[np.float64]:
+        """
+        Return a copy of the bias corrected angular rate measurement (rad/s).
+        """
+        return self._w_b.copy()
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()

From f57c6efc4fc2f75fc9c7d02150136effddf03d1c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:07:09 +0100
Subject: [PATCH 003/217] more methods

---
 src/smsfusion/_v2.py | 63 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 63 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index f543915d..efa695f1 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -172,6 +172,43 @@ def _measurement_matrix(
     return dhdx
 
 
+@njit  # type: ignore[misc]
+def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
+    as a scaled (2x) Gibbs vector:
+
+        q = q ⊗ dq(da)
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion [qw, qx, qy, qz] (modified in place).
+    da : ndarray, shape (3,)
+        Small attitude error parameterized as a scaled (2x) Gibbs vector.
+
+    Notes
+    -----
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
+    References
+    ----------
+    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    and Control, Eq. (6.27)-(6.28).
+    """
+
+    qw, qx, qy, qz = q
+    dax, day, daz = da
+
+    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
+    q[:] = _normalize(q)
+
+
 class VRU:
     """
     Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF).
@@ -269,3 +306,29 @@ def P(self) -> NDArray[np.float64]:
         Copy of the error covariance matrix estimate.
         """
         return self._P.copy()
+
+    def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Gravity reference vector part of the measurement matrix, shape (3, 6).
+        """
+        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
+        return self._dhdx[0:3]
+
+    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Heading (yaw angle) part of the measurement matrix, shape (6,).
+        """
+        self._dhdx[3:4, 0:3] = dhda_head(q_nb)
+        return self._dhdx[3]
+
+    def _reset(self) -> None:
+        """
+        Reset state.
+        """
+
+        if not self._dx.any():
+            return
+
+        _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3])
+        self._bg_b[:] += self._dx[3:6]
+        self._dx[:] = 0.0

From 22271e3ce0e6f169e898c6717b25b4ef128491e8 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:14:05 +0100
Subject: [PATCH 004/217] more code

---
 src/smsfusion/_v2.py | 179 +++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 172 insertions(+), 7 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index efa695f1..89ac2619 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -2,7 +2,7 @@
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import dhda_head
+from ._ins import dhda_head, _signed_smallest_angle, h_head
 from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
 
 
@@ -209,6 +209,137 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
     q[:] = _normalize(q)
 
 
+@njit  # type: ignore[misc]
+def _kalman_gain(
+    P: NDArray[np.float64], h: NDArray[np.float64], r: float
+) -> NDArray[np.float64]:
+    """
+    Compute the Kalman gain for a scalar measurement.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n,)
+        Kalman gain vector.
+    """
+
+    # Innovation covariance (inverse)
+    Ph = np.dot(P, h)
+    s_inv = 1.0 / (np.dot(h, Ph) + r)
+
+    # Kalman gain
+    k = Ph * s_inv
+
+    return k
+
+
+@njit  # type: ignore[misc]
+def _covariance_update(
+    P: NDArray[np.float64],
+    k: NDArray[np.float64],
+    h: NDArray[np.float64],
+    r: float,
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Compute the updated state error covariance matrix estimate (Joseph form).
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    k : ndarray, shape (n,)
+        Kalman gain vector.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    A = I_ - np.outer(k, h)
+    P = A @ P @ A.T + r * np.outer(k, k)
+    return P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_scalar(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: float,
+    r: float,
+    h: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Scalar Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : float
+        Scalar measurement.
+    r : float
+        Scalar measurement noise variance.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+
+    # Kalman gain
+    k = _kalman_gain(P, h, r)
+
+    # Updated (a posteriori) state estimate
+    x[:] += k * (z - np.dot(h, x))
+
+    # Updated (a posteriori) covariance estimate (Joseph form)
+    P[:, :] = _covariance_update(P, k, h, r, I_)
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_sequential(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: NDArray[np.float64],
+    var: NDArray[np.float64],
+    H: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Sequential (one-at-a-time) Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : ndarray, shape (m,)
+        Measurement vector.
+    var : ndarray, shape (m,)
+        Measurement noise variances corresponding to each scalar measurement.
+    H : ndarray, shape (m, n)
+        Measurement matrix where each row corresponds to a scalar measurement model.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    m = z.shape[0]
+    for i in range(m):
+        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
+
+
 class VRU:
     """
     Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF).
@@ -275,12 +406,7 @@ def __init__(
         # Discrete state-space model
         self._phi = _state_transition(self._dt, self._w_b, self._gbc)
         self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
-        self._dhdx = _measurement_matrix(self._q_nb, self._vg_b)
-
-    @property
-    def _vg_b(self):
-        """Gravity reference vector (unit vector) expressed in the body frame."""
-        return _vg_b(self._q_nb, self._nz2vg)
+        self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg))
 
     def quaternion(self) -> NDArray[np.float64]:
         """
@@ -332,3 +458,42 @@ def _reset(self) -> None:
         _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3])
         self._bg_b[:] += self._dx[3:6]
         self._dx[:] = 0.0
+
+    def _aiding_update_gref(
+        self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
+    ) -> None:
+        """
+        Update with gravity reference vector aiding measurement.
+        """
+
+        if vg_meas is None:
+            return None
+
+        if vg_var is None:
+            raise ValueError("'vg_var' not provided.")
+
+        vg_b = _vg_b(self._q_nb, self._nz2vg)
+        dz = vg_meas - vg_b
+        dhdx = self._dhdx_gref(vg_b)
+        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
+
+    def _aiding_update_yaw(
+        self, yaw_meas: float | None, yaw_var: float | None, yaw_degrees: bool
+    ) -> None:
+        """
+        Update with heading aiding measurement.
+        """
+
+        if yaw_meas is None:
+            return None
+
+        if yaw_var is None:
+            raise ValueError("'yaw_var' not provided.")
+
+        if yaw_degrees:
+            yaw_meas = (np.pi / 180.0) * yaw_meas
+            yaw_var = (np.pi / 180.0) ** 2 * yaw_var
+
+        dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb))
+        dhdx = self._dhdx_yaw(self._q_nb)
+        _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I)

From a70983c93e67ce37bb08c63c394e275586b6d28e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:21:12 +0100
Subject: [PATCH 005/217] project ahead

---
 src/smsfusion/_v2.py | 40 +++++++++++++++++++++++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 89ac2619..9182de74 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -3,6 +3,7 @@
 from numpy.typing import ArrayLike, NDArray
 
 from ._ins import dhda_head, _signed_smallest_angle, h_head
+from ._transforms import _angular_matrix_from_quaternion
 from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
 
 
@@ -340,7 +341,32 @@ def _kalman_update_sequential(
         _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
 
 
-class VRU:
+@njit  # type: ignore[misc]
+def _project_cov_ahead(
+    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
+) -> None:
+    """
+    Project the error covariance matrix estimate ahead.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be projected ahead.
+    phi : ndarray, shape (n, n)
+        State transition matrix.
+    Q : ndarray, shape (n, n)
+        Process noise covariance matrix.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Projected error covariance matrix estimate.
+    """
+    P = phi @ P @ phi.T + Q
+    return P
+
+
+class VRUv2:
     """
     Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF).
 
@@ -497,3 +523,15 @@ def _aiding_update_yaw(
         dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb))
         dhdx = self._dhdx_yaw(self._q_nb)
         _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I)
+
+    def _project_ahead(self) -> None:
+        """
+        Project state and covariance estimates ahead.
+        """
+
+        # Attitude (dead reckoning)
+        self._q_nb[:] += self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b
+        self._q_nb[:] = _normalize(self._q_nb)
+
+        # Covariance
+        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From 54a828aebcdd675eeaa5bc9d3982678a8dbf46b1 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:22:35 +0100
Subject: [PATCH 006/217] rename to head

---
 src/smsfusion/_v2.py | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 9182de74..e83528f5 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -503,26 +503,26 @@ def _aiding_update_gref(
         dhdx = self._dhdx_gref(vg_b)
         _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
 
-    def _aiding_update_yaw(
-        self, yaw_meas: float | None, yaw_var: float | None, yaw_degrees: bool
+    def _aiding_update_head(
+        self, head_meas: float | None, head_var: float | None, head_degrees: bool
     ) -> None:
         """
         Update with heading aiding measurement.
         """
 
-        if yaw_meas is None:
+        if head_meas is None:
             return None
 
-        if yaw_var is None:
-            raise ValueError("'yaw_var' not provided.")
+        if head_var is None:
+            raise ValueError("'head_var' not provided.")
 
-        if yaw_degrees:
-            yaw_meas = (np.pi / 180.0) * yaw_meas
-            yaw_var = (np.pi / 180.0) ** 2 * yaw_var
+        if head_degrees:
+            head_meas = (np.pi / 180.0) * head_meas
+            head_var = (np.pi / 180.0) ** 2 * head_var
 
-        dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb))
+        dz = _signed_smallest_angle(head_meas - h_head(self._q_nb))
         dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
 
     def _project_ahead(self) -> None:
         """

From fa2a84a539e6f42415407e17e37b56be0b6f24ac Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:37:22 +0100
Subject: [PATCH 007/217] more code

---
 src/smsfusion/_v2.py | 68 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 68 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index e83528f5..0629c24b 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -1,3 +1,5 @@
+from typing import Self
+
 from numba import njit
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
@@ -535,3 +537,69 @@ def _project_ahead(self) -> None:
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
+
+    def update(
+        self,
+        f: ArrayLike,
+        w: ArrayLike,
+        degrees: bool = False,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = False,
+        g_ref: bool = True,
+        g_var: ArrayLike | None = (0.001, 0.001, 0.001),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        f : array_like, shape (3,)
+            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
+            in m/s^2.
+        w : array_like, shape (3,)
+            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
+            ``degrees`` parameter for units.
+        degrees : bool, optional
+            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
+            rad/s. Defaults to rad/s.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+        g_ref : bool, optional
+            Specifies whether the gravity reference vector is used as an aiding measurement.
+        g_var : array-like, optional
+            Variance of gravitational reference vector measurement noise. Required for
+            ``g_ref``.
+
+        Returns
+        -------
+        MEKF
+            A reference to the instance itself after the update.
+        """
+
+        if degrees:
+            w = np.radians(w)
+
+        # Project (a priori) state and covariance estimates ahead
+        self._project_ahead()
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        self._aiding_update_gref(-_normalize(f) if g_ref else None, g_var)
+        self._aiding_update_head(head, head_var, head_degrees)
+
+        # Reset state
+        self._reset()
+
+        # Update model
+        self._w_b[:] = w - self._bg_b
+        _update_state_transition(self._phi, self._dt, self._w_b)
+
+        return self

From 8a528a41958ba7f870c1b1de42b6da1c83d86409 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 13:40:44 +0100
Subject: [PATCH 008/217] init and style

---
 src/smsfusion/__init__.py |  2 ++
 src/smsfusion/_v2.py      | 15 +++++++++------
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 48ec7ea4..58a51310 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,6 +3,7 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
+from ._v2 import VRUv2
 
 __all__ = [
     "AHRS",
@@ -16,6 +17,7 @@
     "noise",
     "StrapdownINS",
     "VRU",
+    "VRUv2",
     "quaternion_from_euler",
     "ConingScullingAlg",
 ]
diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 0629c24b..17a9e58f 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -1,10 +1,10 @@
 from typing import Self
 
-from numba import njit
 import numpy as np
+from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import dhda_head, _signed_smallest_angle, h_head
+from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion
 from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
 
@@ -171,7 +171,7 @@ def _measurement_matrix(
     """
     dhdx = np.zeros((4, 6))
     dhdx[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity ref vector
-    dhdx[3:4, 0:3] = dhda_head(q_nb)  # heading
+    dhdx[3:4, 0:3] = _dhda_head(q_nb)  # heading
     return dhdx
 
 
@@ -400,6 +400,7 @@ class VRUv2:
         will be expressed relative to this frame.
 
     """
+
     _I: NDArray[np.float64] = np.eye(6)
 
     def __init__(
@@ -472,7 +473,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
         """
         Heading (yaw angle) part of the measurement matrix, shape (6,).
         """
-        self._dhdx[3:4, 0:3] = dhda_head(q_nb)
+        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
         return self._dhdx[3]
 
     def _reset(self) -> None:
@@ -522,7 +523,7 @@ def _aiding_update_head(
             head_meas = (np.pi / 180.0) * head_meas
             head_var = (np.pi / 180.0) ** 2 * head_var
 
-        dz = _signed_smallest_angle(head_meas - h_head(self._q_nb))
+        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
         dhdx = self._dhdx_yaw(self._q_nb)
         _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
 
@@ -532,7 +533,9 @@ def _project_ahead(self) -> None:
         """
 
         # Attitude (dead reckoning)
-        self._q_nb[:] += self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b
+        self._q_nb[:] += (
+            self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b
+        )
         self._q_nb[:] = _normalize(self._q_nb)
 
         # Covariance

From 8d39d4ee1c5bc2d0fa4ce5b974355d4d26bc70fd Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:07:12 +0100
Subject: [PATCH 009/217] remove obsolete import

---
 src/smsfusion/_v2.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 17a9e58f..9cdd6c49 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -6,7 +6,7 @@
 
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion
-from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
+from ._vectorops import _normalize, _skew_symmetric
 
 
 def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:

From a2962bde7a6bada01ca6e0df23e787242a7a37b5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:14:59 +0100
Subject: [PATCH 010/217] delete obsolete function

---
 src/smsfusion/_v2.py | 25 -------------------------
 1 file changed, 25 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 9cdd6c49..bf39ff06 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -9,31 +9,6 @@
 from ._vectorops import _normalize, _skew_symmetric
 
 
-def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
-    """
-    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
-
-    Parameters
-    ----------
-    g : float
-        Gravitational acceleration in m/s^2.
-    nav_frame : {'NED', 'ENU'}
-        Navigation frame in which the gravity vector is expressed.
-
-    Returns
-    -------
-    ndarray, shape (3,)
-        Gravity vector expressed in the navigation frame.
-    """
-    if nav_frame.lower() == "ned":
-        g_n = np.array([0.0, 0.0, g])
-    elif nav_frame.lower() == "enu":
-        g_n = np.array([0.0, 0.0, -g])
-    else:
-        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
-    return g_n
-
-
 def _nz2vg(nav_frame: str) -> float:
     """
     Gravity direction along the navigation frame's z-axis.

From 553b2a211c2b4f1b594f4f78356344d3e70a01e2 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:27:16 +0100
Subject: [PATCH 011/217] euler method

---
 src/smsfusion/_v2.py | 30 ++++++++++++++++++++++++++----
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index bf39ff06..5c40299e 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -5,7 +5,7 @@
 from numpy.typing import ArrayLike, NDArray
 
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _angular_matrix_from_quaternion
+from ._transforms import _angular_matrix_from_quaternion, _euler_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
 
@@ -414,19 +414,41 @@ def __init__(
 
     def quaternion(self) -> NDArray[np.float64]:
         """
-        Return a copy of the attitude quaternion.
+        Attitude expressed as a unit quaternion.
         """
         return self._q_nb.copy()
+    
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
 
     def bias_gyro(self) -> NDArray[np.float64]:
         """
-        Return a copy of the gyroscope bias estimate (rad/s) expressed in the body frame.
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
         """
         return self._bg_b.copy()
 
     def angular_rate(self) -> NDArray[np.float64]:
         """
-        Return a copy of the bias corrected angular rate measurement (rad/s).
+        Bias corrected angular rate measurement (rad/s) expressed in the body frame.
         """
         return self._w_b.copy()
 

From 45275d14c4e9d1d41fc0352dc15ce2dedf0b236e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:37:52 +0100
Subject: [PATCH 012/217] rename to AHRS

---
 src/smsfusion/__init__.py |  4 ++--
 src/smsfusion/_v2.py      | 29 +++++++++++++----------------
 2 files changed, 15 insertions(+), 18 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 58a51310..6dd89429 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,10 +3,11 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import VRUv2
+from ._v2 import AHRSv2
 
 __all__ = [
     "AHRS",
+    "AHRSv2",
     "AidedINS",
     "benchmark",
     "constants",
@@ -17,7 +18,6 @@
     "noise",
     "StrapdownINS",
     "VRU",
-    "VRUv2",
     "quaternion_from_euler",
     "ConingScullingAlg",
 ]
diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 5c40299e..6d8ae286 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -5,7 +5,8 @@
 from numpy.typing import ArrayLike, NDArray
 
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _angular_matrix_from_quaternion, _euler_from_quaternion
+from ._transforms import _angular_matrix_from_quaternion as T
+from ._transforms import _euler_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
 
@@ -24,8 +25,7 @@ def _nz2vg(nav_frame: str) -> float:
 @njit  # type: ignore[misc]
 def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
     """
-    Gravity reference vector expressed in the body frame, computed from the attitude
-    quaternion, q_nb.
+    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
 
     Parameters
     ----------
@@ -158,6 +158,10 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
 
         q = q ⊗ dq(da)
 
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
     Parameters
     ----------
     q : ndarray, shape (4,)
@@ -165,12 +169,6 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
     da : ndarray, shape (3,)
         Small attitude error parameterized as a scaled (2x) Gibbs vector.
 
-    Notes
-    -----
-    As described in ref [1]_, this correction can be simplified by doing it in two
-    steps: first a correction, followed by renormalization. The scaling factor becomes
-    obsolete due to the renormalization step.
-
     References
     ----------
     Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
@@ -343,9 +341,10 @@ def _project_cov_ahead(
     return P
 
 
-class VRUv2:
+class AHRSv2:
     """
-    Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF).
+    Attitude and Heading Reference System (AHRS) using a multiplicative extended
+    Kalman filter (MEKF).
 
     Parameters
     ----------
@@ -417,7 +416,7 @@ def quaternion(self) -> NDArray[np.float64]:
         Attitude expressed as a unit quaternion.
         """
         return self._q_nb.copy()
-    
+
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
         Attitude expressed as Euler angles (roll, pitch, yaw).
@@ -530,9 +529,7 @@ def _project_ahead(self) -> None:
         """
 
         # Attitude (dead reckoning)
-        self._q_nb[:] += (
-            self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b
-        )
+        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
         self._q_nb[:] = _normalize(self._q_nb)
 
         # Covariance
@@ -581,7 +578,7 @@ def update(
 
         Returns
         -------
-        MEKF
+        AHRS
             A reference to the instance itself after the update.
         """
 

From ffcbb233db6cf92a9a51c5ca66c7795adaf6506f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:41:34 +0100
Subject: [PATCH 013/217] gyro bias degrees flag

---
 src/smsfusion/_v2.py | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 6d8ae286..d011fc19 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -439,11 +439,19 @@ def euler(self, degrees: bool = False) -> NDArray[np.float64]:
 
         return theta
 
-    def bias_gyro(self) -> NDArray[np.float64]:
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
         """
         Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
         """
-        return self._bg_b.copy()
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
 
     def angular_rate(self) -> NDArray[np.float64]:
         """

From 45f667b4adee09fb68ebb08c902e2455a525782e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 12 Mar 2026 14:45:26 +0100
Subject: [PATCH 014/217] degrees flag in angular rate

---
 src/smsfusion/_v2.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index d011fc19..bc01b4c7 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -453,11 +453,19 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
             bg_b = (180.0 / np.pi) * bg_b
         return bg_b
 
-    def angular_rate(self) -> NDArray[np.float64]:
+    def angular_rate(self, degrees=False) -> NDArray[np.float64]:
         """
-        Bias corrected angular rate measurement (rad/s) expressed in the body frame.
+        Bias corrected angular rate measurement expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
         """
-        return self._w_b.copy()
+        w_b = self._w_b.copy()
+        if degrees:
+            w_b = (180.0 / np.pi) * w_b
+        return w_b
 
     @property
     def P(self) -> NDArray[np.float64]:

From 4cc61ff3e0b7dba4e07e299a3df9af59d2fb2090 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 08:33:30 +0100
Subject: [PATCH 015/217] v2b

---
 src/smsfusion/__init__.py |   4 +-
 src/smsfusion/_v2.py      |   2 +-
 src/smsfusion/_v2b.py     | 146 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 149 insertions(+), 3 deletions(-)
 create mode 100644 src/smsfusion/_v2b.py

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 6dd89429..0b202de0 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,11 +3,11 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import AHRSv2
+from ._v2 import AHRSv2a
 
 __all__ = [
     "AHRS",
-    "AHRSv2",
+    "AHRSv2a",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index bc01b4c7..f1db0be9 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -341,7 +341,7 @@ def _project_cov_ahead(
     return P
 
 
-class AHRSv2:
+class AHRSv2a:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
     Kalman filter (MEKF).
diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
new file mode 100644
index 00000000..dbce5fe6
--- /dev/null
+++ b/src/smsfusion/_v2b.py
@@ -0,0 +1,146 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from ._ins import _dhda_head, _h_head, _signed_smallest_angle
+from ._transforms import _angular_matrix_from_quaternion as T
+from ._transforms import _euler_from_quaternion
+from ._vectorops import _normalize, _skew_symmetric
+
+
+def _state_transition(
+    dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    f_b : ndarray, shape (3,)
+        Specific force measurement in body frame.
+    w_b : ndarray, shape (3,)
+        Angular rate measurement (bias corrected) in body frame.
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (9, 9)
+        State transition matrix.
+    """
+    phi = np.eye(9)
+    phi[0:3, 0:3] -= dt * _skew_symmetric(w_b)  # NB! update each time step
+    phi[0:3, 3:6] -= dt * np.eye(3)
+    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
+    phi[6:9, 0:3] -= dt * R_nb @ _skew_symmetric(f_b)  # NB! update each time step
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition(
+    phi: NDArray[np.float64],
+    dt: float,
+    f_b: NDArray[np.float64],
+    w_b: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+) -> None:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (9, 9)
+        State transition matrix to be updated in place.
+    dt : float
+        Time step.
+    f_b : ndarray, shape (3,)
+        Specific force measurement in body frame.
+    w_b : ndarray, shape (3,)
+        Angular rate measurement (bias corrected) in body frame.
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    """
+    wx, wy, wz = w_b
+    fx, fy, fz = f_b
+
+    r00, r01, r02 = R_nb[0]
+    r10, r11, r12 = R_nb[1]
+    r20, r21, r22 = R_nb[2]
+
+    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
+    phi[0, 1] = dt * wz
+    phi[0, 2] = -dt * wy
+    phi[1, 0] = -dt * wz
+    phi[1, 2] = dt * wx
+    phi[2, 0] = dt * wy
+    phi[2, 1] = -dt * wx
+
+    # phi[6:9, 0:3] = -dt * R_nb @ S(f_b)
+    phi[6, 0] = -dt * (fz * r01 - fy * r02)
+    phi[7, 0] = -dt * (fz * r11 - fy * r12)
+    phi[8, 0] = -dt * (fz * r21 - fy * r22)
+    phi[6, 1] = -dt * (-fz * r00 + fx * r02)
+    phi[7, 1] = -dt * (-fz * r10 + fx * r12)
+    phi[8, 1] = -dt * (-fz * r20 + fx * r22)
+    phi[6, 2] = -dt * (fy * r00 - fx * r01)
+    phi[7, 2] = -dt * (fy * r10 - fx * r11)
+    phi[8, 2] = -dt * (fy * r20 - fx * r21)
+
+
+def _process_noise_cov(
+    dt: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (6, 6)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((6, 6))
+    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
+    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+def _measurement_matrix(
+    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    vg_b : ndarray, shape (3,)
+        Gravity reference unit vector expressed in the body frame.
+
+    Returns
+    -------
+    ndarray, shape (4, 6)
+        Linearized measurement matrix.
+    """
+    dhdx = np.zeros((4, 6))
+    dhdx[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity ref vector
+    dhdx[3:4, 0:3] = _dhda_head(q_nb)  # heading
+    return dhdx
+

From 49d98f715a85b88b9913372a91a18e5bdbeca394 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 08:54:57 +0100
Subject: [PATCH 016/217] reorder vel first

---
 src/smsfusion/_v2b.py | 69 ++++++++++++++++++++++++-------------------
 1 file changed, 39 insertions(+), 30 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index dbce5fe6..09e527b7 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -10,6 +10,12 @@
 from ._vectorops import _normalize, _skew_symmetric
 
 
+ATT_IDX = slice(0, 3)
+BG_IDX = slice(3, 6)
+VEL_IDX = slice(6, 9)
+
+
+
 def _state_transition(
     dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float
 ) -> NDArray[np.float64]:
@@ -35,10 +41,10 @@ def _state_transition(
         State transition matrix.
     """
     phi = np.eye(9)
-    phi[0:3, 0:3] -= dt * _skew_symmetric(w_b)  # NB! update each time step
-    phi[0:3, 3:6] -= dt * np.eye(3)
-    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
-    phi[6:9, 0:3] -= dt * R_nb @ _skew_symmetric(f_b)  # NB! update each time step
+    phi[VEL_IDX, ATT_IDX] -= dt * R_nb @ _skew_symmetric(f_b)  # NB! update each time step
+    phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b)  # NB! update each time step
+    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
+    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
     return phi
 
 
@@ -73,28 +79,28 @@ def _update_state_transition(
     r10, r11, r12 = R_nb[1]
     r20, r21, r22 = R_nb[2]
 
-    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
-    phi[0, 1] = dt * wz
-    phi[0, 2] = -dt * wy
-    phi[1, 0] = -dt * wz
-    phi[1, 2] = dt * wx
-    phi[2, 0] = dt * wy
-    phi[2, 1] = -dt * wx
-
-    # phi[6:9, 0:3] = -dt * R_nb @ S(f_b)
-    phi[6, 0] = -dt * (fz * r01 - fy * r02)
-    phi[7, 0] = -dt * (fz * r11 - fy * r12)
-    phi[8, 0] = -dt * (fz * r21 - fy * r22)
-    phi[6, 1] = -dt * (-fz * r00 + fx * r02)
-    phi[7, 1] = -dt * (-fz * r10 + fx * r12)
-    phi[8, 1] = -dt * (-fz * r20 + fx * r22)
-    phi[6, 2] = -dt * (fy * r00 - fx * r01)
-    phi[7, 2] = -dt * (fy * r10 - fx * r11)
-    phi[8, 2] = -dt * (fy * r20 - fx * r21)
+    # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b)
+    phi[3, 4] = dt * wz
+    phi[3, 5] = -dt * wy
+    phi[4, 3] = -dt * wz
+    phi[4, 5] = dt * wx
+    phi[5, 3] = dt * wy
+    phi[5, 4] = -dt * wx
+
+    # phi[0:3, 3:6] = -dt * R_nb @ S(f_b)
+    phi[0, 3] = -dt * (fz * r01 - fy * r02)
+    phi[1, 3] = -dt * (fz * r11 - fy * r12)
+    phi[2, 3] = -dt * (fz * r21 - fy * r22)
+    phi[0, 4] = -dt * (-fz * r00 + fx * r02)
+    phi[1, 4] = -dt * (-fz * r10 + fx * r12)
+    phi[2, 4] = -dt * (-fz * r20 + fx * r22)
+    phi[0, 5] = -dt * (fy * r00 - fx * r01)
+    phi[1, 5] = -dt * (fy * r10 - fx * r11)
+    phi[2, 5] = -dt * (fy * r20 - fx * r21)
 
 
 def _process_noise_cov(
-    dt: float, arw: float, gbs: float, gbc: float
+    dt: float, vrw: float, arw: float, gbs: float, gbc: float
 ) -> NDArray[np.float64]:
     """
     Process noise covariance matrix.
@@ -103,6 +109,8 @@ def _process_noise_cov(
     ----------
     dt : float
         Time step in seconds.
+    vrw : float
+        Velocity random walk (accelerometer noise density) in m/s/√Hz.
     arw : float
         Angular random walk (gyroscope noise density) in rad/√Hz.
     gbs : float
@@ -112,12 +120,13 @@ def _process_noise_cov(
 
     Returns
     -------
-    Q : ndarray, shape (6, 6)
+    Q : ndarray, shape (9, 9)
         Process noise covariance matrix.
     """
-    Q = np.zeros((6, 6))
-    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
-    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    Q = np.zeros((9, 9))
+    Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3)
+    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
+    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
     return Q
 
 
@@ -139,8 +148,8 @@ def _measurement_matrix(
     ndarray, shape (4, 6)
         Linearized measurement matrix.
     """
-    dhdx = np.zeros((4, 6))
-    dhdx[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity ref vector
-    dhdx[3:4, 0:3] = _dhda_head(q_nb)  # heading
+    dhdx = np.zeros((4, 9))
+    dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b)  # gravity ref vector
+    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
     return dhdx
 

From 77e1cdd844a28e6ffb9b94a283bc1e1d714cdd40 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:41:35 +0100
Subject: [PATCH 017/217] some changes

---
 src/smsfusion/_v2b.py | 337 +++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 330 insertions(+), 7 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 09e527b7..fb7303a5 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -6,9 +6,10 @@
 
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion as T
-from ._transforms import _euler_from_quaternion
+from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
+from ._v2 import _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, _correct_quat_with_gibbs2
 
 ATT_IDX = slice(0, 3)
 BG_IDX = slice(3, 6)
@@ -130,9 +131,7 @@ def _process_noise_cov(
     return Q
 
 
-def _measurement_matrix(
-    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
-) -> NDArray[np.float64]:
+def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
     """
     Measurement matrix.
 
@@ -140,8 +139,6 @@ def _measurement_matrix(
     ----------
     q_nb : ndarray, shape (4,)
         Unit quaternion.
-    vg_b : ndarray, shape (3,)
-        Gravity reference unit vector expressed in the body frame.
 
     Returns
     -------
@@ -149,7 +146,333 @@ def _measurement_matrix(
         Linearized measurement matrix.
     """
     dhdx = np.zeros((4, 9))
-    dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b)  # gravity ref vector
+    dhdx[0:3, VEL_IDX] = np.eye(3)  # velocity
     dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
     return dhdx
 
+
+class AHRSv2b:
+    """
+    Attitude and Heading Reference System (AHRS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    v_n : array_like, shape (3,), optional
+        Initial velocity estimate in m/s.
+    q_nb : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg_b : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    w_b : array_like, shape (3,), optional
+        Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame.
+        Defaults to zero angular rate (stationary).
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix, **P**. If not
+        given, a small diagonal matrix will be used.
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to
+        0.0007 (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    g : float, default 9.80665
+        The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665
+        m/s^2.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+
+    _I: NDArray[np.float64] = np.eye(9)
+
+    def __init__(
+        self,
+        fs: float,
+        v_n: ArrayLike = (0.0, 0.0, 0.0),
+        a_n: ArrayLike = (0.0, 0.0, 0.0),
+        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg_b: ArrayLike = (0.0, 0.0, 0.0),
+        w_b: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = 1e-6 * np.eye(6),
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.0001,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._g = g
+        self._nav_frame = nav_frame.lower()
+
+        if self._nav_frame == "ned":
+            self._g_n = np.array([0.0, 0.0, g])
+        elif self._nav_frame == "enu":
+            self._g_n = np.array([0.0, 0.0, -g])
+        else:
+            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
+        self._a_n = np.asarray_chkfinite(a_n).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
+        self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
+        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
+        self._f_b = self._R_nb.T @ (self._a_n - self._g_n)
+        self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
+        self._dx = np.zeros(6)
+
+        # Discrete state-space model
+        self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc)
+        self._Q = _process_noise_cov(self._dt, self._vrw, self._arw, self._gbs, self._gbc)
+        self._dhdx = _measurement_matrix(self._q_nb)
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    def angular_rate(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Bias corrected angular rate measurement expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
+        """
+        w_b = self._w_b.copy()
+        if degrees:
+            w_b = (180.0 / np.pi) * w_b
+        return w_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    # def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
+    #     """
+    #     Gravity reference vector part of the measurement matrix, shape (3, 6).
+    #     """
+    #     self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
+    #     return self._dhdx[0:3]
+
+    def _dhdx_vel(self) -> NDArray[np.float64]:
+        """
+        Velocity part of the measurement matrix, shape (3, 6).
+        """
+        return self._dhdx[0:3]
+
+    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Heading (yaw angle) part of the measurement matrix, shape (6,).
+        """
+        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
+        return self._dhdx[3]
+
+    def _reset(self) -> None:
+        """
+        Reset state.
+        """
+
+        if not self._dx.any():
+            return
+
+        _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
+        self._v_n[:] += self._dx[VEL_IDX]
+        self._bg_b[:] += self._dx[BG_IDX]
+        self._dx[:] = 0.0
+
+    # def _aiding_update_gref(
+    #     self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
+    # ) -> None:
+    #     """
+    #     Update with gravity reference vector aiding measurement.
+    #     """
+
+    #     if vg_meas is None:
+    #         return None
+
+    #     if vg_var is None:
+    #         raise ValueError("'vg_var' not provided.")
+
+    #     vg_b = _vg_b(self._q_nb, self._nz2vg)
+    #     dz = vg_meas - vg_b
+    #     dhdx = self._dhdx_gref(vg_b)
+    #     _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
+
+    def _aiding_update_vel(
+        self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
+    ) -> None:
+        """
+        Update with velocity aiding measurement.
+        """
+
+        if vel_meas is None:
+            return None
+
+        if vel_var is None:
+            raise ValueError("'vg_var' not provided.")
+
+        dz = vel_meas - self._v_n
+        dhdx = self._dhdx_vel()
+        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
+
+    def _aiding_update_head(
+        self, head_meas: float | None, head_var: float | None, head_degrees: bool
+    ) -> None:
+        """
+        Update with heading aiding measurement.
+        """
+
+        if head_meas is None:
+            return None
+
+        if head_var is None:
+            raise ValueError("'head_var' not provided.")
+
+        if head_degrees:
+            head_meas = (np.pi / 180.0) * head_meas
+            head_var = (np.pi / 180.0) ** 2 * head_var
+
+        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
+        dhdx = self._dhdx_yaw(self._q_nb)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
+
+    def _project_ahead(self) -> None:
+        """
+        Project state and covariance estimates ahead.
+        """
+
+        # Velocity (dead reckoning)
+        self._v_n[:] += self._dt * self._a_n
+
+        # Attitude (dead reckoning)
+        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
+        self._q_nb[:] = _normalize(self._q_nb)
+
+        # Covariance
+        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
+
+    def update(
+        self,
+        f: ArrayLike,
+        w: ArrayLike,
+        degrees: bool = False,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = False,
+        vel: ArrayLike | None = (0.0, 0.0, 0.0),
+        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        f : array_like, shape (3,)
+            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
+            in m/s^2.
+        w : array_like, shape (3,)
+            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
+            ``degrees`` parameter for units.
+        degrees : bool, optional
+            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
+            rad/s. Defaults to rad/s.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+
+        if degrees:
+            w = np.radians(w)
+
+        # Project (a priori) state and covariance estimates ahead
+        self._project_ahead()
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        self._aiding_update_vel(vel, vel_var)
+        self._aiding_update_head(head, head_var, head_degrees)
+
+        # Reset state
+        self._reset()
+
+        # Update model
+        self._f_b[:] = f
+        self._w_b[:] = w - self._bg_b
+        self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
+        self._a_n[:] = self._R_nb @ self._f_b + self._g_n
+        _update_state_transition(self._phi, self._f_b, self._w_b, self._R_nb)
+
+        return self

From ca6b57c05f49121194492b627f1292cb8867d110 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:43:06 +0100
Subject: [PATCH 018/217] style

---
 src/smsfusion/_v2b.py | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index fb7303a5..0924aa34 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -7,18 +7,25 @@
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion as T
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from ._v2 import (
+    _correct_quat_with_gibbs2,
+    _kalman_update_scalar,
+    _kalman_update_sequential,
+    _project_cov_ahead,
+)
 from ._vectorops import _normalize, _skew_symmetric
 
-from ._v2 import _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, _correct_quat_with_gibbs2
-
 ATT_IDX = slice(0, 3)
 BG_IDX = slice(3, 6)
 VEL_IDX = slice(6, 9)
 
 
-
 def _state_transition(
-    dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float
+    dt: float,
+    f_b: NDArray[np.float64],
+    w_b: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    gbc: float,
 ) -> NDArray[np.float64]:
     """
     State transition matrix.
@@ -42,7 +49,9 @@ def _state_transition(
         State transition matrix.
     """
     phi = np.eye(9)
-    phi[VEL_IDX, ATT_IDX] -= dt * R_nb @ _skew_symmetric(f_b)  # NB! update each time step
+    phi[VEL_IDX, ATT_IDX] -= (
+        dt * R_nb @ _skew_symmetric(f_b)
+    )  # NB! update each time step
     phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b)  # NB! update each time step
     phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
     phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
@@ -243,7 +252,9 @@ def __init__(
 
         # Discrete state-space model
         self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc)
-        self._Q = _process_noise_cov(self._dt, self._vrw, self._arw, self._gbs, self._gbc)
+        self._Q = _process_noise_cov(
+            self._dt, self._vrw, self._arw, self._gbs, self._gbc
+        )
         self._dhdx = _measurement_matrix(self._q_nb)
 
     def quaternion(self) -> NDArray[np.float64]:

From a23dd45946cc23ab8dacd59a1095ca924debaf46 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:45:44 +0100
Subject: [PATCH 019/217] some fixces

---
 src/smsfusion/__init__.py | 2 ++
 src/smsfusion/_v2b.py     | 4 +++-
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 0b202de0..978dfdc1 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -4,10 +4,12 @@
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
 from ._v2 import AHRSv2a
+from ._v2b import AHRSv2b
 
 __all__ = [
     "AHRS",
     "AHRSv2a",
+    "AHRSv2b",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 0924aa34..8f7bcc3c 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -251,7 +251,9 @@ def __init__(
         self._dx = np.zeros(6)
 
         # Discrete state-space model
-        self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc)
+        self._phi = _state_transition(
+            self._dt, self._f_b, self._w_b, self._R_nb, self._gbc
+        )
         self._Q = _process_noise_cov(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
         )

From b9f1fee315f1c93a3286a4800ba47dad3ddbac15 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:46:57 +0100
Subject: [PATCH 020/217] more fixes

---
 src/smsfusion/_v2b.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 8f7bcc3c..10cb9c8f 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -212,7 +212,7 @@ def __init__(
         q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg_b: ArrayLike = (0.0, 0.0, 0.0),
         w_b: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(6),
+        P: ArrayLike = 1e-6 * np.eye(9),
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
         gyro_bias_stability: float = 0.00005,
@@ -247,8 +247,8 @@ def __init__(
         self._f_b = self._R_nb.T @ (self._a_n - self._g_n)
         self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy()
         self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
-        self._dx = np.zeros(6)
+        self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
+        self._dx = np.zeros(9)
 
         # Discrete state-space model
         self._phi = _state_transition(

From 784c23e31bf4a90a448ec36cd8dd19f16ebc4c9b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:47:43 +0100
Subject: [PATCH 021/217] another fix

---
 src/smsfusion/_v2b.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 10cb9c8f..880561bf 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -388,7 +388,7 @@ def _aiding_update_vel(
 
         dz = vel_meas - self._v_n
         dhdx = self._dhdx_vel()
-        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
+        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I)
 
     def _aiding_update_head(
         self, head_meas: float | None, head_var: float | None, head_degrees: bool

From 9adf6dfaf1f8e74dfe40126fba32a58f87804a03 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 09:48:56 +0100
Subject: [PATCH 022/217] small fix

---
 src/smsfusion/_v2b.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 880561bf..40e53811 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -486,6 +486,6 @@ def update(
         self._w_b[:] = w - self._bg_b
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
         self._a_n[:] = self._R_nb @ self._f_b + self._g_n
-        _update_state_transition(self._phi, self._f_b, self._w_b, self._R_nb)
+        _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb)
 
         return self

From 36df90a375dcd253cc490ce18544767373981772 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:02:44 +0100
Subject: [PATCH 023/217] bugfix

---
 src/smsfusion/_v2b.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 40e53811..59050b1d 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -15,9 +15,10 @@
 )
 from ._vectorops import _normalize, _skew_symmetric
 
-ATT_IDX = slice(0, 3)
-BG_IDX = slice(3, 6)
-VEL_IDX = slice(6, 9)
+
+VEL_IDX = slice(0, 3)
+ATT_IDX = slice(3, 6)
+BG_IDX = slice(6, 9)
 
 
 def _state_transition(
@@ -339,7 +340,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
         """
         Heading (yaw angle) part of the measurement matrix, shape (6,).
         """
-        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
+        self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)
         return self._dhdx[3]
 
     def _reset(self) -> None:

From 0bee53b4467f6cb19c6c4af3979f419daa313120 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:03:41 +0100
Subject: [PATCH 024/217] delete commented out code

---
 src/smsfusion/_v2b.py | 25 -------------------------
 1 file changed, 25 deletions(-)

diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 59050b1d..8bb43991 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -323,13 +323,6 @@ def P(self) -> NDArray[np.float64]:
         """
         return self._P.copy()
 
-    # def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
-    #     """
-    #     Gravity reference vector part of the measurement matrix, shape (3, 6).
-    #     """
-    #     self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
-    #     return self._dhdx[0:3]
-
     def _dhdx_vel(self) -> NDArray[np.float64]:
         """
         Velocity part of the measurement matrix, shape (3, 6).
@@ -356,24 +349,6 @@ def _reset(self) -> None:
         self._bg_b[:] += self._dx[BG_IDX]
         self._dx[:] = 0.0
 
-    # def _aiding_update_gref(
-    #     self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
-    # ) -> None:
-    #     """
-    #     Update with gravity reference vector aiding measurement.
-    #     """
-
-    #     if vg_meas is None:
-    #         return None
-
-    #     if vg_var is None:
-    #         raise ValueError("'vg_var' not provided.")
-
-    #     vg_b = _vg_b(self._q_nb, self._nz2vg)
-    #     dz = vg_meas - vg_b
-    #     dhdx = self._dhdx_gref(vg_b)
-    #     _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
-
     def _aiding_update_vel(
         self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
     ) -> None:

From 347a3aeb7f4866d5f8e897659b9cc62a312100e6 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:23:00 +0100
Subject: [PATCH 025/217] common module

---
 src/smsfusion/_v2.py       | 226 +-----------------------------------
 src/smsfusion/_v2b.py      |   2 +-
 src/smsfusion/_v2common.py | 230 +++++++++++++++++++++++++++++++++++++
 3 files changed, 232 insertions(+), 226 deletions(-)
 create mode 100644 src/smsfusion/_v2common.py

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index f1db0be9..427999fe 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -8,40 +8,7 @@
 from ._transforms import _angular_matrix_from_quaternion as T
 from ._transforms import _euler_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
-
-
-def _nz2vg(nav_frame: str) -> float:
-    """
-    Gravity direction along the navigation frame's z-axis.
-    """
-    if nav_frame == "ned":
-        return 1.0
-    elif nav_frame == "enu":
-        return -1.0
-    else:
-        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
-
-
-@njit  # type: ignore[misc]
-def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
-    """
-    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
-
-    Parameters
-    ----------
-    q_nb : numpy.ndarray, shape (4,)
-        Unit quaternion.
-    nz2vg : float
-        Gravity direction along the navigation frame's z-axis. Should be +1 for
-        NED and -1 for ENU.
-    """
-    qw, qx, qy, qz = q_nb
-
-    x = 2.0 * (qx * qz - qw * qy)
-    y = 2.0 * (qy * qz + qw * qx)
-    z = 1.0 - 2.0 * (qx**2 + qy**2)
-
-    return nz2vg * np.array([x, y, z])
+from ._v2common import _nz2vg, _vg_b, _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead
 
 
 def _state_transition(
@@ -150,197 +117,6 @@ def _measurement_matrix(
     return dhdx
 
 
-@njit  # type: ignore[misc]
-def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
-    """
-    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector:
-
-        q = q ⊗ dq(da)
-
-    As described in ref [1]_, this correction can be simplified by doing it in two
-    steps: first a correction, followed by renormalization. The scaling factor becomes
-    obsolete due to the renormalization step.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion [qw, qx, qy, qz] (modified in place).
-    da : ndarray, shape (3,)
-        Small attitude error parameterized as a scaled (2x) Gibbs vector.
-
-    References
-    ----------
-    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
-    and Control, Eq. (6.27)-(6.28).
-    """
-
-    qw, qx, qy, qz = q
-    dax, day, daz = da
-
-    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
-    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
-    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
-    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _kalman_gain(
-    P: NDArray[np.float64], h: NDArray[np.float64], r: float
-) -> NDArray[np.float64]:
-    """
-    Compute the Kalman gain for a scalar measurement.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n,)
-        Kalman gain vector.
-    """
-
-    # Innovation covariance (inverse)
-    Ph = np.dot(P, h)
-    s_inv = 1.0 / (np.dot(h, Ph) + r)
-
-    # Kalman gain
-    k = Ph * s_inv
-
-    return k
-
-
-@njit  # type: ignore[misc]
-def _covariance_update(
-    P: NDArray[np.float64],
-    k: NDArray[np.float64],
-    h: NDArray[np.float64],
-    r: float,
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Compute the updated state error covariance matrix estimate (Joseph form).
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    k : ndarray, shape (n,)
-        Kalman gain vector.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-    A = I_ - np.outer(k, h)
-    P = A @ P @ A.T + r * np.outer(k, k)
-    return P
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_scalar(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: float,
-    r: float,
-    h: NDArray[np.float64],
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Scalar Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    z : float
-        Scalar measurement.
-    r : float
-        Scalar measurement noise variance.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-
-    # Kalman gain
-    k = _kalman_gain(P, h, r)
-
-    # Updated (a posteriori) state estimate
-    x[:] += k * (z - np.dot(h, x))
-
-    # Updated (a posteriori) covariance estimate (Joseph form)
-    P[:, :] = _covariance_update(P, k, h, r, I_)
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_sequential(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: NDArray[np.float64],
-    var: NDArray[np.float64],
-    H: NDArray[np.float64],
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Sequential (one-at-a-time) Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    z : ndarray, shape (m,)
-        Measurement vector.
-    var : ndarray, shape (m,)
-        Measurement noise variances corresponding to each scalar measurement.
-    H : ndarray, shape (m, n)
-        Measurement matrix where each row corresponds to a scalar measurement model.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-    m = z.shape[0]
-    for i in range(m):
-        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
-
-
-@njit  # type: ignore[misc]
-def _project_cov_ahead(
-    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
-) -> None:
-    """
-    Project the error covariance matrix estimate ahead.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be projected ahead.
-    phi : ndarray, shape (n, n)
-        State transition matrix.
-    Q : ndarray, shape (n, n)
-        Process noise covariance matrix.
-
-    Returns
-    -------
-    ndarray, shape (n, n)
-        Projected error covariance matrix estimate.
-    """
-    P = phi @ P @ phi.T + Q
-    return P
-
-
 class AHRSv2a:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index 8bb43991..b7e9c7e8 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -7,7 +7,7 @@
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion as T
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from ._v2 import (
+from ._v2common import (
     _correct_quat_with_gibbs2,
     _kalman_update_scalar,
     _kalman_update_sequential,
diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py
new file mode 100644
index 00000000..ebd01a02
--- /dev/null
+++ b/src/smsfusion/_v2common.py
@@ -0,0 +1,230 @@
+import numpy as np
+from numba import njit
+from numpy.typing import NDArray
+
+from ._vectorops import _normalize
+
+
+def _nz2vg(nav_frame: str) -> float:
+    """
+    Gravity direction along the navigation frame's z-axis.
+    """
+    if nav_frame == "ned":
+        return 1.0
+    elif nav_frame == "enu":
+        return -1.0
+    else:
+        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+
+@njit  # type: ignore[misc]
+def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
+    """
+    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
+
+    Parameters
+    ----------
+    q_nb : numpy.ndarray, shape (4,)
+        Unit quaternion.
+    nz2vg : float
+        Gravity direction along the navigation frame's z-axis. Should be +1 for
+        NED and -1 for ENU.
+    """
+    qw, qx, qy, qz = q_nb
+
+    x = 2.0 * (qx * qz - qw * qy)
+    y = 2.0 * (qy * qz + qw * qx)
+    z = 1.0 - 2.0 * (qx**2 + qy**2)
+
+    return nz2vg * np.array([x, y, z])
+
+
+@njit  # type: ignore[misc]
+def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
+    as a scaled (2x) Gibbs vector:
+
+        q = q ⊗ dq(da)
+
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion [qw, qx, qy, qz] (modified in place).
+    da : ndarray, shape (3,)
+        Small attitude error parameterized as a scaled (2x) Gibbs vector.
+
+    References
+    ----------
+    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    and Control, Eq. (6.27)-(6.28).
+    """
+
+    qw, qx, qy, qz = q
+    dax, day, daz = da
+
+    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
+    q[:] = _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _kalman_gain(
+    P: NDArray[np.float64], h: NDArray[np.float64], r: float
+) -> NDArray[np.float64]:
+    """
+    Compute the Kalman gain for a scalar measurement.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n,)
+        Kalman gain vector.
+    """
+
+    # Innovation covariance (inverse)
+    Ph = np.dot(P, h)
+    s_inv = 1.0 / (np.dot(h, Ph) + r)
+
+    # Kalman gain
+    k = Ph * s_inv
+
+    return k
+
+
+@njit  # type: ignore[misc]
+def _covariance_update(
+    P: NDArray[np.float64],
+    k: NDArray[np.float64],
+    h: NDArray[np.float64],
+    r: float,
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Compute the updated state error covariance matrix estimate (Joseph form).
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    k : ndarray, shape (n,)
+        Kalman gain vector.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    A = I_ - np.outer(k, h)
+    P = A @ P @ A.T + r * np.outer(k, k)
+    return P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_scalar(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: float,
+    r: float,
+    h: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Scalar Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : float
+        Scalar measurement.
+    r : float
+        Scalar measurement noise variance.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+
+    # Kalman gain
+    k = _kalman_gain(P, h, r)
+
+    # Updated (a posteriori) state estimate
+    x[:] += k * (z - np.dot(h, x))
+
+    # Updated (a posteriori) covariance estimate (Joseph form)
+    P[:, :] = _covariance_update(P, k, h, r, I_)
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_sequential(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: NDArray[np.float64],
+    var: NDArray[np.float64],
+    H: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Sequential (one-at-a-time) Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : ndarray, shape (m,)
+        Measurement vector.
+    var : ndarray, shape (m,)
+        Measurement noise variances corresponding to each scalar measurement.
+    H : ndarray, shape (m, n)
+        Measurement matrix where each row corresponds to a scalar measurement model.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    m = z.shape[0]
+    for i in range(m):
+        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
+
+
+@njit  # type: ignore[misc]
+def _project_cov_ahead(
+    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
+) -> None:
+    """
+    Project the error covariance matrix estimate ahead.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be projected ahead.
+    phi : ndarray, shape (n, n)
+        State transition matrix.
+    Q : ndarray, shape (n, n)
+        Process noise covariance matrix.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Projected error covariance matrix estimate.
+    """
+    P = phi @ P @ phi.T + Q
+    return P

From cf5b82060044f3e8bed011b730f7d3310461820b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:33:48 +0100
Subject: [PATCH 026/217] v2c

---
 src/smsfusion/_v2c.py | 462 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 462 insertions(+)
 create mode 100644 src/smsfusion/_v2c.py

diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py
new file mode 100644
index 00000000..b5477532
--- /dev/null
+++ b/src/smsfusion/_v2c.py
@@ -0,0 +1,462 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from ._ins import _dhda_head, _h_head, _signed_smallest_angle
+from ._transforms import _angular_matrix_from_quaternion as T
+from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from ._v2common import (
+    _correct_quat_with_gibbs2,
+    _kalman_update_scalar,
+    _kalman_update_sequential,
+    _project_cov_ahead,
+)
+from ._vectorops import _normalize, _skew_symmetric
+
+
+VEL_IDX = slice(0, 3)
+ATT_IDX = slice(3, 6)
+BG_IDX = slice(6, 9)
+
+
+def _state_transition(
+    dt: float,
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    dvel : ndarray, shape (3,)
+        Velocity change vector (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude change vector (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (9, 9)
+        State transition matrix.
+    """
+    phi = np.eye(9)
+    phi[VEL_IDX, ATT_IDX] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
+    phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
+    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition(
+    phi: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+) -> None:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (9, 9)
+        State transition matrix to be updated in place.
+    dvel : ndarray, shape (3,)
+        Velocity change vector (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude change vector (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    """
+    dtx, dty, dtz = dtheta
+    dvx, dvy, dvz = dvel
+
+    r00, r01, r02 = R_nb[0]
+    r10, r11, r12 = R_nb[1]
+    r20, r21, r22 = R_nb[2]
+
+    # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b)
+    phi[3, 4] = dtz
+    phi[3, 5] = -dty
+    phi[4, 3] = -dtz
+    phi[4, 5] = dtx
+    phi[5, 3] = dty
+    phi[5, 4] = -dtx
+
+    # phi[0:3, 3:6] = -dt * R_nb @ S(f_b)
+    phi[0, 3] = -(dvz * r01 - dvy * r02)
+    phi[1, 3] = -(dvz * r11 - dvy * r12)
+    phi[2, 3] = -(dvz * r21 - dvy * r22)
+    phi[0, 4] = -(-dvz * r00 + dvx * r02)
+    phi[1, 4] = -(-dvz * r10 + dvx * r12)
+    phi[2, 4] = -(-dvz * r20 + dvx * r22)
+    phi[0, 5] = -(dvy * r00 - dvx * r01)
+    phi[1, 5] = -(dvy * r10 - dvx * r11)
+    phi[2, 5] = -(dvy * r20 - dvx * r21)
+
+
+def _process_noise_cov(
+    dt: float, vrw: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    vrw : float
+        Velocity random walk (accelerometer noise density) in m/s/√Hz.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (9, 9)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((9, 9))
+    Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3)
+    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
+    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+
+    Returns
+    -------
+    ndarray, shape (4, 6)
+        Linearized measurement matrix.
+    """
+    dhdx = np.zeros((4, 9))
+    dhdx[0:3, VEL_IDX] = np.eye(3)  # velocity
+    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
+    return dhdx
+
+
+class AHRSv2c:
+    """
+    Attitude and Heading Reference System (AHRS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    v_n : array_like, shape (3,), optional
+        Initial velocity estimate in m/s.
+    q_nb : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg_b : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    dvel : array_like, shape (3,), optional
+        Initial velocity change vector (sculling integral).
+    dtheta : array_like, shape (3,), optional
+        Initial attitude change vector (coning integral).
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix, **P**. If not
+        given, a small diagonal matrix will be used.
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to
+        0.0007 (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    g : float, default 9.80665
+        The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665
+        m/s^2.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+
+    _I: NDArray[np.float64] = np.eye(9)
+
+    def __init__(
+        self,
+        fs: float,
+        v_n: ArrayLike = (0.0, 0.0, 0.0),
+        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg_b: ArrayLike = (0.0, 0.0, 0.0),
+        dvel: ArrayLike = (0.0, 0.0, 0.0),
+        dtheta: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = 1e-6 * np.eye(9),
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.0001,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._g = g
+        self._nav_frame = nav_frame.lower()
+
+        if self._nav_frame == "ned":
+            self._g_n = np.array([0.0, 0.0, g])
+        elif self._nav_frame == "enu":
+            self._g_n = np.array([0.0, 0.0, -g])
+        else:
+            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
+        self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
+        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
+        self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy()
+        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
+        self._dx = np.zeros(9)
+
+        # Discrete state-space model
+        self._phi = _state_transition(
+            self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc
+        )
+        self._Q = _process_noise_cov(
+            self._dt, self._vrw, self._arw, self._gbs, self._gbc
+        )
+        self._dhdx = _measurement_matrix(self._q_nb)
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    # def angular_rate(self, degrees=False) -> NDArray[np.float64]:
+    #     """
+    #     Bias corrected angular rate measurement expressed in the body frame.
+
+    #     Parameters
+    #     ----------
+    #     degrees : bool, optional
+    #         Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
+    #     """
+    #     w_b = self._w_b.copy()
+    #     if degrees:
+    #         w_b = (180.0 / np.pi) * w_b
+    #     return w_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    def _dhdx_vel(self) -> NDArray[np.float64]:
+        """
+        Velocity part of the measurement matrix, shape (3, 6).
+        """
+        return self._dhdx[0:3]
+
+    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Heading (yaw angle) part of the measurement matrix, shape (6,).
+        """
+        self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)
+        return self._dhdx[3]
+
+    def _reset(self) -> None:
+        """
+        Reset state.
+        """
+
+        if not self._dx.any():
+            return
+
+        _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
+        self._v_n[:] += self._dx[VEL_IDX]
+        self._bg_b[:] += self._dx[BG_IDX]
+        self._dx[:] = 0.0
+
+    def _aiding_update_vel(
+        self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
+    ) -> None:
+        """
+        Update with velocity aiding measurement.
+        """
+
+        if vel_meas is None:
+            return None
+
+        if vel_var is None:
+            raise ValueError("'vg_var' not provided.")
+
+        dz = vel_meas - self._v_n
+        dhdx = self._dhdx_vel()
+        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I)
+
+    def _aiding_update_head(
+        self, head_meas: float | None, head_var: float | None, head_degrees: bool
+    ) -> None:
+        """
+        Update with heading aiding measurement.
+        """
+
+        if head_meas is None:
+            return None
+
+        if head_var is None:
+            raise ValueError("'head_var' not provided.")
+
+        if head_degrees:
+            head_meas = (np.pi / 180.0) * head_meas
+            head_var = (np.pi / 180.0) ** 2 * head_var
+
+        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
+        dhdx = self._dhdx_yaw(self._q_nb)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
+
+    def _project_ahead(self) -> None:
+        """
+        Project state and covariance estimates ahead.
+        """
+
+        # Velocity (dead reckoning)
+        self._v_n[:] += self._dvel
+
+        # Attitude (dead reckoning)
+        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
+        self._q_nb[:] = _normalize(self._q_nb)
+
+        # Covariance
+        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
+
+    def update(
+        self,
+        f: ArrayLike,
+        w: ArrayLike,
+        degrees: bool = False,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = False,
+        vel: ArrayLike | None = (0.0, 0.0, 0.0),
+        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        f : array_like, shape (3,)
+            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
+            in m/s^2.
+        w : array_like, shape (3,)
+            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
+            ``degrees`` parameter for units.
+        degrees : bool, optional
+            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
+            rad/s. Defaults to rad/s.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+
+        if degrees:
+            w = np.radians(w)
+
+        # Project (a priori) state and covariance estimates ahead
+        self._project_ahead()
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        self._aiding_update_vel(vel, vel_var)
+        self._aiding_update_head(head, head_var, head_degrees)
+
+        # Reset state
+        self._reset()
+
+        # Update model
+        self._f_b[:] = f
+        self._w_b[:] = w - self._bg_b
+        self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
+        self._a_n[:] = self._R_nb @ self._f_b + self._g_n
+        _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb)
+
+        return self

From 111e9addf1f984280c0032d6235cbbec8fc1ca8c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:39:07 +0100
Subject: [PATCH 027/217] more changes to vc

---
 src/smsfusion/_v2c.py      | 31 ++++++++++-----------
 src/smsfusion/_v2common.py | 56 +++++++++++++++++++++++++++++++++++++-
 2 files changed, 69 insertions(+), 18 deletions(-)

diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py
index b5477532..d96fed72 100644
--- a/src/smsfusion/_v2c.py
+++ b/src/smsfusion/_v2c.py
@@ -12,6 +12,7 @@
     _kalman_update_scalar,
     _kalman_update_sequential,
     _project_cov_ahead,
+    _correct_quat_with_rotvec
 )
 from ._vectorops import _normalize, _skew_symmetric
 
@@ -391,16 +392,15 @@ def _project_ahead(self) -> None:
         self._v_n[:] += self._dvel
 
         # Attitude (dead reckoning)
-        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
-        self._q_nb[:] = _normalize(self._q_nb)
+        _correct_quat_with_rotvec(self._q_nb, self._dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
 
     def update(
         self,
-        f: ArrayLike,
-        w: ArrayLike,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
         degrees: bool = False,
         head: float | None = None,
         head_var: float | None = None,
@@ -413,15 +413,13 @@ def update(
 
         Parameters
         ----------
-        f : array_like, shape (3,)
-            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
-            in m/s^2.
-        w : array_like, shape (3,)
-            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
-            ``degrees`` parameter for units.
+        dvel : array_like, shape (3,), optional
+            Initial velocity change vector (sculling integral).
+        dtheta : array_like, shape (3,), optional
+            Initial attitude change vector (coning integral).
         degrees : bool, optional
-            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
-            rad/s. Defaults to rad/s.
+            Specifies whether the unit of the attitude change vector, ``dtheta``,
+            is degrees or radians. Defaults to radians.
         head : float, optional
             Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
             assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
@@ -440,7 +438,7 @@ def update(
         """
 
         if degrees:
-            w = np.radians(w)
+            dtheta = np.radians(dtheta)
 
         # Project (a priori) state and covariance estimates ahead
         self._project_ahead()
@@ -453,10 +451,9 @@ def update(
         self._reset()
 
         # Update model
-        self._f_b[:] = f
-        self._w_b[:] = w - self._bg_b
+        self._dvel[:] = dvel
+        self._dtheta[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        self._a_n[:] = self._R_nb @ self._f_b + self._g_n
-        _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb)
+        _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
 
         return self
diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py
index ebd01a02..e7d6fea5 100644
--- a/src/smsfusion/_v2common.py
+++ b/src/smsfusion/_v2common.py
@@ -2,7 +2,7 @@
 from numba import njit
 from numpy.typing import NDArray
 
-from ._vectorops import _normalize
+from ._vectorops import _normalize, _quaternion_product
 
 
 def _nz2vg(nav_frame: str) -> float:
@@ -74,6 +74,60 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
     q[:] = _normalize(q)
 
 
+@njit  # type: ignore[misc]
+def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Compute the unit quaternion from a rotation vector.
+
+    Parameters
+    ----------
+    r : numpy.ndarray, shape (3,)
+        Rotation vector (rx, ry, rz).
+
+    Returns
+    -------
+    numpy.ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz).
+    """
+    # TODO: add reference
+
+    rx, ry, rz = r
+
+    angle2 = rx**2 + ry**2 + rz**2
+
+    if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
+        a = 0.25 * angle2
+        c = 1.0 - a / 2.0
+        s = 0.5 * (1.0 - a / 6.0)
+    else:
+        angle = np.sqrt(angle2)
+        half_angle = 0.5 * angle
+        c = np.cos(half_angle)
+        s = np.sin(half_angle) / angle
+
+    q = np.array([c, s * rx, s * ry, s * rz])
+
+    return _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _correct_quat_with_rotvec(q: NDArray[np.float64], dtheta: NDArray[np.float64]) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+    parameterized as a rotation vector:
+
+        q = q ⊗ dq(dtheta)
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (modified in place).
+    dtheta : ndarray, shape (3,)
+        Small attitude change parameterized as a rotation vector.
+    """
+    q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
+
+
 @njit  # type: ignore[misc]
 def _kalman_gain(
     P: NDArray[np.float64], h: NDArray[np.float64], r: float

From b2104ced75395e956c211915275aa9675797a290 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:39:29 +0100
Subject: [PATCH 028/217] style

---
 src/smsfusion/_v2.py       | 9 ++++++++-
 src/smsfusion/_v2b.py      | 1 -
 src/smsfusion/_v2c.py      | 3 +--
 src/smsfusion/_v2common.py | 4 +++-
 4 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 427999fe..c402417f 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -7,8 +7,15 @@
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _angular_matrix_from_quaternion as T
 from ._transforms import _euler_from_quaternion
+from ._v2common import (
+    _correct_quat_with_gibbs2,
+    _kalman_update_scalar,
+    _kalman_update_sequential,
+    _nz2vg,
+    _project_cov_ahead,
+    _vg_b,
+)
 from ._vectorops import _normalize, _skew_symmetric
-from ._v2common import _nz2vg, _vg_b, _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead
 
 
 def _state_transition(
diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py
index b7e9c7e8..6352a1e5 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2b.py
@@ -15,7 +15,6 @@
 )
 from ._vectorops import _normalize, _skew_symmetric
 
-
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
 BG_IDX = slice(6, 9)
diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py
index d96fed72..a338ec18 100644
--- a/src/smsfusion/_v2c.py
+++ b/src/smsfusion/_v2c.py
@@ -9,14 +9,13 @@
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from ._v2common import (
     _correct_quat_with_gibbs2,
+    _correct_quat_with_rotvec,
     _kalman_update_scalar,
     _kalman_update_sequential,
     _project_cov_ahead,
-    _correct_quat_with_rotvec
 )
 from ._vectorops import _normalize, _skew_symmetric
 
-
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
 BG_IDX = slice(6, 9)
diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py
index e7d6fea5..7d1e3370 100644
--- a/src/smsfusion/_v2common.py
+++ b/src/smsfusion/_v2common.py
@@ -111,7 +111,9 @@ def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
 
 
 @njit  # type: ignore[misc]
-def _correct_quat_with_rotvec(q: NDArray[np.float64], dtheta: NDArray[np.float64]) -> None:
+def _correct_quat_with_rotvec(
+    q: NDArray[np.float64], dtheta: NDArray[np.float64]
+) -> None:
     """
     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
     parameterized as a rotation vector:

From a55246743a5a21361f7c59743ffbc9ba690d94e6 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:49:53 +0100
Subject: [PATCH 029/217] move to v2 module

---
 src/smsfusion/__init__.py             | 4 ++--
 src/smsfusion/_v2/__init__.py         | 5 +++++
 src/smsfusion/{_v2.py => _v2/_v2a.py} | 8 ++++----
 src/smsfusion/{ => _v2}/_v2b.py       | 8 ++++----
 src/smsfusion/{ => _v2}/_v2c.py       | 7 +++----
 src/smsfusion/{ => _v2}/_v2common.py  | 2 +-
 6 files changed, 19 insertions(+), 15 deletions(-)
 create mode 100644 src/smsfusion/_v2/__init__.py
 rename src/smsfusion/{_v2.py => _v2/_v2a.py} (98%)
 rename src/smsfusion/{ => _v2}/_v2b.py (98%)
 rename src/smsfusion/{ => _v2}/_v2c.py (98%)
 rename src/smsfusion/{ => _v2}/_v2common.py (99%)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 978dfdc1..1c8a6eca 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,13 +3,13 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import AHRSv2a
-from ._v2b import AHRSv2b
+from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c
 
 __all__ = [
     "AHRS",
     "AHRSv2a",
     "AHRSv2b",
+    "AHRSv2c",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py
new file mode 100644
index 00000000..cba3bc67
--- /dev/null
+++ b/src/smsfusion/_v2/__init__.py
@@ -0,0 +1,5 @@
+from ._v2a import AHRSv2a
+from ._v2b import AHRSv2b
+from ._v2c import AHRSv2c
+
+__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c"]
diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2/_v2a.py
similarity index 98%
rename from src/smsfusion/_v2.py
rename to src/smsfusion/_v2/_v2a.py
index c402417f..e04707e9 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2/_v2a.py
@@ -4,9 +4,10 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _angular_matrix_from_quaternion as T
-from ._transforms import _euler_from_quaternion
+from .._ins import _dhda_head, _h_head, _signed_smallest_angle
+from .._transforms import _angular_matrix_from_quaternion as T
+from .._transforms import _euler_from_quaternion
+from .._vectorops import _normalize, _skew_symmetric
 from ._v2common import (
     _correct_quat_with_gibbs2,
     _kalman_update_scalar,
@@ -15,7 +16,6 @@
     _project_cov_ahead,
     _vg_b,
 )
-from ._vectorops import _normalize, _skew_symmetric
 
 
 def _state_transition(
diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2/_v2b.py
similarity index 98%
rename from src/smsfusion/_v2b.py
rename to src/smsfusion/_v2/_v2b.py
index 6352a1e5..15cd71ee 100644
--- a/src/smsfusion/_v2b.py
+++ b/src/smsfusion/_v2/_v2b.py
@@ -4,16 +4,16 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _angular_matrix_from_quaternion as T
-from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from .._ins import _dhda_head, _h_head, _signed_smallest_angle
+from .._transforms import _angular_matrix_from_quaternion as T
+from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from .._vectorops import _normalize, _skew_symmetric
 from ._v2common import (
     _correct_quat_with_gibbs2,
     _kalman_update_scalar,
     _kalman_update_sequential,
     _project_cov_ahead,
 )
-from ._vectorops import _normalize, _skew_symmetric
 
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2/_v2c.py
similarity index 98%
rename from src/smsfusion/_v2c.py
rename to src/smsfusion/_v2/_v2c.py
index a338ec18..d59caf84 100644
--- a/src/smsfusion/_v2c.py
+++ b/src/smsfusion/_v2/_v2c.py
@@ -4,9 +4,9 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _angular_matrix_from_quaternion as T
-from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from .._ins import _dhda_head, _h_head, _signed_smallest_angle
+from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from .._vectorops import _skew_symmetric
 from ._v2common import (
     _correct_quat_with_gibbs2,
     _correct_quat_with_rotvec,
@@ -14,7 +14,6 @@
     _kalman_update_sequential,
     _project_cov_ahead,
 )
-from ._vectorops import _normalize, _skew_symmetric
 
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2/_v2common.py
similarity index 99%
rename from src/smsfusion/_v2common.py
rename to src/smsfusion/_v2/_v2common.py
index 7d1e3370..e2127a03 100644
--- a/src/smsfusion/_v2common.py
+++ b/src/smsfusion/_v2/_v2common.py
@@ -2,7 +2,7 @@
 from numba import njit
 from numpy.typing import NDArray
 
-from ._vectorops import _normalize, _quaternion_product
+from .._vectorops import _normalize, _quaternion_product
 
 
 def _nz2vg(nav_frame: str) -> float:

From 96f41d9739e774b58713c30401d44f432e7647f5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 10:56:44 +0100
Subject: [PATCH 030/217] project ahead fix

---
 src/smsfusion/_v2/_v2c.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py
index d59caf84..9522f9b6 100644
--- a/src/smsfusion/_v2/_v2c.py
+++ b/src/smsfusion/_v2/_v2c.py
@@ -387,7 +387,7 @@ def _project_ahead(self) -> None:
         """
 
         # Velocity (dead reckoning)
-        self._v_n[:] += self._dvel
+        self._v_n[:] += self._R_nb @ self._dvel + self._dt * self._g_n
 
         # Attitude (dead reckoning)
         _correct_quat_with_rotvec(self._q_nb, self._dtheta)

From 3602c38eef3a85ec0ffb2af71e6317222b7a7617 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 11:00:29 +0100
Subject: [PATCH 031/217] subtract gyro bias

---
 src/smsfusion/_v2/_v2c.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py
index 9522f9b6..58fe378a 100644
--- a/src/smsfusion/_v2/_v2c.py
+++ b/src/smsfusion/_v2/_v2c.py
@@ -450,7 +450,7 @@ def update(
 
         # Update model
         self._dvel[:] = dvel
-        self._dtheta[:] = dtheta
+        self._dtheta[:] = dtheta - self._dt * self._bg_b
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
         _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
 

From f928c8f3b64dd117e6366b7082c6d592be469031 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 11:41:15 +0100
Subject: [PATCH 032/217] v2d

---
 src/smsfusion/_v2/_v2d.py | 402 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 402 insertions(+)
 create mode 100644 src/smsfusion/_v2/_v2d.py

diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py
new file mode 100644
index 00000000..cf41dd7c
--- /dev/null
+++ b/src/smsfusion/_v2/_v2d.py
@@ -0,0 +1,402 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from .._ins import _dhda_head, _h_head, _signed_smallest_angle
+from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from .._vectorops import _skew_symmetric
+from .._vectorops import _normalize
+from ._v2common import (
+    _correct_quat_with_gibbs2,
+    _correct_quat_with_rotvec,
+    _kalman_update_scalar,
+    _kalman_update_sequential,
+    _project_cov_ahead,
+    _nz2vg,
+    _vg_b,
+)
+
+ATT_IDX = slice(0, 3)
+BG_IDX = slice(3, 6)
+
+
+def _state_transition(
+    dt: float,
+    dtheta: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    dtheta : ndarray, shape (3,)
+        Attitude change vector (coning integral).
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (6, 6)
+        State transition matrix.
+    """
+    phi = np.eye(6)
+    phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
+    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition(
+    phi: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+) -> None:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (9, 9)
+        State transition matrix to be updated in place.
+    dtheta : ndarray, shape (3,)
+        Attitude change vector (coning integral).
+    """
+    dtx, dty, dtz = dtheta
+
+    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
+    phi[0, 1] = dtz
+    phi[0, 2] = -dty
+    phi[1, 0] = -dtz
+    phi[1, 2] = dtx
+    phi[2, 0] = dty
+    phi[2, 1] = -dtx
+
+
+def _process_noise_cov(
+    dt: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (9, 9)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((6, 6))
+    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
+    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+def _measurement_matrix(
+    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    vg_b : ndarray, shape (3,)
+        Gravity reference unit vector expressed in the body frame.
+
+    Returns
+    -------
+    ndarray, shape (4, 6)
+        Linearized measurement matrix.
+    """
+    dhdx = np.zeros((4, 6))
+    dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b)  # gravity ref vector
+    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
+    return dhdx
+
+
+class AHRSv2a:
+    """
+    Attitude and Heading Reference System (AHRS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    q_nb : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg_b : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    dtheta : array_like, shape (3,), optional
+        Initial attitude change vector (coning integral).
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix, **P**. If not
+        given, a small diagonal matrix will be used.
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+
+    _I: NDArray[np.float64] = np.eye(6)
+
+    def __init__(
+        self,
+        fs: float,
+        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg_b: ArrayLike = (0.0, 0.0, 0.0),
+        dtheta: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = 1e-6 * np.eye(6),
+        gyro_noise_density: float = 0.0001,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+
+        # IMU noise parameters
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
+        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
+        self._dx = np.zeros(6)
+
+        # Discrete state-space model
+        self._phi = _state_transition(self._dt, self._dtheta, self._gbc)
+        self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
+        self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg))
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    # def angular_rate(self, degrees=False) -> NDArray[np.float64]:
+    #     """
+    #     Bias corrected angular rate measurement expressed in the body frame.
+
+    #     Parameters
+    #     ----------
+    #     degrees : bool, optional
+    #         Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
+    #     """
+    #     w_b = self._w_b.copy()
+    #     if degrees:
+    #         w_b = (180.0 / np.pi) * w_b
+    #     return w_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Gravity reference vector part of the measurement matrix, shape (3, 6).
+        """
+        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
+        return self._dhdx[0:3]
+
+    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+        """
+        Heading (yaw angle) part of the measurement matrix, shape (6,).
+        """
+        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
+        return self._dhdx[3]
+
+    def _reset(self) -> None:
+        """
+        Reset state.
+        """
+
+        if not self._dx.any():
+            return
+
+        _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3])
+        self._bg_b[:] += self._dx[3:6]
+        self._dx[:] = 0.0
+
+    def _aiding_update_gref(
+        self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
+    ) -> None:
+        """
+        Update with gravity reference vector aiding measurement.
+        """
+
+        if vg_meas is None:
+            return None
+
+        if vg_var is None:
+            raise ValueError("'vg_var' not provided.")
+
+        vg_b = _vg_b(self._q_nb, self._nz2vg)
+        dz = vg_meas - vg_b
+        dhdx = self._dhdx_gref(vg_b)
+        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
+
+    def _aiding_update_head(
+        self, head_meas: float | None, head_var: float | None, head_degrees: bool
+    ) -> None:
+        """
+        Update with heading aiding measurement.
+        """
+
+        if head_meas is None:
+            return None
+
+        if head_var is None:
+            raise ValueError("'head_var' not provided.")
+
+        if head_degrees:
+            head_meas = (np.pi / 180.0) * head_meas
+            head_var = (np.pi / 180.0) ** 2 * head_var
+
+        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
+        dhdx = self._dhdx_yaw(self._q_nb)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
+
+    def _project_ahead(self) -> None:
+        """
+        Project state and covariance estimates ahead.
+        """
+
+        # Attitude (dead reckoning)
+        _correct_quat_with_rotvec(self._q_nb, self._dtheta)
+
+        # Covariance
+        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
+
+    def update(
+        self,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
+        degrees: bool = False,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = False,
+        g_ref: bool = True,
+        g_var: ArrayLike | None = (0.001, 0.001, 0.001),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        dvel : array_like, shape (3,), optional
+            Initial velocity change vector (sculling integral).
+        dtheta : array_like, shape (3,), optional
+            Initial attitude change vector (coning integral).
+        degrees : bool, optional
+            Specifies whether the unit of the attitude change vector, ``dtheta``,
+            is degrees or radians. Defaults to radians.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+        g_ref : bool, optional
+            Specifies whether the gravity reference vector is used as an aiding measurement.
+        g_var : array-like, optional
+            Variance of gravitational reference vector measurement noise. Required for
+            ``g_ref``.
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+
+        if degrees:
+            dtheta = np.radians(dtheta)
+
+        # Project (a priori) state and covariance estimates ahead
+        self._project_ahead()
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var)
+        self._aiding_update_head(head, head_var, head_degrees)
+
+        # Reset state
+        self._reset()
+
+        # Update model
+        self._dtheta[:] = dtheta - self._dt * self._bg_b
+        _update_state_transition(self._phi, self._dt, self._dtheta)
+
+        return self

From 1cf779f6d77d175cfc5514d97d3f45dc794f8813 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 11:42:42 +0100
Subject: [PATCH 033/217] init

---
 src/smsfusion/__init__.py     | 3 ++-
 src/smsfusion/_v2/__init__.py | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 1c8a6eca..15bb24ae 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,13 +3,14 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c
+from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c, AHRSv2d
 
 __all__ = [
     "AHRS",
     "AHRSv2a",
     "AHRSv2b",
     "AHRSv2c",
+    "AHRSv2d",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py
index cba3bc67..8f8718ff 100644
--- a/src/smsfusion/_v2/__init__.py
+++ b/src/smsfusion/_v2/__init__.py
@@ -1,5 +1,6 @@
 from ._v2a import AHRSv2a
 from ._v2b import AHRSv2b
 from ._v2c import AHRSv2c
+from ._v2d import AHRSv2d
 
-__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c"]
+__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c", "AHRSv2d"]

From 96f6577af69e0de49b81d34afe8901597db226e4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 11:43:15 +0100
Subject: [PATCH 034/217] fix name

---
 src/smsfusion/_v2/_v2d.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py
index cf41dd7c..7acf5856 100644
--- a/src/smsfusion/_v2/_v2d.py
+++ b/src/smsfusion/_v2/_v2d.py
@@ -129,7 +129,7 @@ def _measurement_matrix(
     return dhdx
 
 
-class AHRSv2a:
+class AHRSv2d:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
     Kalman filter (MEKF).

From 7318c5ab7f7cb07b4cd7fb9a7675b11726fe075d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 11:44:20 +0100
Subject: [PATCH 035/217] fix

---
 src/smsfusion/_v2/_v2d.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py
index 7acf5856..92a6ab9b 100644
--- a/src/smsfusion/_v2/_v2d.py
+++ b/src/smsfusion/_v2/_v2d.py
@@ -397,6 +397,6 @@ def update(
 
         # Update model
         self._dtheta[:] = dtheta - self._dt * self._bg_b
-        _update_state_transition(self._phi, self._dt, self._dtheta)
+        _update_state_transition(self._phi, self._dtheta)
 
         return self

From e21c6dd09f1552934a0e1b57f6af3a39e58edd12 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 15:48:18 +0100
Subject: [PATCH 036/217] some fixes

---
 src/smsfusion/_v2/_v2d.py | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py
index 92a6ab9b..eb86eb6a 100644
--- a/src/smsfusion/_v2/_v2d.py
+++ b/src/smsfusion/_v2/_v2d.py
@@ -169,7 +169,6 @@ def __init__(
         fs: float,
         q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        dtheta: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = 1e-6 * np.eye(6),
         gyro_noise_density: float = 0.0001,
         gyro_bias_stability: float = 0.00005,
@@ -189,12 +188,11 @@ def __init__(
         # State and covariance estimates
         self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
         self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
         self._dx = np.zeros(6)
 
         # Discrete state-space model
-        self._phi = _state_transition(self._dt, self._dtheta, self._gbc)
+        self._phi = _state_transition(self._dt, np.zeros(3), self._gbc)
         self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
         self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg))
 
@@ -326,13 +324,13 @@ def _aiding_update_head(
         dhdx = self._dhdx_yaw(self._q_nb)
         _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
 
-    def _project_ahead(self) -> None:
+    def _project_ahead(self, dtheta) -> None:
         """
         Project state and covariance estimates ahead.
         """
 
         # Attitude (dead reckoning)
-        _correct_quat_with_rotvec(self._q_nb, self._dtheta)
+        _correct_quat_with_rotvec(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
@@ -382,11 +380,16 @@ def update(
             A reference to the instance itself after the update.
         """
 
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
+
         if degrees:
             dtheta = np.radians(dtheta)
 
+        dtheta -= self._dt * self._bg_b
+
         # Project (a priori) state and covariance estimates ahead
-        self._project_ahead()
+        self._project_ahead(dtheta)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var)
@@ -396,7 +399,6 @@ def update(
         self._reset()
 
         # Update model
-        self._dtheta[:] = dtheta - self._dt * self._bg_b
-        _update_state_transition(self._phi, self._dtheta)
+        _update_state_transition(self._phi, dtheta)
 
         return self

From 52492e514ff09b97e0a33c29c5505266073ab288 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:03:21 +0100
Subject: [PATCH 037/217] fix use correct timestamp dv dth v2c

---
 src/smsfusion/_v2/_v2c.py | 35 +++++++++++++++++++----------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py
index 58fe378a..8ec4cca0 100644
--- a/src/smsfusion/_v2/_v2c.py
+++ b/src/smsfusion/_v2/_v2c.py
@@ -171,10 +171,10 @@ class AHRSv2c:
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
     bg_b : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    dvel : array_like, shape (3,), optional
-        Initial velocity change vector (sculling integral).
-    dtheta : array_like, shape (3,), optional
-        Initial attitude change vector (coning integral).
+    dvel_prev : array_like, shape (3,), optional
+        Previous velocity change vector measurement (sculling integral).
+    dtheta_prev : array_like, shape (3,), optional
+        Previous attitude change vector measurement (coning integral).
     P : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix, **P**. If not
         given, a small diagonal matrix will be used.
@@ -206,8 +206,8 @@ def __init__(
         v_n: ArrayLike = (0.0, 0.0, 0.0),
         q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        dvel: ArrayLike = (0.0, 0.0, 0.0),
-        dtheta: ArrayLike = (0.0, 0.0, 0.0),
+        dvel_prev: ArrayLike = (0.0, 0.0, 0.0),
+        dtheta_prev: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = 1e-6 * np.eye(9),
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
@@ -239,15 +239,15 @@ def __init__(
         self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
         self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
         self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy()
-        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
+        self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy()
+        self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy()
         self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 
         # Discrete state-space model
         self._phi = _state_transition(
-            self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc
+            self._dt, self._dvel_prev, self._dtheta_prev, self._R_nb, self._gbc
         )
         self._Q = _process_noise_cov(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
@@ -381,16 +381,16 @@ def _aiding_update_head(
         dhdx = self._dhdx_yaw(self._q_nb)
         _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
 
-    def _project_ahead(self) -> None:
+    def _project_ahead(self, dvel, dtheta) -> None:
         """
         Project state and covariance estimates ahead.
         """
 
         # Velocity (dead reckoning)
-        self._v_n[:] += self._R_nb @ self._dvel + self._dt * self._g_n
+        self._v_n[:] += self._R_nb @ dvel + self._dt * self._g_n
 
         # Attitude (dead reckoning)
-        _correct_quat_with_rotvec(self._q_nb, self._dtheta)
+        _correct_quat_with_rotvec(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
@@ -435,11 +435,16 @@ def update(
             A reference to the instance itself after the update.
         """
 
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
+
         if degrees:
             dtheta = np.radians(dtheta)
 
+        dtheta -= self._dt * self._bg_b
+
         # Project (a priori) state and covariance estimates ahead
-        self._project_ahead()
+        self._project_ahead(dvel, dtheta)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         self._aiding_update_vel(vel, vel_var)
@@ -449,9 +454,7 @@ def update(
         self._reset()
 
         # Update model
-        self._dvel[:] = dvel
-        self._dtheta[:] = dtheta - self._dt * self._bg_b
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
+        _update_state_transition(self._phi, dvel, dtheta, self._R_nb)
 
         return self

From 8e6a4b18bf18b7102810d5b9c92287745601b102 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:17:09 +0100
Subject: [PATCH 038/217] keep only v2c

---
 src/smsfusion/__init__.py             |   7 +-
 src/smsfusion/{_v2/_v2c.py => _v2.py} | 296 +++++++++++++++-
 src/smsfusion/_v2/__init__.py         |   6 -
 src/smsfusion/_v2/_v2a.py             | 401 ----------------------
 src/smsfusion/_v2/_v2b.py             | 466 --------------------------
 src/smsfusion/_v2/_v2common.py        | 286 ----------------
 src/smsfusion/_v2/_v2d.py             | 404 ----------------------
 7 files changed, 287 insertions(+), 1579 deletions(-)
 rename src/smsfusion/{_v2/_v2c.py => _v2.py} (65%)
 delete mode 100644 src/smsfusion/_v2/__init__.py
 delete mode 100644 src/smsfusion/_v2/_v2a.py
 delete mode 100644 src/smsfusion/_v2/_v2b.py
 delete mode 100644 src/smsfusion/_v2/_v2common.py
 delete mode 100644 src/smsfusion/_v2/_v2d.py

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 15bb24ae..6dd89429 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,14 +3,11 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c, AHRSv2d
+from ._v2 import AHRSv2
 
 __all__ = [
     "AHRS",
-    "AHRSv2a",
-    "AHRSv2b",
-    "AHRSv2c",
-    "AHRSv2d",
+    "AHRSv2",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2.py
similarity index 65%
rename from src/smsfusion/_v2/_v2c.py
rename to src/smsfusion/_v2.py
index 8ec4cca0..7e31526a 100644
--- a/src/smsfusion/_v2/_v2c.py
+++ b/src/smsfusion/_v2.py
@@ -4,22 +4,296 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from .._ins import _dhda_head, _h_head, _signed_smallest_angle
-from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from .._vectorops import _skew_symmetric
-from ._v2common import (
-    _correct_quat_with_gibbs2,
-    _correct_quat_with_rotvec,
-    _kalman_update_scalar,
-    _kalman_update_sequential,
-    _project_cov_ahead,
-)
+from ._ins import _dhda_head, _h_head, _signed_smallest_angle
+from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
 
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
 BG_IDX = slice(6, 9)
 
 
+def _nz2vg(nav_frame: str) -> float:
+    """
+    Gravity direction along the navigation frame's z-axis.
+    """
+    if nav_frame == "ned":
+        return 1.0
+    elif nav_frame == "enu":
+        return -1.0
+    else:
+        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+
+@njit  # type: ignore[misc]
+def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
+    """
+    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
+
+    Parameters
+    ----------
+    q_nb : numpy.ndarray, shape (4,)
+        Unit quaternion.
+    nz2vg : float
+        Gravity direction along the navigation frame's z-axis. Should be +1 for
+        NED and -1 for ENU.
+    """
+    qw, qx, qy, qz = q_nb
+
+    x = 2.0 * (qx * qz - qw * qy)
+    y = 2.0 * (qy * qz + qw * qx)
+    z = 1.0 - 2.0 * (qx**2 + qy**2)
+
+    return nz2vg * np.array([x, y, z])
+
+
+@njit  # type: ignore[misc]
+def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
+    as a scaled (2x) Gibbs vector:
+
+        q = q ⊗ dq(da)
+
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion [qw, qx, qy, qz] (modified in place).
+    da : ndarray, shape (3,)
+        Small attitude error parameterized as a scaled (2x) Gibbs vector.
+
+    References
+    ----------
+    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    and Control, Eq. (6.27)-(6.28).
+    """
+
+    qw, qx, qy, qz = q
+    dax, day, daz = da
+
+    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
+    q[:] = _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Compute the unit quaternion from a rotation vector.
+
+    Parameters
+    ----------
+    r : numpy.ndarray, shape (3,)
+        Rotation vector (rx, ry, rz).
+
+    Returns
+    -------
+    numpy.ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz).
+    """
+    # TODO: add reference
+
+    rx, ry, rz = r
+
+    angle2 = rx**2 + ry**2 + rz**2
+
+    if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
+        a = 0.25 * angle2
+        c = 1.0 - a / 2.0
+        s = 0.5 * (1.0 - a / 6.0)
+    else:
+        angle = np.sqrt(angle2)
+        half_angle = 0.5 * angle
+        c = np.cos(half_angle)
+        s = np.sin(half_angle) / angle
+
+    q = np.array([c, s * rx, s * ry, s * rz])
+
+    return _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _correct_quat_with_rotvec(
+    q: NDArray[np.float64], dtheta: NDArray[np.float64]
+) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+    parameterized as a rotation vector:
+
+        q = q ⊗ dq(dtheta)
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (modified in place).
+    dtheta : ndarray, shape (3,)
+        Small attitude change parameterized as a rotation vector.
+    """
+    q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
+
+
+@njit  # type: ignore[misc]
+def _kalman_gain(
+    P: NDArray[np.float64], h: NDArray[np.float64], r: float
+) -> NDArray[np.float64]:
+    """
+    Compute the Kalman gain for a scalar measurement.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n,)
+        Kalman gain vector.
+    """
+
+    # Innovation covariance (inverse)
+    Ph = np.dot(P, h)
+    s_inv = 1.0 / (np.dot(h, Ph) + r)
+
+    # Kalman gain
+    k = Ph * s_inv
+
+    return k
+
+
+@njit  # type: ignore[misc]
+def _covariance_update(
+    P: NDArray[np.float64],
+    k: NDArray[np.float64],
+    h: NDArray[np.float64],
+    r: float,
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Compute the updated state error covariance matrix estimate (Joseph form).
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    k : ndarray, shape (n,)
+        Kalman gain vector.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    A = I_ - np.outer(k, h)
+    P = A @ P @ A.T + r * np.outer(k, k)
+    return P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_scalar(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: float,
+    r: float,
+    h: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Scalar Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : float
+        Scalar measurement.
+    r : float
+        Scalar measurement noise variance.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+
+    # Kalman gain
+    k = _kalman_gain(P, h, r)
+
+    # Updated (a posteriori) state estimate
+    x[:] += k * (z - np.dot(h, x))
+
+    # Updated (a posteriori) covariance estimate (Joseph form)
+    P[:, :] = _covariance_update(P, k, h, r, I_)
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_sequential(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: NDArray[np.float64],
+    var: NDArray[np.float64],
+    H: NDArray[np.float64],
+    I_: NDArray[np.float64],
+) -> None:
+    """
+    Sequential (one-at-a-time) Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be updated in place.
+    z : ndarray, shape (m,)
+        Measurement vector.
+    var : ndarray, shape (m,)
+        Measurement noise variances corresponding to each scalar measurement.
+    H : ndarray, shape (m, n)
+        Measurement matrix where each row corresponds to a scalar measurement model.
+    I_ : ndarray, shape (n, n)
+        Identity matrix.
+    """
+    m = z.shape[0]
+    for i in range(m):
+        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
+
+
+@njit  # type: ignore[misc]
+def _project_cov_ahead(
+    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
+) -> None:
+    """
+    Project the error covariance matrix estimate ahead.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix to be projected ahead.
+    phi : ndarray, shape (n, n)
+        State transition matrix.
+    Q : ndarray, shape (n, n)
+        Process noise covariance matrix.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Projected error covariance matrix estimate.
+    """
+    P = phi @ P @ phi.T + Q
+    return P
+
+
 def _state_transition(
     dt: float,
     dvel: NDArray[np.float64],
@@ -155,7 +429,7 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
     return dhdx
 
 
-class AHRSv2c:
+class AHRSv2:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
     Kalman filter (MEKF).
diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py
deleted file mode 100644
index 8f8718ff..00000000
--- a/src/smsfusion/_v2/__init__.py
+++ /dev/null
@@ -1,6 +0,0 @@
-from ._v2a import AHRSv2a
-from ._v2b import AHRSv2b
-from ._v2c import AHRSv2c
-from ._v2d import AHRSv2d
-
-__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c", "AHRSv2d"]
diff --git a/src/smsfusion/_v2/_v2a.py b/src/smsfusion/_v2/_v2a.py
deleted file mode 100644
index e04707e9..00000000
--- a/src/smsfusion/_v2/_v2a.py
+++ /dev/null
@@ -1,401 +0,0 @@
-from typing import Self
-
-import numpy as np
-from numba import njit
-from numpy.typing import ArrayLike, NDArray
-
-from .._ins import _dhda_head, _h_head, _signed_smallest_angle
-from .._transforms import _angular_matrix_from_quaternion as T
-from .._transforms import _euler_from_quaternion
-from .._vectorops import _normalize, _skew_symmetric
-from ._v2common import (
-    _correct_quat_with_gibbs2,
-    _kalman_update_scalar,
-    _kalman_update_sequential,
-    _nz2vg,
-    _project_cov_ahead,
-    _vg_b,
-)
-
-
-def _state_transition(
-    dt: float, w_b: NDArray[np.float64], gbc: float
-) -> NDArray[np.float64]:
-    """
-    State transition matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    w_b : ndarray, shape (3,)
-        Angular rate measurement (bias corrected) in body frame.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (6, 6)
-        State transition matrix.
-    """
-    phi = np.eye(6)
-    phi[0:3, 0:3] -= dt * _skew_symmetric(w_b)  # NB! update each time step
-    phi[0:3, 3:6] -= dt * np.eye(3)
-    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
-    return phi
-
-
-@njit  # type: ignore[misc]
-def _update_state_transition(
-    phi: NDArray[np.float64],
-    dt: float,
-    w_b: NDArray[np.float64],
-) -> None:
-    """
-    Update the state transition matrix in place.
-
-    Parameters
-    ----------
-    phi : ndarray, shape (6, 6)
-        State transition matrix to be updated in place.
-    dt : float
-        Time step.
-    w_b : ndarray, shape (3,)
-        Angular rate measurement (bias corrected) in body frame.
-    """
-    wx, wy, wz = w_b
-    phi[0, 1] = dt * wz
-    phi[0, 2] = -dt * wy
-    phi[1, 0] = -dt * wz
-    phi[1, 2] = dt * wx
-    phi[2, 0] = dt * wy
-    phi[2, 1] = -dt * wx
-
-
-def _process_noise_cov(
-    dt: float, arw: float, gbs: float, gbc: float
-) -> NDArray[np.float64]:
-    """
-    Process noise covariance matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    arw : float
-        Angular random walk (gyroscope noise density) in rad/√Hz.
-    gbs : float
-        Gyro bias stability (bias instability) in rad/s.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    Q : ndarray, shape (6, 6)
-        Process noise covariance matrix.
-    """
-    Q = np.zeros((6, 6))
-    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
-    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
-    return Q
-
-
-def _measurement_matrix(
-    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
-) -> NDArray[np.float64]:
-    """
-    Measurement matrix.
-
-    Parameters
-    ----------
-    q_nb : ndarray, shape (4,)
-        Unit quaternion.
-    vg_b : ndarray, shape (3,)
-        Gravity reference unit vector expressed in the body frame.
-
-    Returns
-    -------
-    ndarray, shape (4, 6)
-        Linearized measurement matrix.
-    """
-    dhdx = np.zeros((4, 6))
-    dhdx[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity ref vector
-    dhdx[3:4, 0:3] = _dhda_head(q_nb)  # heading
-    return dhdx
-
-
-class AHRSv2a:
-    """
-    Attitude and Heading Reference System (AHRS) using a multiplicative extended
-    Kalman filter (MEKF).
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    q_nb : Attitude or array_like, shape (4,), optional
-        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
-        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg_b : array_like, shape (3,), optional
-        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    w_b : array_like, shape (3,), optional
-        Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame.
-        Defaults to zero angular rate (stationary).
-    P : array_like, shape (6, 6), optional
-        Initial (a priori) estimate of the error covariance matrix, **P**. If not
-        given, a small diagonal matrix will be used.
-    gyro_noise_density : float, optional
-        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (SMS Motion 2 noise level).
-    gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
-    gyro_bias_corr_time : float, optional
-        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
-
-    """
-
-    _I: NDArray[np.float64] = np.eye(6)
-
-    def __init__(
-        self,
-        fs: float,
-        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        w_b: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(6),
-        gyro_noise_density: float = 0.0001,
-        gyro_bias_stability: float = 0.00005,
-        gyro_bias_corr_time: float = 50.0,
-        nav_frame: str = "NED",
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-        self._nav_frame = nav_frame.lower()
-        self._nz2vg = _nz2vg(self._nav_frame)
-
-        # IMU noise parameters
-        self._arw = gyro_noise_density  # angular random walk
-        self._gbs = gyro_bias_stability  # gyro bias stability
-        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
-
-        # State and covariance estimates
-        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
-        self._dx = np.zeros(6)
-
-        # Discrete state-space model
-        self._phi = _state_transition(self._dt, self._w_b, self._gbc)
-        self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
-        self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg))
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles (roll, pitch, yaw).
-        """
-
-        theta = _euler_from_quaternion(self._q_nb)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta
-
-    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
-        """
-        bg_b = self._bg_b.copy()
-        if degrees:
-            bg_b = (180.0 / np.pi) * bg_b
-        return bg_b
-
-    def angular_rate(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Bias corrected angular rate measurement expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
-        """
-        w_b = self._w_b.copy()
-        if degrees:
-            w_b = (180.0 / np.pi) * w_b
-        return w_b
-
-    @property
-    def P(self) -> NDArray[np.float64]:
-        """
-        Copy of the error covariance matrix estimate.
-        """
-        return self._P.copy()
-
-    def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Gravity reference vector part of the measurement matrix, shape (3, 6).
-        """
-        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
-        return self._dhdx[0:3]
-
-    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Heading (yaw angle) part of the measurement matrix, shape (6,).
-        """
-        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
-        return self._dhdx[3]
-
-    def _reset(self) -> None:
-        """
-        Reset state.
-        """
-
-        if not self._dx.any():
-            return
-
-        _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3])
-        self._bg_b[:] += self._dx[3:6]
-        self._dx[:] = 0.0
-
-    def _aiding_update_gref(
-        self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
-    ) -> None:
-        """
-        Update with gravity reference vector aiding measurement.
-        """
-
-        if vg_meas is None:
-            return None
-
-        if vg_var is None:
-            raise ValueError("'vg_var' not provided.")
-
-        vg_b = _vg_b(self._q_nb, self._nz2vg)
-        dz = vg_meas - vg_b
-        dhdx = self._dhdx_gref(vg_b)
-        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
-
-    def _aiding_update_head(
-        self, head_meas: float | None, head_var: float | None, head_degrees: bool
-    ) -> None:
-        """
-        Update with heading aiding measurement.
-        """
-
-        if head_meas is None:
-            return None
-
-        if head_var is None:
-            raise ValueError("'head_var' not provided.")
-
-        if head_degrees:
-            head_meas = (np.pi / 180.0) * head_meas
-            head_var = (np.pi / 180.0) ** 2 * head_var
-
-        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
-        dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
-
-    def _project_ahead(self) -> None:
-        """
-        Project state and covariance estimates ahead.
-        """
-
-        # Attitude (dead reckoning)
-        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
-        self._q_nb[:] = _normalize(self._q_nb)
-
-        # Covariance
-        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
-
-    def update(
-        self,
-        f: ArrayLike,
-        w: ArrayLike,
-        degrees: bool = False,
-        head: float | None = None,
-        head_var: float | None = None,
-        head_degrees: bool = False,
-        g_ref: bool = True,
-        g_var: ArrayLike | None = (0.001, 0.001, 0.001),
-    ) -> Self:
-        """
-        Update state estimates with IMU and aiding measurements.
-
-        Parameters
-        ----------
-        f : array_like, shape (3,)
-            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
-            in m/s^2.
-        w : array_like, shape (3,)
-            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
-            ``degrees`` parameter for units.
-        degrees : bool, optional
-            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
-            rad/s. Defaults to rad/s.
-        head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
-        head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
-        head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
-        g_ref : bool, optional
-            Specifies whether the gravity reference vector is used as an aiding measurement.
-        g_var : array-like, optional
-            Variance of gravitational reference vector measurement noise. Required for
-            ``g_ref``.
-
-        Returns
-        -------
-        AHRS
-            A reference to the instance itself after the update.
-        """
-
-        if degrees:
-            w = np.radians(w)
-
-        # Project (a priori) state and covariance estimates ahead
-        self._project_ahead()
-
-        # Update (a posteriori) state and covariance estimates with aiding measurements
-        self._aiding_update_gref(-_normalize(f) if g_ref else None, g_var)
-        self._aiding_update_head(head, head_var, head_degrees)
-
-        # Reset state
-        self._reset()
-
-        # Update model
-        self._w_b[:] = w - self._bg_b
-        _update_state_transition(self._phi, self._dt, self._w_b)
-
-        return self
diff --git a/src/smsfusion/_v2/_v2b.py b/src/smsfusion/_v2/_v2b.py
deleted file mode 100644
index 15cd71ee..00000000
--- a/src/smsfusion/_v2/_v2b.py
+++ /dev/null
@@ -1,466 +0,0 @@
-from typing import Self
-
-import numpy as np
-from numba import njit
-from numpy.typing import ArrayLike, NDArray
-
-from .._ins import _dhda_head, _h_head, _signed_smallest_angle
-from .._transforms import _angular_matrix_from_quaternion as T
-from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from .._vectorops import _normalize, _skew_symmetric
-from ._v2common import (
-    _correct_quat_with_gibbs2,
-    _kalman_update_scalar,
-    _kalman_update_sequential,
-    _project_cov_ahead,
-)
-
-VEL_IDX = slice(0, 3)
-ATT_IDX = slice(3, 6)
-BG_IDX = slice(6, 9)
-
-
-def _state_transition(
-    dt: float,
-    f_b: NDArray[np.float64],
-    w_b: NDArray[np.float64],
-    R_nb: NDArray[np.float64],
-    gbc: float,
-) -> NDArray[np.float64]:
-    """
-    State transition matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    f_b : ndarray, shape (3,)
-        Specific force measurement in body frame.
-    w_b : ndarray, shape (3,)
-        Angular rate measurement (bias corrected) in body frame.
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (9, 9)
-        State transition matrix.
-    """
-    phi = np.eye(9)
-    phi[VEL_IDX, ATT_IDX] -= (
-        dt * R_nb @ _skew_symmetric(f_b)
-    )  # NB! update each time step
-    phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b)  # NB! update each time step
-    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
-    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
-    return phi
-
-
-@njit  # type: ignore[misc]
-def _update_state_transition(
-    phi: NDArray[np.float64],
-    dt: float,
-    f_b: NDArray[np.float64],
-    w_b: NDArray[np.float64],
-    R_nb: NDArray[np.float64],
-) -> None:
-    """
-    Update the state transition matrix in place.
-
-    Parameters
-    ----------
-    phi : ndarray, shape (9, 9)
-        State transition matrix to be updated in place.
-    dt : float
-        Time step.
-    f_b : ndarray, shape (3,)
-        Specific force measurement in body frame.
-    w_b : ndarray, shape (3,)
-        Angular rate measurement (bias corrected) in body frame.
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    """
-    wx, wy, wz = w_b
-    fx, fy, fz = f_b
-
-    r00, r01, r02 = R_nb[0]
-    r10, r11, r12 = R_nb[1]
-    r20, r21, r22 = R_nb[2]
-
-    # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b)
-    phi[3, 4] = dt * wz
-    phi[3, 5] = -dt * wy
-    phi[4, 3] = -dt * wz
-    phi[4, 5] = dt * wx
-    phi[5, 3] = dt * wy
-    phi[5, 4] = -dt * wx
-
-    # phi[0:3, 3:6] = -dt * R_nb @ S(f_b)
-    phi[0, 3] = -dt * (fz * r01 - fy * r02)
-    phi[1, 3] = -dt * (fz * r11 - fy * r12)
-    phi[2, 3] = -dt * (fz * r21 - fy * r22)
-    phi[0, 4] = -dt * (-fz * r00 + fx * r02)
-    phi[1, 4] = -dt * (-fz * r10 + fx * r12)
-    phi[2, 4] = -dt * (-fz * r20 + fx * r22)
-    phi[0, 5] = -dt * (fy * r00 - fx * r01)
-    phi[1, 5] = -dt * (fy * r10 - fx * r11)
-    phi[2, 5] = -dt * (fy * r20 - fx * r21)
-
-
-def _process_noise_cov(
-    dt: float, vrw: float, arw: float, gbs: float, gbc: float
-) -> NDArray[np.float64]:
-    """
-    Process noise covariance matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    vrw : float
-        Velocity random walk (accelerometer noise density) in m/s/√Hz.
-    arw : float
-        Angular random walk (gyroscope noise density) in rad/√Hz.
-    gbs : float
-        Gyro bias stability (bias instability) in rad/s.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    Q : ndarray, shape (9, 9)
-        Process noise covariance matrix.
-    """
-    Q = np.zeros((9, 9))
-    Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3)
-    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
-    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
-    return Q
-
-
-def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-    """
-    Measurement matrix.
-
-    Parameters
-    ----------
-    q_nb : ndarray, shape (4,)
-        Unit quaternion.
-
-    Returns
-    -------
-    ndarray, shape (4, 6)
-        Linearized measurement matrix.
-    """
-    dhdx = np.zeros((4, 9))
-    dhdx[0:3, VEL_IDX] = np.eye(3)  # velocity
-    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
-    return dhdx
-
-
-class AHRSv2b:
-    """
-    Attitude and Heading Reference System (AHRS) using a multiplicative extended
-    Kalman filter (MEKF).
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    v_n : array_like, shape (3,), optional
-        Initial velocity estimate in m/s.
-    q_nb : Attitude or array_like, shape (4,), optional
-        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
-        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg_b : array_like, shape (3,), optional
-        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    w_b : array_like, shape (3,), optional
-        Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame.
-        Defaults to zero angular rate (stationary).
-    P : array_like, shape (6, 6), optional
-        Initial (a priori) estimate of the error covariance matrix, **P**. If not
-        given, a small diagonal matrix will be used.
-    acc_noise_density : float, optional
-        Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to
-        0.0007 (SMS Motion 2 noise level).
-    gyro_noise_density : float, optional
-        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (SMS Motion 2 noise level).
-    gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
-    gyro_bias_corr_time : float, optional
-        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    g : float, default 9.80665
-        The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665
-        m/s^2.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
-
-    """
-
-    _I: NDArray[np.float64] = np.eye(9)
-
-    def __init__(
-        self,
-        fs: float,
-        v_n: ArrayLike = (0.0, 0.0, 0.0),
-        a_n: ArrayLike = (0.0, 0.0, 0.0),
-        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        w_b: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(9),
-        acc_noise_density: float = 0.0007,
-        gyro_noise_density: float = 0.0001,
-        gyro_bias_stability: float = 0.00005,
-        gyro_bias_corr_time: float = 50.0,
-        g: float = 9.80665,
-        nav_frame: str = "NED",
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-        self._g = g
-        self._nav_frame = nav_frame.lower()
-
-        if self._nav_frame == "ned":
-            self._g_n = np.array([0.0, 0.0, g])
-        elif self._nav_frame == "enu":
-            self._g_n = np.array([0.0, 0.0, -g])
-        else:
-            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
-
-        # IMU noise parameters
-        self._vrw = acc_noise_density  # velocity random walk
-        self._arw = gyro_noise_density  # angular random walk
-        self._gbs = gyro_bias_stability  # gyro bias stability
-        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
-
-        # State and covariance estimates
-        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
-        self._a_n = np.asarray_chkfinite(a_n).reshape(3).copy()
-        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
-        self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._f_b = self._R_nb.T @ (self._a_n - self._g_n)
-        self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy()
-        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
-        self._dx = np.zeros(9)
-
-        # Discrete state-space model
-        self._phi = _state_transition(
-            self._dt, self._f_b, self._w_b, self._R_nb, self._gbc
-        )
-        self._Q = _process_noise_cov(
-            self._dt, self._vrw, self._arw, self._gbs, self._gbc
-        )
-        self._dhdx = _measurement_matrix(self._q_nb)
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles (roll, pitch, yaw).
-        """
-
-        theta = _euler_from_quaternion(self._q_nb)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta
-
-    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
-        """
-        bg_b = self._bg_b.copy()
-        if degrees:
-            bg_b = (180.0 / np.pi) * bg_b
-        return bg_b
-
-    def angular_rate(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Bias corrected angular rate measurement expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
-        """
-        w_b = self._w_b.copy()
-        if degrees:
-            w_b = (180.0 / np.pi) * w_b
-        return w_b
-
-    @property
-    def P(self) -> NDArray[np.float64]:
-        """
-        Copy of the error covariance matrix estimate.
-        """
-        return self._P.copy()
-
-    def _dhdx_vel(self) -> NDArray[np.float64]:
-        """
-        Velocity part of the measurement matrix, shape (3, 6).
-        """
-        return self._dhdx[0:3]
-
-    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Heading (yaw angle) part of the measurement matrix, shape (6,).
-        """
-        self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)
-        return self._dhdx[3]
-
-    def _reset(self) -> None:
-        """
-        Reset state.
-        """
-
-        if not self._dx.any():
-            return
-
-        _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
-        self._v_n[:] += self._dx[VEL_IDX]
-        self._bg_b[:] += self._dx[BG_IDX]
-        self._dx[:] = 0.0
-
-    def _aiding_update_vel(
-        self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
-    ) -> None:
-        """
-        Update with velocity aiding measurement.
-        """
-
-        if vel_meas is None:
-            return None
-
-        if vel_var is None:
-            raise ValueError("'vg_var' not provided.")
-
-        dz = vel_meas - self._v_n
-        dhdx = self._dhdx_vel()
-        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I)
-
-    def _aiding_update_head(
-        self, head_meas: float | None, head_var: float | None, head_degrees: bool
-    ) -> None:
-        """
-        Update with heading aiding measurement.
-        """
-
-        if head_meas is None:
-            return None
-
-        if head_var is None:
-            raise ValueError("'head_var' not provided.")
-
-        if head_degrees:
-            head_meas = (np.pi / 180.0) * head_meas
-            head_var = (np.pi / 180.0) ** 2 * head_var
-
-        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
-        dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
-
-    def _project_ahead(self) -> None:
-        """
-        Project state and covariance estimates ahead.
-        """
-
-        # Velocity (dead reckoning)
-        self._v_n[:] += self._dt * self._a_n
-
-        # Attitude (dead reckoning)
-        self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b
-        self._q_nb[:] = _normalize(self._q_nb)
-
-        # Covariance
-        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
-
-    def update(
-        self,
-        f: ArrayLike,
-        w: ArrayLike,
-        degrees: bool = False,
-        head: float | None = None,
-        head_var: float | None = None,
-        head_degrees: bool = False,
-        vel: ArrayLike | None = (0.0, 0.0, 0.0),
-        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
-    ) -> Self:
-        """
-        Update state estimates with IMU and aiding measurements.
-
-        Parameters
-        ----------
-        f : array_like, shape (3,)
-            Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz)
-            in m/s^2.
-        w : array_like, shape (3,)
-            Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See
-            ``degrees`` parameter for units.
-        degrees : bool, optional
-            Specifies whether the unit of the rotation rate, ``w``, is deg/s or
-            rad/s. Defaults to rad/s.
-        head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
-        head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
-        head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
-
-        Returns
-        -------
-        AHRS
-            A reference to the instance itself after the update.
-        """
-
-        if degrees:
-            w = np.radians(w)
-
-        # Project (a priori) state and covariance estimates ahead
-        self._project_ahead()
-
-        # Update (a posteriori) state and covariance estimates with aiding measurements
-        self._aiding_update_vel(vel, vel_var)
-        self._aiding_update_head(head, head_var, head_degrees)
-
-        # Reset state
-        self._reset()
-
-        # Update model
-        self._f_b[:] = f
-        self._w_b[:] = w - self._bg_b
-        self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        self._a_n[:] = self._R_nb @ self._f_b + self._g_n
-        _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb)
-
-        return self
diff --git a/src/smsfusion/_v2/_v2common.py b/src/smsfusion/_v2/_v2common.py
deleted file mode 100644
index e2127a03..00000000
--- a/src/smsfusion/_v2/_v2common.py
+++ /dev/null
@@ -1,286 +0,0 @@
-import numpy as np
-from numba import njit
-from numpy.typing import NDArray
-
-from .._vectorops import _normalize, _quaternion_product
-
-
-def _nz2vg(nav_frame: str) -> float:
-    """
-    Gravity direction along the navigation frame's z-axis.
-    """
-    if nav_frame == "ned":
-        return 1.0
-    elif nav_frame == "enu":
-        return -1.0
-    else:
-        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
-
-
-@njit  # type: ignore[misc]
-def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
-    """
-    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
-
-    Parameters
-    ----------
-    q_nb : numpy.ndarray, shape (4,)
-        Unit quaternion.
-    nz2vg : float
-        Gravity direction along the navigation frame's z-axis. Should be +1 for
-        NED and -1 for ENU.
-    """
-    qw, qx, qy, qz = q_nb
-
-    x = 2.0 * (qx * qz - qw * qy)
-    y = 2.0 * (qy * qz + qw * qx)
-    z = 1.0 - 2.0 * (qx**2 + qy**2)
-
-    return nz2vg * np.array([x, y, z])
-
-
-@njit  # type: ignore[misc]
-def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
-    """
-    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector:
-
-        q = q ⊗ dq(da)
-
-    As described in ref [1]_, this correction can be simplified by doing it in two
-    steps: first a correction, followed by renormalization. The scaling factor becomes
-    obsolete due to the renormalization step.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion [qw, qx, qy, qz] (modified in place).
-    da : ndarray, shape (3,)
-        Small attitude error parameterized as a scaled (2x) Gibbs vector.
-
-    References
-    ----------
-    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
-    and Control, Eq. (6.27)-(6.28).
-    """
-
-    qw, qx, qy, qz = q
-    dax, day, daz = da
-
-    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
-    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
-    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
-    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
-    """
-    Compute the unit quaternion from a rotation vector.
-
-    Parameters
-    ----------
-    r : numpy.ndarray, shape (3,)
-        Rotation vector (rx, ry, rz).
-
-    Returns
-    -------
-    numpy.ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz).
-    """
-    # TODO: add reference
-
-    rx, ry, rz = r
-
-    angle2 = rx**2 + ry**2 + rz**2
-
-    if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
-        a = 0.25 * angle2
-        c = 1.0 - a / 2.0
-        s = 0.5 * (1.0 - a / 6.0)
-    else:
-        angle = np.sqrt(angle2)
-        half_angle = 0.5 * angle
-        c = np.cos(half_angle)
-        s = np.sin(half_angle) / angle
-
-    q = np.array([c, s * rx, s * ry, s * rz])
-
-    return _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _correct_quat_with_rotvec(
-    q: NDArray[np.float64], dtheta: NDArray[np.float64]
-) -> None:
-    """
-    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-    parameterized as a rotation vector:
-
-        q = q ⊗ dq(dtheta)
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (modified in place).
-    dtheta : ndarray, shape (3,)
-        Small attitude change parameterized as a rotation vector.
-    """
-    q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
-
-
-@njit  # type: ignore[misc]
-def _kalman_gain(
-    P: NDArray[np.float64], h: NDArray[np.float64], r: float
-) -> NDArray[np.float64]:
-    """
-    Compute the Kalman gain for a scalar measurement.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n,)
-        Kalman gain vector.
-    """
-
-    # Innovation covariance (inverse)
-    Ph = np.dot(P, h)
-    s_inv = 1.0 / (np.dot(h, Ph) + r)
-
-    # Kalman gain
-    k = Ph * s_inv
-
-    return k
-
-
-@njit  # type: ignore[misc]
-def _covariance_update(
-    P: NDArray[np.float64],
-    k: NDArray[np.float64],
-    h: NDArray[np.float64],
-    r: float,
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Compute the updated state error covariance matrix estimate (Joseph form).
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    k : ndarray, shape (n,)
-        Kalman gain vector.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-    A = I_ - np.outer(k, h)
-    P = A @ P @ A.T + r * np.outer(k, k)
-    return P
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_scalar(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: float,
-    r: float,
-    h: NDArray[np.float64],
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Scalar Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    z : float
-        Scalar measurement.
-    r : float
-        Scalar measurement noise variance.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-
-    # Kalman gain
-    k = _kalman_gain(P, h, r)
-
-    # Updated (a posteriori) state estimate
-    x[:] += k * (z - np.dot(h, x))
-
-    # Updated (a posteriori) covariance estimate (Joseph form)
-    P[:, :] = _covariance_update(P, k, h, r, I_)
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_sequential(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: NDArray[np.float64],
-    var: NDArray[np.float64],
-    H: NDArray[np.float64],
-    I_: NDArray[np.float64],
-) -> None:
-    """
-    Sequential (one-at-a-time) Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
-    z : ndarray, shape (m,)
-        Measurement vector.
-    var : ndarray, shape (m,)
-        Measurement noise variances corresponding to each scalar measurement.
-    H : ndarray, shape (m, n)
-        Measurement matrix where each row corresponds to a scalar measurement model.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
-    """
-    m = z.shape[0]
-    for i in range(m):
-        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
-
-
-@njit  # type: ignore[misc]
-def _project_cov_ahead(
-    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
-) -> None:
-    """
-    Project the error covariance matrix estimate ahead.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix to be projected ahead.
-    phi : ndarray, shape (n, n)
-        State transition matrix.
-    Q : ndarray, shape (n, n)
-        Process noise covariance matrix.
-
-    Returns
-    -------
-    ndarray, shape (n, n)
-        Projected error covariance matrix estimate.
-    """
-    P = phi @ P @ phi.T + Q
-    return P
diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py
deleted file mode 100644
index eb86eb6a..00000000
--- a/src/smsfusion/_v2/_v2d.py
+++ /dev/null
@@ -1,404 +0,0 @@
-from typing import Self
-
-import numpy as np
-from numba import njit
-from numpy.typing import ArrayLike, NDArray
-
-from .._ins import _dhda_head, _h_head, _signed_smallest_angle
-from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from .._vectorops import _skew_symmetric
-from .._vectorops import _normalize
-from ._v2common import (
-    _correct_quat_with_gibbs2,
-    _correct_quat_with_rotvec,
-    _kalman_update_scalar,
-    _kalman_update_sequential,
-    _project_cov_ahead,
-    _nz2vg,
-    _vg_b,
-)
-
-ATT_IDX = slice(0, 3)
-BG_IDX = slice(3, 6)
-
-
-def _state_transition(
-    dt: float,
-    dtheta: NDArray[np.float64],
-    gbc: float,
-) -> NDArray[np.float64]:
-    """
-    State transition matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    dtheta : ndarray, shape (3,)
-        Attitude change vector (coning integral).
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (6, 6)
-        State transition matrix.
-    """
-    phi = np.eye(6)
-    phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta)  # NB! update each time step
-    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
-    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
-    return phi
-
-
-@njit  # type: ignore[misc]
-def _update_state_transition(
-    phi: NDArray[np.float64],
-    dtheta: NDArray[np.float64],
-) -> None:
-    """
-    Update the state transition matrix in place.
-
-    Parameters
-    ----------
-    phi : ndarray, shape (9, 9)
-        State transition matrix to be updated in place.
-    dtheta : ndarray, shape (3,)
-        Attitude change vector (coning integral).
-    """
-    dtx, dty, dtz = dtheta
-
-    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
-    phi[0, 1] = dtz
-    phi[0, 2] = -dty
-    phi[1, 0] = -dtz
-    phi[1, 2] = dtx
-    phi[2, 0] = dty
-    phi[2, 1] = -dtx
-
-
-def _process_noise_cov(
-    dt: float, arw: float, gbs: float, gbc: float
-) -> NDArray[np.float64]:
-    """
-    Process noise covariance matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    arw : float
-        Angular random walk (gyroscope noise density) in rad/√Hz.
-    gbs : float
-        Gyro bias stability (bias instability) in rad/s.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    Q : ndarray, shape (9, 9)
-        Process noise covariance matrix.
-    """
-    Q = np.zeros((6, 6))
-    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
-    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
-    return Q
-
-
-def _measurement_matrix(
-    q_nb: NDArray[np.float64], vg_b: NDArray[np.float64]
-) -> NDArray[np.float64]:
-    """
-    Measurement matrix.
-
-    Parameters
-    ----------
-    q_nb : ndarray, shape (4,)
-        Unit quaternion.
-    vg_b : ndarray, shape (3,)
-        Gravity reference unit vector expressed in the body frame.
-
-    Returns
-    -------
-    ndarray, shape (4, 6)
-        Linearized measurement matrix.
-    """
-    dhdx = np.zeros((4, 6))
-    dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b)  # gravity ref vector
-    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
-    return dhdx
-
-
-class AHRSv2d:
-    """
-    Attitude and Heading Reference System (AHRS) using a multiplicative extended
-    Kalman filter (MEKF).
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    q_nb : Attitude or array_like, shape (4,), optional
-        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
-        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg_b : array_like, shape (3,), optional
-        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    dtheta : array_like, shape (3,), optional
-        Initial attitude change vector (coning integral).
-    P : array_like, shape (6, 6), optional
-        Initial (a priori) estimate of the error covariance matrix, **P**. If not
-        given, a small diagonal matrix will be used.
-    gyro_noise_density : float, optional
-        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (SMS Motion 2 noise level).
-    gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
-    gyro_bias_corr_time : float, optional
-        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
-
-    """
-
-    _I: NDArray[np.float64] = np.eye(6)
-
-    def __init__(
-        self,
-        fs: float,
-        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(6),
-        gyro_noise_density: float = 0.0001,
-        gyro_bias_stability: float = 0.00005,
-        gyro_bias_corr_time: float = 50.0,
-        nav_frame: str = "NED",
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-        self._nav_frame = nav_frame.lower()
-        self._nz2vg = _nz2vg(self._nav_frame)
-
-        # IMU noise parameters
-        self._arw = gyro_noise_density  # angular random walk
-        self._gbs = gyro_bias_stability  # gyro bias stability
-        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
-
-        # State and covariance estimates
-        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
-        self._dx = np.zeros(6)
-
-        # Discrete state-space model
-        self._phi = _state_transition(self._dt, np.zeros(3), self._gbc)
-        self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc)
-        self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg))
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles (roll, pitch, yaw).
-        """
-
-        theta = _euler_from_quaternion(self._q_nb)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta
-
-    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
-        """
-        bg_b = self._bg_b.copy()
-        if degrees:
-            bg_b = (180.0 / np.pi) * bg_b
-        return bg_b
-
-    # def angular_rate(self, degrees=False) -> NDArray[np.float64]:
-    #     """
-    #     Bias corrected angular rate measurement expressed in the body frame.
-
-    #     Parameters
-    #     ----------
-    #     degrees : bool, optional
-    #         Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
-    #     """
-    #     w_b = self._w_b.copy()
-    #     if degrees:
-    #         w_b = (180.0 / np.pi) * w_b
-    #     return w_b
-
-    @property
-    def P(self) -> NDArray[np.float64]:
-        """
-        Copy of the error covariance matrix estimate.
-        """
-        return self._P.copy()
-
-    def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Gravity reference vector part of the measurement matrix, shape (3, 6).
-        """
-        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
-        return self._dhdx[0:3]
-
-    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Heading (yaw angle) part of the measurement matrix, shape (6,).
-        """
-        self._dhdx[3:4, 0:3] = _dhda_head(q_nb)
-        return self._dhdx[3]
-
-    def _reset(self) -> None:
-        """
-        Reset state.
-        """
-
-        if not self._dx.any():
-            return
-
-        _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3])
-        self._bg_b[:] += self._dx[3:6]
-        self._dx[:] = 0.0
-
-    def _aiding_update_gref(
-        self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None
-    ) -> None:
-        """
-        Update with gravity reference vector aiding measurement.
-        """
-
-        if vg_meas is None:
-            return None
-
-        if vg_var is None:
-            raise ValueError("'vg_var' not provided.")
-
-        vg_b = _vg_b(self._q_nb, self._nz2vg)
-        dz = vg_meas - vg_b
-        dhdx = self._dhdx_gref(vg_b)
-        _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I)
-
-    def _aiding_update_head(
-        self, head_meas: float | None, head_var: float | None, head_degrees: bool
-    ) -> None:
-        """
-        Update with heading aiding measurement.
-        """
-
-        if head_meas is None:
-            return None
-
-        if head_var is None:
-            raise ValueError("'head_var' not provided.")
-
-        if head_degrees:
-            head_meas = (np.pi / 180.0) * head_meas
-            head_var = (np.pi / 180.0) ** 2 * head_var
-
-        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
-        dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
-
-    def _project_ahead(self, dtheta) -> None:
-        """
-        Project state and covariance estimates ahead.
-        """
-
-        # Attitude (dead reckoning)
-        _correct_quat_with_rotvec(self._q_nb, dtheta)
-
-        # Covariance
-        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
-
-    def update(
-        self,
-        dvel: ArrayLike,
-        dtheta: ArrayLike,
-        degrees: bool = False,
-        head: float | None = None,
-        head_var: float | None = None,
-        head_degrees: bool = False,
-        g_ref: bool = True,
-        g_var: ArrayLike | None = (0.001, 0.001, 0.001),
-    ) -> Self:
-        """
-        Update state estimates with IMU and aiding measurements.
-
-        Parameters
-        ----------
-        dvel : array_like, shape (3,), optional
-            Initial velocity change vector (sculling integral).
-        dtheta : array_like, shape (3,), optional
-            Initial attitude change vector (coning integral).
-        degrees : bool, optional
-            Specifies whether the unit of the attitude change vector, ``dtheta``,
-            is degrees or radians. Defaults to radians.
-        head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
-        head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
-        head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
-        g_ref : bool, optional
-            Specifies whether the gravity reference vector is used as an aiding measurement.
-        g_var : array-like, optional
-            Variance of gravitational reference vector measurement noise. Required for
-            ``g_ref``.
-
-        Returns
-        -------
-        AHRS
-            A reference to the instance itself after the update.
-        """
-
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
-
-        if degrees:
-            dtheta = np.radians(dtheta)
-
-        dtheta -= self._dt * self._bg_b
-
-        # Project (a priori) state and covariance estimates ahead
-        self._project_ahead(dtheta)
-
-        # Update (a posteriori) state and covariance estimates with aiding measurements
-        self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var)
-        self._aiding_update_head(head, head_var, head_degrees)
-
-        # Reset state
-        self._reset()
-
-        # Update model
-        _update_state_transition(self._phi, dtheta)
-
-        return self

From ce634865c02cd3eea03c344c3e3f80b503a796b5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:29:59 +0100
Subject: [PATCH 039/217] delete obsolete functions

---
 src/smsfusion/_v2.py | 34 ----------------------------------
 1 file changed, 34 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 7e31526a..3a28eae4 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -13,40 +13,6 @@
 BG_IDX = slice(6, 9)
 
 
-def _nz2vg(nav_frame: str) -> float:
-    """
-    Gravity direction along the navigation frame's z-axis.
-    """
-    if nav_frame == "ned":
-        return 1.0
-    elif nav_frame == "enu":
-        return -1.0
-    else:
-        raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
-
-
-@njit  # type: ignore[misc]
-def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]:
-    """
-    Gravity reference vector expressed in the body frame, computed from a unit quaternion.
-
-    Parameters
-    ----------
-    q_nb : numpy.ndarray, shape (4,)
-        Unit quaternion.
-    nz2vg : float
-        Gravity direction along the navigation frame's z-axis. Should be +1 for
-        NED and -1 for ENU.
-    """
-    qw, qx, qy, qz = q_nb
-
-    x = 2.0 * (qx * qz - qw * qy)
-    y = 2.0 * (qy * qz + qw * qx)
-    z = 1.0 - 2.0 * (qx**2 + qy**2)
-
-    return nz2vg * np.array([x, y, z])
-
-
 @njit  # type: ignore[misc]
 def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
     """

From 32e4dfac41101550cba12be0a8a3efebb5a5f993 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:37:12 +0100
Subject: [PATCH 040/217] docstring fix

---
 src/smsfusion/_v2.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 3a28eae4..6410e430 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -652,9 +652,9 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,), optional
-            Initial velocity change vector (sculling integral).
+            Velocity change vector (sculling integral).
         dtheta : array_like, shape (3,), optional
-            Initial attitude change vector (coning integral).
+            Attitude change vector (coning integral).
         degrees : bool, optional
             Specifies whether the unit of the attitude change vector, ``dtheta``,
             is degrees or radians. Defaults to radians.

From 137c2159c30c605950001bd0be3fd6027bc966fc Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:46:38 +0100
Subject: [PATCH 041/217] precalculate dvel_g_corr

---
 src/smsfusion/_v2.py | 36 ++++++++++++++++++++++++++++--------
 1 file changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 6410e430..d1100b93 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -395,6 +395,31 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
     return dhdx
 
 
+def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
+    """
+    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
+
+    Parameters
+    ----------
+    g : float
+        Gravitational acceleration in m/s^2.
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame in which the gravity vector is expressed.
+
+    Returns
+    -------
+    ndarray, shape (3,)
+        Gravity vector expressed in the navigation frame.
+    """
+    if nav_frame.lower() == "ned":
+        g_n = np.array([0.0, 0.0, g])
+    elif nav_frame.lower() == "enu":
+        g_n = np.array([0.0, 0.0, -g])
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+    return g_n
+
+
 class AHRSv2:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
@@ -460,13 +485,8 @@ def __init__(
         self._dt = 1.0 / fs
         self._g = g
         self._nav_frame = nav_frame.lower()
-
-        if self._nav_frame == "ned":
-            self._g_n = np.array([0.0, 0.0, g])
-        elif self._nav_frame == "enu":
-            self._g_n = np.array([0.0, 0.0, -g])
-        else:
-            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+        self._g_n = _gravity_nav(self._g, self._nav_frame)
+        self._dvel_g_corr = self._dt * self._g_n
 
         # IMU noise parameters
         self._vrw = acc_noise_density  # velocity random walk
@@ -627,7 +647,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         """
 
         # Velocity (dead reckoning)
-        self._v_n[:] += self._R_nb @ dvel + self._dt * self._g_n
+        self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
         _correct_quat_with_rotvec(self._q_nb, dtheta)

From c853029f4679a5b9198a493f885444d0e44a2982 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 16:48:15 +0100
Subject: [PATCH 042/217] delete commented out method

---
 src/smsfusion/_v2.py | 14 --------------
 1 file changed, 14 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index d1100b93..76a60045 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -556,20 +556,6 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
             bg_b = (180.0 / np.pi) * bg_b
         return bg_b
 
-    # def angular_rate(self, degrees=False) -> NDArray[np.float64]:
-    #     """
-    #     Bias corrected angular rate measurement expressed in the body frame.
-
-    #     Parameters
-    #     ----------
-    #     degrees : bool, optional
-    #         Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s.
-    #     """
-    #     w_b = self._w_b.copy()
-    #     if degrees:
-    #         w_b = (180.0 / np.pi) * w_b
-    #     return w_b
-
     @property
     def P(self) -> NDArray[np.float64]:
         """

From 9f116e9a845ba4a655e4595ec9dfa6f3ec15b2c3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 17:10:08 +0100
Subject: [PATCH 043/217] fix correct quat with rotvec

---
 src/smsfusion/_v2.py | 79 +++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 78 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 76a60045..7b7fe54c 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -104,6 +104,83 @@ def _correct_quat_with_rotvec(
     q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
 
 
+# @njit  # type: ignore[misc]
+# def _correct_quat_with_rotvec_new(
+#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
+# ) -> None:
+#     """
+#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+#     parameterized as a rotation vector.:
+
+#         q = q ⊗ dq(dtheta)
+
+#     Parameters
+#     ----------
+#     q : ndarray, shape (4,)
+#         Unit quaternion (modified in place).
+#     dtheta : ndarray, shape (3,)
+#         Small attitude change parameterized as a rotation vector.
+#     """
+
+#     rx, ry, rz = dtheta
+
+#     half_angle = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0
+
+#     if half_angle >= 1e-5:
+#         psi = dtheta * np.sin(half_angle) / (2.0 * half_angle)
+#     else:
+#         psi = 0.5 * dtheta
+
+#     cos_half_angle = np.cos(half_angle)
+
+#     qw_new = cos_half_angle * q[0] - psi @ q[1:]
+
+#     qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
+
+#     return np.array([qw_new, *qxyz_new])
+
+
+@njit  # type: ignore[misc]
+def _correct_quat_with_rotvec_new(
+    q: NDArray[np.float64], dtheta: NDArray[np.float64]
+) -> None:
+    """
+    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+    parameterized as a rotation vector.:
+
+        q = q ⊗ dq(dtheta)
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (modified in place).
+    dtheta : ndarray, shape (3,)
+        Small attitude change parameterized as a rotation vector.
+    """
+
+    rx, ry, rz = dtheta
+
+    norm = np.sqrt(rx**2 + ry**2 + rz**2)
+    half_angle = norm / 2.0
+
+    if half_angle >= 1e-5:
+        psi = dtheta * np.sin(half_angle) / norm
+    else:
+        psi = 0.5 * dtheta
+
+    cos_half_angle = np.cos(half_angle)
+
+    qw_new = cos_half_angle * q[0] - psi @ q[1:]
+
+    qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
+
+    q[0] = qw_new
+    q[1] = qxyz_new[0]
+    q[2] = qxyz_new[1]
+    q[3] = qxyz_new[2]
+
+
+
 @njit  # type: ignore[misc]
 def _kalman_gain(
     P: NDArray[np.float64], h: NDArray[np.float64], r: float
@@ -636,7 +713,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
-        _correct_quat_with_rotvec(self._q_nb, dtheta)
+        _correct_quat_with_rotvec_new(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From 9a9050a99f2b6a5a9655fabd319f92a463f72bae Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 17:18:57 +0100
Subject: [PATCH 044/217] black

---
 src/smsfusion/_v2.py | 84 +++++++++++++++++++++++++++++++++++---------
 1 file changed, 68 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 7b7fe54c..82d53852 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -140,45 +140,97 @@ def _correct_quat_with_rotvec(
 #     return np.array([qw_new, *qxyz_new])
 
 
+# @njit  # type: ignore[misc]
+# def _correct_quat_with_rotvec_new(
+#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
+# ) -> None:
+#     """
+#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+#     parameterized as a rotation vector.:
+
+#         q = q ⊗ dq(dtheta)
+
+#     Parameters
+#     ----------
+#     q : ndarray, shape (4,)
+#         Unit quaternion (modified in place).
+#     dtheta : ndarray, shape (3,)
+#         Small attitude change parameterized as a rotation vector.
+#     """
+
+#     rx, ry, rz = dtheta
+
+#     norm = np.sqrt(rx**2 + ry**2 + rz**2)
+#     half_angle = norm / 2.0
+
+#     if half_angle >= 1e-5:
+#         psi = dtheta * np.sin(half_angle) / norm
+#     else:
+#         psi = 0.5 * dtheta
+
+#     cos_half_angle = np.cos(half_angle)
+
+#     qw_new = cos_half_angle * q[0] - psi @ q[1:]
+
+#     qxyz_new = (
+#         psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
+#     )
+
+#     q[0] = qw_new
+#     q[1] = qxyz_new[0]
+#     q[2] = qxyz_new[1]
+#     q[3] = qxyz_new[2]
+
+
 @njit  # type: ignore[misc]
 def _correct_quat_with_rotvec_new(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
 ) -> None:
     """
     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-    parameterized as a rotation vector.:
+    parameterized as a rotation vector
 
-        q = q ⊗ dq(dtheta)
+        q_{k+1} = M(dtheta) @ q_k
 
     Parameters
     ----------
     q : ndarray, shape (4,)
-        Unit quaternion (modified in place).
+        Unit quaternion [w, x, y, z] (scalar first), modified in place.
     dtheta : ndarray, shape (3,)
-        Small attitude change parameterized as a rotation vector.
+        Delta-Theta rotation vector (radians).
+
+    References
+    ----------
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
     """
 
     rx, ry, rz = dtheta
 
-    norm = np.sqrt(rx**2 + ry**2 + rz**2)
-    half_angle = norm / 2.0
+    gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0
 
-    if half_angle >= 1e-5:
-        psi = dtheta * np.sin(half_angle) / norm
+    if gamma >= 1e-5:
+        psi = np.sin(gamma) / (2.0 * gamma) * dtheta
     else:
         psi = 0.5 * dtheta
 
-    cos_half_angle = np.cos(half_angle)
+    cos_gamma = np.cos(gamma)
+    p1, p2, p3 = psi
 
-    qw_new = cos_half_angle * q[0] - psi @ q[1:]
-
-    qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
+    qw_new = cos_gamma * q[0] - p1 * q[1] - p2 * q[2] - p3 * q[3]
+    qx_new = p1 * q[0] + cos_gamma * q[1] + p3 * q[2] - p2 * q[3]
+    qy_new = p2 * q[0] - p3 * q[1] + cos_gamma * q[2] + p1 * q[3]
+    qz_new = p3 * q[0] + p2 * q[1] - p1 * q[2] + cos_gamma * q[3]
 
     q[0] = qw_new
-    q[1] = qxyz_new[0]
-    q[2] = qxyz_new[1]
-    q[3] = qxyz_new[2]
-
+    q[1] = qx_new
+    q[2] = qy_new
+    q[3] = qz_new
+
+    norm = np.sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2)
+    q[0] /= norm
+    q[1] /= norm
+    q[2] /= norm
+    q[3] /= norm
 
 
 @njit  # type: ignore[misc]

From 9a6fee9e7c10bde50ead1d7a8551952cec94f431 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 17:36:24 +0100
Subject: [PATCH 045/217] some changes

---
 src/smsfusion/_v2.py | 31 ++++++++++++++-----------------
 1 file changed, 14 insertions(+), 17 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 82d53852..86111c8a 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -195,15 +195,21 @@ def _correct_quat_with_rotvec_new(
     Parameters
     ----------
     q : ndarray, shape (4,)
-        Unit quaternion [w, x, y, z] (scalar first), modified in place.
+        Unit quaternion.
     dtheta : ndarray, shape (3,)
-        Delta-Theta rotation vector (radians).
+        Rotation vector.
+
+    Returns
+    -------
+    ndarray, shape (4,)
+        Updated unit quaternion after applying the rotation vector correction.
 
     References
     ----------
     .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
     """
 
+    qw, qx, qy, qz = q
     rx, ry, rz = dtheta
 
     gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0
@@ -216,21 +222,12 @@ def _correct_quat_with_rotvec_new(
     cos_gamma = np.cos(gamma)
     p1, p2, p3 = psi
 
-    qw_new = cos_gamma * q[0] - p1 * q[1] - p2 * q[2] - p3 * q[3]
-    qx_new = p1 * q[0] + cos_gamma * q[1] + p3 * q[2] - p2 * q[3]
-    qy_new = p2 * q[0] - p3 * q[1] + cos_gamma * q[2] + p1 * q[3]
-    qz_new = p3 * q[0] + p2 * q[1] - p1 * q[2] + cos_gamma * q[3]
-
-    q[0] = qw_new
-    q[1] = qx_new
-    q[2] = qy_new
-    q[3] = qz_new
+    qw_new = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz
+    qx_new = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz
+    qy_new = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz
+    qz_new = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz
 
-    norm = np.sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2)
-    q[0] /= norm
-    q[1] /= norm
-    q[2] /= norm
-    q[3] /= norm
+    return _normalize(np.array((qw_new, qx_new, qy_new, qz_new)))
 
 
 @njit  # type: ignore[misc]
@@ -765,7 +762,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
-        _correct_quat_with_rotvec_new(self._q_nb, dtheta)
+        self._q_nb[:] = _correct_quat_with_rotvec_new(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From e63bdf8278989b0b1316b394314f4ad74b6c25c5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 13 Mar 2026 17:40:59 +0100
Subject: [PATCH 046/217] small fix

---
 src/smsfusion/_v2.py | 92 ++++++++++++++++++++++----------------------
 1 file changed, 46 insertions(+), 46 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 86111c8a..ff01d69d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -48,60 +48,60 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
     q[:] = _normalize(q)
 
 
-@njit  # type: ignore[misc]
-def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
-    """
-    Compute the unit quaternion from a rotation vector.
+# @njit  # type: ignore[misc]
+# def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
+#     """
+#     Compute the unit quaternion from a rotation vector.
 
-    Parameters
-    ----------
-    r : numpy.ndarray, shape (3,)
-        Rotation vector (rx, ry, rz).
+#     Parameters
+#     ----------
+#     r : numpy.ndarray, shape (3,)
+#         Rotation vector (rx, ry, rz).
 
-    Returns
-    -------
-    numpy.ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz).
-    """
-    # TODO: add reference
+#     Returns
+#     -------
+#     numpy.ndarray, shape (4,)
+#         Unit quaternion (qw, qx, qy, qz).
+#     """
+#     # TODO: add reference
 
-    rx, ry, rz = r
+#     rx, ry, rz = r
 
-    angle2 = rx**2 + ry**2 + rz**2
+#     angle2 = rx**2 + ry**2 + rz**2
 
-    if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
-        a = 0.25 * angle2
-        c = 1.0 - a / 2.0
-        s = 0.5 * (1.0 - a / 6.0)
-    else:
-        angle = np.sqrt(angle2)
-        half_angle = 0.5 * angle
-        c = np.cos(half_angle)
-        s = np.sin(half_angle) / angle
+#     if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
+#         a = 0.25 * angle2
+#         c = 1.0 - a / 2.0
+#         s = 0.5 * (1.0 - a / 6.0)
+#     else:
+#         angle = np.sqrt(angle2)
+#         half_angle = 0.5 * angle
+#         c = np.cos(half_angle)
+#         s = np.sin(half_angle) / angle
 
-    q = np.array([c, s * rx, s * ry, s * rz])
+#     q = np.array([c, s * rx, s * ry, s * rz])
 
-    return _normalize(q)
+#     return _normalize(q)
 
 
-@njit  # type: ignore[misc]
-def _correct_quat_with_rotvec(
-    q: NDArray[np.float64], dtheta: NDArray[np.float64]
-) -> None:
-    """
-    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-    parameterized as a rotation vector:
+# @njit  # type: ignore[misc]
+# def _correct_quat_with_rotvec(
+#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
+# ) -> None:
+#     """
+#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
+#     parameterized as a rotation vector:
 
-        q = q ⊗ dq(dtheta)
+#         q = q ⊗ dq(dtheta)
 
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (modified in place).
-    dtheta : ndarray, shape (3,)
-        Small attitude change parameterized as a rotation vector.
-    """
-    q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
+#     Parameters
+#     ----------
+#     q : ndarray, shape (4,)
+#         Unit quaternion (modified in place).
+#     dtheta : ndarray, shape (3,)
+#         Small attitude change parameterized as a rotation vector.
+#     """
+#     q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
 
 
 # @njit  # type: ignore[misc]
@@ -183,9 +183,9 @@ def _correct_quat_with_rotvec(
 
 
 @njit  # type: ignore[misc]
-def _correct_quat_with_rotvec_new(
+def _update_quaternion(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
-) -> None:
+) -> NDArray[np.float64]:
     """
     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
     parameterized as a rotation vector
@@ -762,7 +762,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
-        self._q_nb[:] = _correct_quat_with_rotvec_new(self._q_nb, dtheta)
+        self._q_nb[:] = _update_quaternion(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From 122a884267ddfd0a27866f6d41c8c499a3a19647 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 08:41:43 +0100
Subject: [PATCH 047/217] delete old commented out code

---
 src/smsfusion/_v2.py | 134 -------------------------------------------
 1 file changed, 134 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index ff01d69d..e0798d5e 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -48,140 +48,6 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
     q[:] = _normalize(q)
 
 
-# @njit  # type: ignore[misc]
-# def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]:
-#     """
-#     Compute the unit quaternion from a rotation vector.
-
-#     Parameters
-#     ----------
-#     r : numpy.ndarray, shape (3,)
-#         Rotation vector (rx, ry, rz).
-
-#     Returns
-#     -------
-#     numpy.ndarray, shape (4,)
-#         Unit quaternion (qw, qx, qy, qz).
-#     """
-#     # TODO: add reference
-
-#     rx, ry, rz = r
-
-#     angle2 = rx**2 + ry**2 + rz**2
-
-#     if angle2 < 1e-6:  # 2nd order approximation (avoids division by zero)
-#         a = 0.25 * angle2
-#         c = 1.0 - a / 2.0
-#         s = 0.5 * (1.0 - a / 6.0)
-#     else:
-#         angle = np.sqrt(angle2)
-#         half_angle = 0.5 * angle
-#         c = np.cos(half_angle)
-#         s = np.sin(half_angle) / angle
-
-#     q = np.array([c, s * rx, s * ry, s * rz])
-
-#     return _normalize(q)
-
-
-# @njit  # type: ignore[misc]
-# def _correct_quat_with_rotvec(
-#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
-# ) -> None:
-#     """
-#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-#     parameterized as a rotation vector:
-
-#         q = q ⊗ dq(dtheta)
-
-#     Parameters
-#     ----------
-#     q : ndarray, shape (4,)
-#         Unit quaternion (modified in place).
-#     dtheta : ndarray, shape (3,)
-#         Small attitude change parameterized as a rotation vector.
-#     """
-#     q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta)))
-
-
-# @njit  # type: ignore[misc]
-# def _correct_quat_with_rotvec_new(
-#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
-# ) -> None:
-#     """
-#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-#     parameterized as a rotation vector.:
-
-#         q = q ⊗ dq(dtheta)
-
-#     Parameters
-#     ----------
-#     q : ndarray, shape (4,)
-#         Unit quaternion (modified in place).
-#     dtheta : ndarray, shape (3,)
-#         Small attitude change parameterized as a rotation vector.
-#     """
-
-#     rx, ry, rz = dtheta
-
-#     half_angle = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0
-
-#     if half_angle >= 1e-5:
-#         psi = dtheta * np.sin(half_angle) / (2.0 * half_angle)
-#     else:
-#         psi = 0.5 * dtheta
-
-#     cos_half_angle = np.cos(half_angle)
-
-#     qw_new = cos_half_angle * q[0] - psi @ q[1:]
-
-#     qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
-
-#     return np.array([qw_new, *qxyz_new])
-
-
-# @njit  # type: ignore[misc]
-# def _correct_quat_with_rotvec_new(
-#     q: NDArray[np.float64], dtheta: NDArray[np.float64]
-# ) -> None:
-#     """
-#     Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-#     parameterized as a rotation vector.:
-
-#         q = q ⊗ dq(dtheta)
-
-#     Parameters
-#     ----------
-#     q : ndarray, shape (4,)
-#         Unit quaternion (modified in place).
-#     dtheta : ndarray, shape (3,)
-#         Small attitude change parameterized as a rotation vector.
-#     """
-
-#     rx, ry, rz = dtheta
-
-#     norm = np.sqrt(rx**2 + ry**2 + rz**2)
-#     half_angle = norm / 2.0
-
-#     if half_angle >= 1e-5:
-#         psi = dtheta * np.sin(half_angle) / norm
-#     else:
-#         psi = 0.5 * dtheta
-
-#     cos_half_angle = np.cos(half_angle)
-
-#     qw_new = cos_half_angle * q[0] - psi @ q[1:]
-
-#     qxyz_new = (
-#         psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:]
-#     )
-
-#     q[0] = qw_new
-#     q[1] = qxyz_new[0]
-#     q[2] = qxyz_new[1]
-#     q[3] = qxyz_new[2]
-
-
 @njit  # type: ignore[misc]
 def _update_quaternion(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]

From 6a900d3183c212ab56c788531163409a60b15462 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 08:50:13 +0100
Subject: [PATCH 048/217] few changes

---
 src/smsfusion/_v2.py | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index e0798d5e..3bb456f6 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -6,7 +6,7 @@
 
 from ._ins import _dhda_head, _h_head, _signed_smallest_angle
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
+from ._vectorops import _normalize, _skew_symmetric
 
 VEL_IDX = slice(0, 3)
 ATT_IDX = slice(3, 6)
@@ -14,12 +14,10 @@
 
 
 @njit  # type: ignore[misc]
-def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
+def _correct_quaternion_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
     """
     Corrects a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector:
-
-        q = q ⊗ dq(da)
+    as a scaled (2x) Gibbs vector.
 
     As described in ref [1]_, this correction can be simplified by doing it in two
     steps: first a correction, followed by renormalization. The scaling factor becomes
@@ -49,7 +47,7 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -
 
 
 @njit  # type: ignore[misc]
-def _update_quaternion(
+def _update_quaternion_with_rotvec(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
 ) -> NDArray[np.float64]:
     """
@@ -576,7 +574,7 @@ def _reset(self) -> None:
         if not self._dx.any():
             return
 
-        _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
+        _correct_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
         self._v_n[:] += self._dx[VEL_IDX]
         self._bg_b[:] += self._dx[BG_IDX]
         self._dx[:] = 0.0
@@ -628,7 +626,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
-        self._q_nb[:] = _update_quaternion(self._q_nb, dtheta)
+        self._q_nb[:] = _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From 2dc3856cac6ca355621cf4b6219cfccec457ab8f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 08:50:31 +0100
Subject: [PATCH 049/217] style

---
 src/smsfusion/_v2.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 3bb456f6..76ded857 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -14,7 +14,9 @@
 
 
 @njit  # type: ignore[misc]
-def _correct_quaternion_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None:
+def _correct_quaternion_with_gibbs2(
+    q: NDArray[np.float64], da: NDArray[np.float64]
+) -> None:
     """
     Corrects a unit quaternion, q, with a small attitude error, da, parameterized
     as a scaled (2x) Gibbs vector.

From 82ece316c0fa19b3d8dd1dd890ef84afa4523106 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 08:52:15 +0100
Subject: [PATCH 050/217] delete repeded code

---
 src/smsfusion/_v2.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 76ded857..b251ab5d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -493,7 +493,6 @@ def __init__(
         self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
         self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy()
         self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy()
-        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 

From 144f59ad693175cddc66ffb96cb1734c6621b140 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 08:54:57 +0100
Subject: [PATCH 051/217] store dvel_prev

---
 src/smsfusion/_v2.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index b251ab5d..06bc657a 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -691,6 +691,8 @@ def update(
         self._reset()
 
         # Update model
+        self._dvel_prev[:] = dvel
+        self._dtheta_prev[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
         _update_state_transition(self._phi, dvel, dtheta, self._R_nb)
 

From 3cbbef61fdcba66f839626d20db60c1240cc3e2b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 09:02:08 +0100
Subject: [PATCH 052/217] some changes

---
 src/smsfusion/_v2.py | 34 ++++++++++++++--------------------
 1 file changed, 14 insertions(+), 20 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 06bc657a..d21b2f3d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -428,10 +428,10 @@ class AHRSv2:
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
     bg_b : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    dvel_prev : array_like, shape (3,), optional
-        Previous velocity change vector measurement (sculling integral).
-    dtheta_prev : array_like, shape (3,), optional
-        Previous attitude change vector measurement (coning integral).
+    dvel : array_like, shape (3,), optional
+        Initial velocity change vector measurement (sculling integral).
+    dtheta : array_like, shape (3,), optional
+        Initial attitude change vector measurement (coning integral).
     P : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix, **P**. If not
         given, a small diagonal matrix will be used.
@@ -463,8 +463,8 @@ def __init__(
         v_n: ArrayLike = (0.0, 0.0, 0.0),
         q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg_b: ArrayLike = (0.0, 0.0, 0.0),
-        dvel_prev: ArrayLike = (0.0, 0.0, 0.0),
-        dtheta_prev: ArrayLike = (0.0, 0.0, 0.0),
+        dvel: ArrayLike = (0.0, 0.0, 0.0),
+        dtheta: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = 1e-6 * np.eye(9),
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
@@ -491,14 +491,14 @@ def __init__(
         self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
         self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
         self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
-        self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy()
-        self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy()
+        self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy()
+        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 
         # Discrete state-space model
         self._phi = _state_transition(
-            self._dt, self._dvel_prev, self._dtheta_prev, self._R_nb, self._gbc
+            self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc
         )
         self._Q = _process_noise_cov(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
@@ -672,16 +672,12 @@ def update(
             A reference to the instance itself after the update.
         """
 
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
-
-        if degrees:
-            dtheta = np.radians(dtheta)
-
-        dtheta -= self._dt * self._bg_b
+        self._dvel[:] = dvel
+        self._dtheta[:] = np.degrees(dtheta) if degrees else dtheta
+        self._dtheta -= self._dt * self._bg_b
 
         # Project (a priori) state and covariance estimates ahead
-        self._project_ahead(dvel, dtheta)
+        self._project_ahead(self._dvel, self._dtheta)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         self._aiding_update_vel(vel, vel_var)
@@ -691,9 +687,7 @@ def update(
         self._reset()
 
         # Update model
-        self._dvel_prev[:] = dvel
-        self._dtheta_prev[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        _update_state_transition(self._phi, dvel, dtheta, self._R_nb)
+        _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
 
         return self

From f672efcdc93498f215c4b2ba26c0f26f068bfea2 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 09:06:21 +0100
Subject: [PATCH 053/217] dvel and dtheta methods

---
 src/smsfusion/_v2.py | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index d21b2f3d..c104970d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -547,6 +547,27 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
             bg_b = (180.0 / np.pi) * bg_b
         return bg_b
 
+    def dvel(self) -> NDArray[np.float64]:
+        """
+        Previous velocity change vector measurement (sculling integral).
+        """
+        return self._dvel.copy()
+
+    def dtheta(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Previous attitude change vector measurement (coning integral).
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the coning integral in degrees or radians. Defaults
+            to radians.
+        """
+        dtheta = self._dtheta.copy()
+        if degrees:
+            dtheta = (180.0 / np.pi) * dtheta
+        return dtheta
+
     @property
     def P(self) -> NDArray[np.float64]:
         """

From 4c9ade026024315ffc22e1621f48d83c5a97b7f2 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 14 Mar 2026 09:14:08 +0100
Subject: [PATCH 054/217] renaming

---
 src/smsfusion/_v2.py | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index c104970d..0a320709 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -421,12 +421,12 @@ class AHRSv2:
     ----------
     fs : float
         Sampling rate in Hz.
-    v_n : array_like, shape (3,), optional
+    v : array_like, shape (3,), optional
         Initial velocity estimate in m/s.
-    q_nb : Attitude or array_like, shape (4,), optional
+    q : Attitude or array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg_b : array_like, shape (3,), optional
+    bg : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
     dvel : array_like, shape (3,), optional
         Initial velocity change vector measurement (sculling integral).
@@ -460,9 +460,9 @@ class AHRSv2:
     def __init__(
         self,
         fs: float,
-        v_n: ArrayLike = (0.0, 0.0, 0.0),
-        q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg_b: ArrayLike = (0.0, 0.0, 0.0),
+        v: ArrayLike = (0.0, 0.0, 0.0),
+        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg: ArrayLike = (0.0, 0.0, 0.0),
         dvel: ArrayLike = (0.0, 0.0, 0.0),
         dtheta: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = 1e-6 * np.eye(9),
@@ -487,10 +487,10 @@ def __init__(
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy()
-        self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy()
+        self._v_n = np.asarray_chkfinite(v).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
         self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy()
+        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
         self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy()
         self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()

From bfe3d18c476a559843ef7e41295fbccd35786192 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 08:00:01 +0100
Subject: [PATCH 055/217] small fix

---
 src/smsfusion/_v2.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 0a320709..2a653a84 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -41,10 +41,10 @@ def _correct_quaternion_with_gibbs2(
     qw, qx, qy, qz = q
     dax, day, daz = da
 
-    q[0] -= 0.5 * (qx * dax + qy * day + qz * daz)
-    q[1] += 0.5 * (qw * dax + qy * daz - qz * day)
-    q[2] += 0.5 * (qw * day - qx * daz + qz * dax)
-    q[3] += 0.5 * (qw * daz + qx * day - qy * dax)
+    q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
     q[:] = _normalize(q)
 
 

From 3fdbb0a281998a4bb94b5514ffeb2c0506a03936 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 08:19:55 +0100
Subject: [PATCH 056/217] some changes

---
 src/smsfusion/_v2.py | 53 ++++++++++++++++++++------------------------
 1 file changed, 24 insertions(+), 29 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 2a653a84..1225a0f6 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -14,11 +14,11 @@
 
 
 @njit  # type: ignore[misc]
-def _correct_quaternion_with_gibbs2(
+def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
 ) -> None:
     """
-    Corrects a unit quaternion, q, with a small attitude error, da, parameterized
+    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
     as a scaled (2x) Gibbs vector.
 
     As described in ref [1]_, this correction can be simplified by doing it in two
@@ -28,13 +28,13 @@ def _correct_quaternion_with_gibbs2(
     Parameters
     ----------
     q : ndarray, shape (4,)
-        Unit quaternion [qw, qx, qy, qz] (modified in place).
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
     da : ndarray, shape (3,)
-        Small attitude error parameterized as a scaled (2x) Gibbs vector.
+        Attitude error correction parameterized as a scaled (2x) Gibbs vector.
 
     References
     ----------
-    Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
     and Control, Eq. (6.27)-(6.28).
     """
 
@@ -53,22 +53,15 @@ def _update_quaternion_with_rotvec(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
 ) -> NDArray[np.float64]:
     """
-    Corrects a unit quaternion, q, with a small attitude change vector, dtheta,
-    parameterized as a rotation vector
-
-        q_{k+1} = M(dtheta) @ q_k
+    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
+    as a rotation vector.
 
     Parameters
     ----------
     q : ndarray, shape (4,)
-        Unit quaternion.
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
     dtheta : ndarray, shape (3,)
-        Rotation vector.
-
-    Returns
-    -------
-    ndarray, shape (4,)
-        Updated unit quaternion after applying the rotation vector correction.
+        Attitude increment (rotation vector).
 
     References
     ----------
@@ -78,22 +71,24 @@ def _update_quaternion_with_rotvec(
     qw, qx, qy, qz = q
     rx, ry, rz = dtheta
 
-    gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0
+    gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2)
+    cos_gamma = np.cos(gamma)
 
     if gamma >= 1e-5:
-        psi = np.sin(gamma) / (2.0 * gamma) * dtheta
+        scale = np.sin(gamma) / (2.0 * gamma)
     else:
-        psi = 0.5 * dtheta
-
-    cos_gamma = np.cos(gamma)
-    p1, p2, p3 = psi
+        scale = 0.5
 
-    qw_new = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz
-    qx_new = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz
-    qy_new = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz
-    qz_new = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz
+    # Psi
+    p1 = scale * rx
+    p2 = scale * ry
+    p3 = scale * rz
 
-    return _normalize(np.array((qw_new, qx_new, qy_new, qz_new)))
+    q[0] = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz
+    q[1] = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz
+    q[2] = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz
+    q[3] = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz
+    q[:] = _normalize(q)
 
 
 @njit  # type: ignore[misc]
@@ -596,7 +591,7 @@ def _reset(self) -> None:
         if not self._dx.any():
             return
 
-        _correct_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
+        _update_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
         self._v_n[:] += self._dx[VEL_IDX]
         self._bg_b[:] += self._dx[BG_IDX]
         self._dx[:] = 0.0
@@ -648,7 +643,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
 
         # Attitude (dead reckoning)
-        self._q_nb[:] = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+        _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Covariance
         self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)

From ecb87fff55f8d78b9bc577a1e1f76a50d7e4a2eb Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 08:28:57 +0100
Subject: [PATCH 057/217] small fix

---
 src/smsfusion/_v2.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 1225a0f6..a88502ef 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -232,7 +232,7 @@ def _project_cov_ahead(
     Parameters
     ----------
     P : ndarray, shape (n, n)
-        State error covariance matrix to be projected ahead.
+        State error covariance matrix to be projected ahead (in place).
     phi : ndarray, shape (n, n)
         State transition matrix.
     Q : ndarray, shape (n, n)
@@ -243,8 +243,7 @@ def _project_cov_ahead(
     ndarray, shape (n, n)
         Projected error covariance matrix estimate.
     """
-    P = phi @ P @ phi.T + Q
-    return P
+    P[:, :] = phi @ P @ phi.T + Q
 
 
 def _state_transition(
@@ -646,7 +645,7 @@ def _project_ahead(self, dvel, dtheta) -> None:
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Covariance
-        self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q)
+        _project_cov_ahead(self._P, self._phi, self._Q)
 
     def update(
         self,

From 42a31695ff9ef1915774e93804aeb00099d84f91 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 08:40:08 +0100
Subject: [PATCH 058/217] remove idx slices

---
 src/smsfusion/_v2.py | 30 +++++++++++++-----------------
 1 file changed, 13 insertions(+), 17 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index a88502ef..9062b3e4 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -8,10 +8,6 @@
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
-VEL_IDX = slice(0, 3)
-ATT_IDX = slice(3, 6)
-BG_IDX = slice(6, 9)
-
 
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
@@ -275,10 +271,10 @@ def _state_transition(
         State transition matrix.
     """
     phi = np.eye(9)
-    phi[VEL_IDX, ATT_IDX] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
-    phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta)  # NB! update each time step
-    phi[ATT_IDX, BG_IDX] -= dt * np.eye(3)
-    phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc
+    phi[0:3, 3:6] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
+    phi[3:6, 3:6] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[3:6, 6:9] -= dt * np.eye(3)
+    phi[6:9, 6:9] -= dt * np.eye(3) / gbc
     return phi
 
 
@@ -355,9 +351,9 @@ def _process_noise_cov(
         Process noise covariance matrix.
     """
     Q = np.zeros((9, 9))
-    Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3)
-    Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3)
-    Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    Q[0:3, 0:3] = dt * vrw**2 * np.eye(3)
+    Q[3:6, 3:6] = dt * arw**2 * np.eye(3)
+    Q[6:9, 6:9] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
     return Q
 
 
@@ -376,8 +372,8 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
         Linearized measurement matrix.
     """
     dhdx = np.zeros((4, 9))
-    dhdx[0:3, VEL_IDX] = np.eye(3)  # velocity
-    dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)  # heading
+    dhdx[0:3, 0:3] = np.eye(3)  # velocity
+    dhdx[3:4, 3:6] = _dhda_head(q_nb)  # heading
     return dhdx
 
 
@@ -579,7 +575,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
         """
         Heading (yaw angle) part of the measurement matrix, shape (6,).
         """
-        self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb)
+        self._dhdx[3:4, 3:6] = _dhda_head(q_nb)
         return self._dhdx[3]
 
     def _reset(self) -> None:
@@ -590,9 +586,9 @@ def _reset(self) -> None:
         if not self._dx.any():
             return
 
-        _update_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX])
-        self._v_n[:] += self._dx[VEL_IDX]
-        self._bg_b[:] += self._dx[BG_IDX]
+        _update_quaternion_with_gibbs2(self._q_nb, self._dx[3:6])
+        self._v_n[:] += self._dx[0:3]
+        self._bg_b[:] += self._dx[6:9]
         self._dx[:] = 0.0
 
     def _aiding_update_vel(

From b5d96e8c91fc70041e04f74de5ecd6d957083e8d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 09:16:02 +0100
Subject: [PATCH 059/217] docstring fix

---
 src/smsfusion/_v2.py | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 9062b3e4..cc1a4f09 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -426,19 +426,20 @@ class AHRSv2:
         Initial (a priori) estimate of the error covariance matrix, **P**. If not
         given, a small diagonal matrix will be used.
     acc_noise_density : float, optional
-        Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to
-        0.0007 (SMS Motion 2 noise level).
+        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
+        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
     gyro_noise_density : float, optional
         Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (SMS Motion 2 noise level).
+        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
     gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level).
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
+        noise level).
     gyro_bias_corr_time : float, optional
         Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    g : float, default 9.80665
-        The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665
-        m/s^2.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
+    g : float, optional
+        The gravitational acceleration in m/s^2. Default is 'standard gravity' of
+        9.80665 m/s^2.
+    nav_frame : {'NED', 'ENU'}, optional
         Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
         (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
         will be expressed relative to this frame.

From ba368d10310d23e8bfa7bef084ca411e09d2fd6d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 16 Mar 2026 12:08:01 +0100
Subject: [PATCH 060/217] small change

---
 src/smsfusion/_v2.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index cc1a4f09..70fa23b3 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -685,7 +685,11 @@ def update(
         """
 
         self._dvel[:] = dvel
-        self._dtheta[:] = np.degrees(dtheta) if degrees else dtheta
+        self._dtheta[:] = dtheta
+
+        if degrees:
+            self._dtheta *= np.pi / 180.0
+
         self._dtheta -= self._dt * self._bg_b
 
         # Project (a priori) state and covariance estimates ahead

From 8f3ff975efaf5bd0456b3d9a80cbbeccb9f4b47a Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 17 Mar 2026 14:57:26 +0100
Subject: [PATCH 061/217] psi xyz

---
 src/smsfusion/_v2.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 70fa23b3..8400e6e7 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -76,14 +76,14 @@ def _update_quaternion_with_rotvec(
         scale = 0.5
 
     # Psi
-    p1 = scale * rx
-    p2 = scale * ry
-    p3 = scale * rz
-
-    q[0] = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz
-    q[1] = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz
-    q[2] = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz
-    q[3] = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz
+    px = scale * rx
+    py = scale * ry
+    pz = scale * rz
+
+    q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz
+    q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz
+    q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
+    q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
     q[:] = _normalize(q)
 
 

From ea457c6cb48039a76e5af9a4da0d6b8bd5519e5f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 09:40:13 +0100
Subject: [PATCH 062/217] velocity method

---
 src/smsfusion/_v2.py | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 8400e6e7..81f17cd7 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -501,6 +501,12 @@ def quaternion(self) -> NDArray[np.float64]:
         Attitude expressed as a unit quaternion.
         """
         return self._q_nb.copy()
+    
+    def velocity(self) -> NDArray[np.float64]:
+        """
+        Velocity expressed in the navigation frame.
+        """
+        return self._v_n.copy()
 
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """

From 4618eb6244ac534c7c26f1c90bbde82a5f280be3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 09:40:19 +0100
Subject: [PATCH 063/217] test v2

---
 tests/test_v2.py | 120 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 120 insertions(+)
 create mode 100644 tests/test_v2.py

diff --git a/tests/test_v2.py b/tests/test_v2.py
new file mode 100644
index 00000000..e79dd048
--- /dev/null
+++ b/tests/test_v2.py
@@ -0,0 +1,120 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion import AHRSv2
+from smsfusion.benchmark import (
+    benchmark_full_pva_beat_202311A,
+    benchmark_full_pva_chirp_202311A,
+)
+
+
+class Test_v2:
+    @pytest.mark.parametrize(
+        "benchmark_gen",
+        [benchmark_full_pva_beat_202311A], #, benchmark_full_pva_chirp_202311A],
+    )
+    def test_benchmark(self, benchmark_gen):
+        fs_imu = 100.0
+        fs_aiding = 1.0
+        fs_ratio = np.ceil(fs_imu / fs_aiding)
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+        compass_noise_std = 0.5
+        vel_noise_std = 0.1
+
+        # Reference signals (without noise)
+        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU measurements (with noise)
+        err_acc_true = sf.constants.ERR_ACC_MOTION2
+        err_gyro_true = sf.constants.ERR_GYRO_MOTION2
+        bg = np.array([0.01, -0.02, 0.015])
+        noise_model = sf.noise.IMUNoise(
+            err_acc=err_acc_true, err_gyro=err_gyro_true, seed=0
+        )
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_noise = acc_ref + imu_noise[:, :3]
+        gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+        # Compass / heading (aiding) measurements
+        head_meas = euler_ref[:, 2] + sf.noise.white_noise(
+            compass_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=1
+        )
+
+        # Velocity (aiding) measurements
+        vel_noise = np.column_stack(
+            [
+                sf.noise.white_noise(
+                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=5
+                ),
+                sf.noise.white_noise(
+                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=6
+                ),
+                sf.noise.white_noise(
+                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=7
+                ),
+            ]
+        )
+        vel_meas = vel_ref + vel_noise
+
+        # MEKF
+        v0 = vel_ref[0]
+        q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+        mekf = AHRSv2(
+            fs_imu,
+            v=v0,
+            q=q0,
+            acc_noise_density=sf.constants.ERR_ACC_MOTION2["N"],
+            gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+            gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+            gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+        )
+
+        # Apply filter
+        vel_out, euler_out, bias_gyro_out = [], [], []
+        for i, (f_i, w_i, v_i, h_i) in enumerate(
+            zip(acc_noise, gyro_noise, vel_meas, head_meas)
+        ):
+            if not (i % fs_ratio):  # with aiding
+                mekf.update(
+                    f_i / fs_imu,
+                    w_i / fs_imu,
+                    degrees=False,
+                    vel=v_i,
+                    vel_var=vel_noise_std**2 * np.ones(3),
+                    head=h_i,
+                    head_var=compass_noise_std**2,
+                    head_degrees=True,
+                )
+            else:  # without aiding
+                mekf.update(f_i / fs_imu, w_i / fs_imu, degrees=False)
+            vel_out.append(mekf.velocity())
+            euler_out.append(mekf.euler(degrees=False))
+            bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+        vel_out = np.array(vel_out)
+        euler_out = np.array(euler_out)
+        bias_gyro_out = np.array(bias_gyro_out)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        vel_out = resample_poly(vel_out, 2, 1)[1:-1:2]
+        vel_ref = vel_ref[:-1, :]
+        euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+
+        vel_x_rms, vel_y_rms, vel_z_rms = np.std((vel_out - vel_ref)[warmup:], axis=0)
+        roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+        bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+            (bias_gyro_out - bg)[warmup:], axis=0
+        )
+
+        assert vel_x_rms <= 0.05
+        assert vel_y_rms <= 0.05
+        assert vel_z_rms <= 0.05
+        assert np.degrees(roll_rms) <= 0.02
+        assert np.degrees(pitch_rms) <= 0.02
+        assert np.degrees(yaw_rms) <= 0.1
+        assert np.degrees(bias_gyro_x_rms) <= 1e-3
+        assert np.degrees(bias_gyro_y_rms) <= 1e-3
+        assert np.degrees(bias_gyro_z_rms) <= 1e-3

From 4eab3e03bf7f339837870b6fb46abae2f72bdbef Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 09:43:53 +0100
Subject: [PATCH 064/217] adjust tolerance test

---
 src/smsfusion/_v2.py |  4 ++--
 tests/test_v2.py     | 12 ++++++------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 81f17cd7..0b9e5f1d 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -667,9 +667,9 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,), optional
-            Velocity change vector (sculling integral).
+            Velocity change vector (sculling integral) in m/s.
         dtheta : array_like, shape (3,), optional
-            Attitude change vector (coning integral).
+            Attitude change vector (coning integral) in radians.
         degrees : bool, optional
             Specifies whether the unit of the attitude change vector, ``dtheta``,
             is degrees or radians. Defaults to radians.
diff --git a/tests/test_v2.py b/tests/test_v2.py
index e79dd048..55a39d8f 100644
--- a/tests/test_v2.py
+++ b/tests/test_v2.py
@@ -112,9 +112,9 @@ def test_benchmark(self, benchmark_gen):
         assert vel_x_rms <= 0.05
         assert vel_y_rms <= 0.05
         assert vel_z_rms <= 0.05
-        assert np.degrees(roll_rms) <= 0.02
-        assert np.degrees(pitch_rms) <= 0.02
-        assert np.degrees(yaw_rms) <= 0.1
-        assert np.degrees(bias_gyro_x_rms) <= 1e-3
-        assert np.degrees(bias_gyro_y_rms) <= 1e-3
-        assert np.degrees(bias_gyro_z_rms) <= 1e-3
+        assert np.degrees(roll_rms) <= 0.1
+        assert np.degrees(pitch_rms) <= 0.1
+        assert np.degrees(yaw_rms) <= 0.2
+        assert np.degrees(bias_gyro_x_rms) <= 0.005
+        assert np.degrees(bias_gyro_y_rms) <= 0.005
+        assert np.degrees(bias_gyro_z_rms) <= 0.005

From 53ef2d15d7f315fc6f2e95504f4b22f7e104b970 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 09:51:06 +0100
Subject: [PATCH 065/217] test fix head aiding

---
 src/smsfusion/_v2.py |  2 +-
 tests/test_v2.py     | 44 +++++++++++++++++++-------------------------
 2 files changed, 20 insertions(+), 26 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index 0b9e5f1d..a2483e14 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -501,7 +501,7 @@ def quaternion(self) -> NDArray[np.float64]:
         Attitude expressed as a unit quaternion.
         """
         return self._q_nb.copy()
-    
+
     def velocity(self) -> NDArray[np.float64]:
         """
         Velocity expressed in the navigation frame.
diff --git a/tests/test_v2.py b/tests/test_v2.py
index 55a39d8f..0a7ee862 100644
--- a/tests/test_v2.py
+++ b/tests/test_v2.py
@@ -13,50 +13,34 @@
 class Test_v2:
     @pytest.mark.parametrize(
         "benchmark_gen",
-        [benchmark_full_pva_beat_202311A], #, benchmark_full_pva_chirp_202311A],
+        [benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A],
     )
     def test_benchmark(self, benchmark_gen):
         fs_imu = 100.0
         fs_aiding = 1.0
         fs_ratio = np.ceil(fs_imu / fs_aiding)
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-        compass_noise_std = 0.5
+        compass_noise_std = np.radians(0.5)
         vel_noise_std = 0.1
 
         # Reference signals (without noise)
         t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
         # IMU measurements (with noise)
-        err_acc_true = sf.constants.ERR_ACC_MOTION2
-        err_gyro_true = sf.constants.ERR_GYRO_MOTION2
         bg = np.array([0.01, -0.02, 0.015])
         noise_model = sf.noise.IMUNoise(
-            err_acc=err_acc_true, err_gyro=err_gyro_true, seed=0
+            err_acc=sf.constants.ERR_ACC_MOTION2, err_gyro=sf.constants.ERR_GYRO_MOTION2, seed=0
         )
         imu_noise = noise_model(fs_imu, len(t))
         acc_noise = acc_ref + imu_noise[:, :3]
         gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
 
-        # Compass / heading (aiding) measurements
-        head_meas = euler_ref[:, 2] + sf.noise.white_noise(
-            compass_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=1
+        # Aiding measurements (with noise)
+        rng = np.random.default_rng(seed=42)
+        head_meas = euler_ref[:, 2] + compass_noise_std * rng.standard_normal(
+            euler_ref.shape[0]
         )
-
-        # Velocity (aiding) measurements
-        vel_noise = np.column_stack(
-            [
-                sf.noise.white_noise(
-                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=5
-                ),
-                sf.noise.white_noise(
-                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=6
-                ),
-                sf.noise.white_noise(
-                    vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=7
-                ),
-            ]
-        )
-        vel_meas = vel_ref + vel_noise
+        vel_meas = vel_ref + vel_noise_std * rng.standard_normal(vel_ref.shape)
 
         # MEKF
         v0 = vel_ref[0]
@@ -76,6 +60,16 @@ def test_benchmark(self, benchmark_gen):
         for i, (f_i, w_i, v_i, h_i) in enumerate(
             zip(acc_noise, gyro_noise, vel_meas, head_meas)
         ):
+            # mekf.update(
+            #     f_i / fs_imu,
+            #     w_i / fs_imu,
+            #     degrees=False,
+            #     vel=v_i,
+            #     vel_var=vel_noise_std**2 * np.ones(3),
+            #     head=h_i,
+            #     head_var=compass_noise_std**2,
+            #     head_degrees=False,
+            # )
             if not (i % fs_ratio):  # with aiding
                 mekf.update(
                     f_i / fs_imu,
@@ -85,7 +79,7 @@ def test_benchmark(self, benchmark_gen):
                     vel_var=vel_noise_std**2 * np.ones(3),
                     head=h_i,
                     head_var=compass_noise_std**2,
-                    head_degrees=True,
+                    head_degrees=False,
                 )
             else:  # without aiding
                 mekf.update(f_i / fs_imu, w_i / fs_imu, degrees=False)

From 6471514681b788530db9b4352cde12ab311c7065 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 14:28:49 +0100
Subject: [PATCH 066/217] docstring

---
 src/smsfusion/_v2.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py
index a2483e14..fb69610a 100644
--- a/src/smsfusion/_v2.py
+++ b/src/smsfusion/_v2.py
@@ -412,19 +412,19 @@ class AHRSv2:
     fs : float
         Sampling rate in Hz.
     v : array_like, shape (3,), optional
-        Initial velocity estimate in m/s.
+        Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
     q : Attitude or array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
     bg : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
     dvel : array_like, shape (3,), optional
-        Initial velocity change vector measurement (sculling integral).
+        Initial velocity change vector measurement (sculling integral). Defaults to zero.
     dtheta : array_like, shape (3,), optional
-        Initial attitude change vector measurement (coning integral).
+        Initial attitude change vector measurement (coning integral). Defaults to zero.
     P : array_like, shape (6, 6), optional
-        Initial (a priori) estimate of the error covariance matrix, **P**. If not
-        given, a small diagonal matrix will be used.
+        Initial (a priori) estimate of the error covariance matrix. Defaults to
+        a small diagonal matrix (1e-6 * np.eye(9)).
     acc_noise_density : float, optional
         Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
         0.0007 (m/s)/√Hz (SMS Motion 2 noise level).

From f8ec4fb1ac37a9c6f966011ee088a26046452800 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 15:14:21 +0100
Subject: [PATCH 067/217] rename module to ins v2

---
 src/smsfusion/__init__.py            | 2 +-
 src/smsfusion/{_v2.py => _ins_v2.py} | 0
 tests/{test_v2.py => test_ins_v2.py} | 0
 3 files changed, 1 insertion(+), 1 deletion(-)
 rename src/smsfusion/{_v2.py => _ins_v2.py} (100%)
 rename tests/{test_v2.py => test_ins_v2.py} (100%)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 6dd89429..35b6b599 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -3,7 +3,7 @@
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._v2 import AHRSv2
+from ._ins_v2 import AHRSv2
 
 __all__ = [
     "AHRS",
diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_ins_v2.py
similarity index 100%
rename from src/smsfusion/_v2.py
rename to src/smsfusion/_ins_v2.py
diff --git a/tests/test_v2.py b/tests/test_ins_v2.py
similarity index 100%
rename from tests/test_v2.py
rename to tests/test_ins_v2.py

From 35da43726f7e2e52cbc0a3c6bdd11c004621c458 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 15:21:32 +0100
Subject: [PATCH 068/217] remove eye prealloc

---
 src/smsfusion/_ins_v2.py | 21 +++++----------------
 1 file changed, 5 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index fb69610a..4156a9d3 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -125,7 +125,6 @@ def _covariance_update(
     k: NDArray[np.float64],
     h: NDArray[np.float64],
     r: float,
-    I_: NDArray[np.float64],
 ) -> None:
     """
     Compute the updated state error covariance matrix estimate (Joseph form).
@@ -140,10 +139,8 @@ def _covariance_update(
         Measurement matrix (row vector).
     r : float
         Scalar measurement noise variance.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
     """
-    A = I_ - np.outer(k, h)
+    A = np.eye(k.size) - np.outer(k, h)
     P = A @ P @ A.T + r * np.outer(k, k)
     return P
 
@@ -155,7 +152,6 @@ def _kalman_update_scalar(
     z: float,
     r: float,
     h: NDArray[np.float64],
-    I_: NDArray[np.float64],
 ) -> None:
     """
     Scalar Kalman filter measurement update.
@@ -172,8 +168,6 @@ def _kalman_update_scalar(
         Scalar measurement noise variance.
     h : ndarray, shape (n,)
         Measurement matrix (row vector).
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
     """
 
     # Kalman gain
@@ -183,7 +177,7 @@ def _kalman_update_scalar(
     x[:] += k * (z - np.dot(h, x))
 
     # Updated (a posteriori) covariance estimate (Joseph form)
-    P[:, :] = _covariance_update(P, k, h, r, I_)
+    P[:, :] = _covariance_update(P, k, h, r)
 
 
 @njit  # type: ignore[misc]
@@ -193,7 +187,6 @@ def _kalman_update_sequential(
     z: NDArray[np.float64],
     var: NDArray[np.float64],
     H: NDArray[np.float64],
-    I_: NDArray[np.float64],
 ) -> None:
     """
     Sequential (one-at-a-time) Kalman filter measurement update.
@@ -210,12 +203,10 @@ def _kalman_update_sequential(
         Measurement noise variances corresponding to each scalar measurement.
     H : ndarray, shape (m, n)
         Measurement matrix where each row corresponds to a scalar measurement model.
-    I_ : ndarray, shape (n, n)
-        Identity matrix.
     """
     m = z.shape[0]
     for i in range(m):
-        _kalman_update_scalar(x, P, z[i], var[i], H[i], I_)
+        _kalman_update_scalar(x, P, z[i], var[i], H[i])
 
 
 @njit  # type: ignore[misc]
@@ -446,8 +437,6 @@ class AHRSv2:
 
     """
 
-    _I: NDArray[np.float64] = np.eye(9)
-
     def __init__(
         self,
         fs: float,
@@ -613,7 +602,7 @@ def _aiding_update_vel(
 
         dz = vel_meas - self._v_n
         dhdx = self._dhdx_vel()
-        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I)
+        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx)
 
     def _aiding_update_head(
         self, head_meas: float | None, head_var: float | None, head_degrees: bool
@@ -634,7 +623,7 @@ def _aiding_update_head(
 
         dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
         dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx)
 
     def _project_ahead(self, dvel, dtheta) -> None:
         """

From 63ec7cfa747f65a30e5867d2ed0f8e1a2a96cd6d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 15:22:29 +0100
Subject: [PATCH 069/217] docstring fix

---
 src/smsfusion/_ins_v2.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 4156a9d3..7d1b1c5d 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -224,11 +224,6 @@ def _project_cov_ahead(
         State transition matrix.
     Q : ndarray, shape (n, n)
         Process noise covariance matrix.
-
-    Returns
-    -------
-    ndarray, shape (n, n)
-        Projected error covariance matrix estimate.
     """
     P[:, :] = phi @ P @ phi.T + Q
 

From f246bd9e39d239d563c12e2ac56d04d2f282317f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 19 Mar 2026 15:46:28 +0100
Subject: [PATCH 070/217] docstring

---
 src/smsfusion/_ins_v2.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 7d1b1c5d..99bb0df4 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -125,7 +125,7 @@ def _covariance_update(
     k: NDArray[np.float64],
     h: NDArray[np.float64],
     r: float,
-) -> None:
+) -> NDArray[np.float64]:
     """
     Compute the updated state error covariance matrix estimate (Joseph form).
 
@@ -139,6 +139,11 @@ def _covariance_update(
         Measurement matrix (row vector).
     r : float
         Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Updated state error covariance matrix.
     """
     A = np.eye(k.size) - np.outer(k, h)
     P = A @ P @ A.T + r * np.outer(k, k)

From f27053b1c0c7378f82ebed35d29c1416cf679cfe Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 20 Mar 2026 09:44:55 +0100
Subject: [PATCH 071/217] small fix

---
 src/smsfusion/_ins_v2.py | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 99bb0df4..887413de 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -679,16 +679,16 @@ def update(
             A reference to the instance itself after the update.
         """
 
-        self._dvel[:] = dvel
-        self._dtheta[:] = dtheta
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
 
         if degrees:
-            self._dtheta *= np.pi / 180.0
+            dtheta = np.radians(dtheta)
 
-        self._dtheta -= self._dt * self._bg_b
+        dtheta = dtheta - self._dt * self._bg_b
 
         # Project (a priori) state and covariance estimates ahead
-        self._project_ahead(self._dvel, self._dtheta)
+        self._project_ahead(dvel, dtheta)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         self._aiding_update_vel(vel, vel_var)
@@ -698,6 +698,8 @@ def update(
         self._reset()
 
         # Update model
+        self._dvel[:] = dvel
+        self._dtheta[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
         _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
 

From edcdc01156e0ab15acf4323746c6a157404d79b7 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 25 Mar 2026 20:22:35 +0100
Subject: [PATCH 072/217] check if aid is None

---
 src/smsfusion/_ins_v2.py | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 887413de..1c85eba5 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -594,9 +594,6 @@ def _aiding_update_vel(
         Update with velocity aiding measurement.
         """
 
-        if vel_meas is None:
-            return None
-
         if vel_var is None:
             raise ValueError("'vg_var' not provided.")
 
@@ -611,9 +608,6 @@ def _aiding_update_head(
         Update with heading aiding measurement.
         """
 
-        if head_meas is None:
-            return None
-
         if head_var is None:
             raise ValueError("'head_var' not provided.")
 
@@ -691,8 +685,10 @@ def update(
         self._project_ahead(dvel, dtheta)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
-        self._aiding_update_vel(vel, vel_var)
-        self._aiding_update_head(head, head_var, head_degrees)
+        if vel is not None:
+            self._aiding_update_vel(vel, vel_var)
+        if head is not None:
+            self._aiding_update_head(head, head_var, head_degrees)
 
         # Reset state
         self._reset()

From 3bc96d7cf8d6f884d571d52a40dea32fa938c1c4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 26 Mar 2026 11:27:00 +0100
Subject: [PATCH 073/217] remove dhdx methods

---
 src/smsfusion/__init__.py |  2 +-
 src/smsfusion/_ins_v2.py  | 21 ++++-----------------
 tests/test_ins_v2.py      |  4 +++-
 3 files changed, 8 insertions(+), 19 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 35b6b599..de008f93 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,9 +1,9 @@
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
+from ._ins_v2 import AHRSv2
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from ._ins_v2 import AHRSv2
 
 __all__ = [
     "AHRS",
diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 1c85eba5..604491e8 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -561,19 +561,6 @@ def P(self) -> NDArray[np.float64]:
         """
         return self._P.copy()
 
-    def _dhdx_vel(self) -> NDArray[np.float64]:
-        """
-        Velocity part of the measurement matrix, shape (3, 6).
-        """
-        return self._dhdx[0:3]
-
-    def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-        """
-        Heading (yaw angle) part of the measurement matrix, shape (6,).
-        """
-        self._dhdx[3:4, 3:6] = _dhda_head(q_nb)
-        return self._dhdx[3]
-
     def _reset(self) -> None:
         """
         Reset state.
@@ -598,8 +585,7 @@ def _aiding_update_vel(
             raise ValueError("'vg_var' not provided.")
 
         dz = vel_meas - self._v_n
-        dhdx = self._dhdx_vel()
-        _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx)
+        _kalman_update_sequential(self._dx, self._P, dz, vel_var, self._dhdx[0:3])
 
     def _aiding_update_head(
         self, head_meas: float | None, head_var: float | None, head_degrees: bool
@@ -615,9 +601,10 @@ def _aiding_update_head(
             head_meas = (np.pi / 180.0) * head_meas
             head_var = (np.pi / 180.0) ** 2 * head_var
 
+        self._dhdx[3, 3:6] = _dhda_head(self._q_nb)
+
         dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
-        dhdx = self._dhdx_yaw(self._q_nb)
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx)
+        _kalman_update_scalar(self._dx, self._P, dz, head_var, self._dhdx[3])
 
     def _project_ahead(self, dvel, dtheta) -> None:
         """
diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py
index 0a7ee862..a6f7d54a 100644
--- a/tests/test_ins_v2.py
+++ b/tests/test_ins_v2.py
@@ -29,7 +29,9 @@ def test_benchmark(self, benchmark_gen):
         # IMU measurements (with noise)
         bg = np.array([0.01, -0.02, 0.015])
         noise_model = sf.noise.IMUNoise(
-            err_acc=sf.constants.ERR_ACC_MOTION2, err_gyro=sf.constants.ERR_GYRO_MOTION2, seed=0
+            err_acc=sf.constants.ERR_ACC_MOTION2,
+            err_gyro=sf.constants.ERR_GYRO_MOTION2,
+            seed=0,
         )
         imu_noise = noise_model(fs_imu, len(t))
         acc_noise = acc_ref + imu_noise[:, :3]

From 40473d76b3806cf41c39b2f08aeb8758475fb8dd Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 26 Mar 2026 14:22:30 +0100
Subject: [PATCH 074/217] hide v2

---
 src/smsfusion/__init__.py | 3 +--
 tests/test_ins_v2.py      | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index de008f93..4e91acfa 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,13 +1,12 @@
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
-from ._ins_v2 import AHRSv2
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
+from . import _ins_v2  # hidden
 
 __all__ = [
     "AHRS",
-    "AHRSv2",
     "AidedINS",
     "benchmark",
     "constants",
diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py
index a6f7d54a..3dcd7586 100644
--- a/tests/test_ins_v2.py
+++ b/tests/test_ins_v2.py
@@ -3,7 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion import AHRSv2
+from smsfusion._ins_v2 import AHRSv2
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
     benchmark_full_pva_chirp_202311A,

From d531b5b3f51f607a9fecf970710182b44326945d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 26 Mar 2026 14:22:51 +0100
Subject: [PATCH 075/217] delete commented out code in test

---
 tests/test_ins_v2.py | 10 ----------
 1 file changed, 10 deletions(-)

diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py
index 3dcd7586..a21ddc75 100644
--- a/tests/test_ins_v2.py
+++ b/tests/test_ins_v2.py
@@ -62,16 +62,6 @@ def test_benchmark(self, benchmark_gen):
         for i, (f_i, w_i, v_i, h_i) in enumerate(
             zip(acc_noise, gyro_noise, vel_meas, head_meas)
         ):
-            # mekf.update(
-            #     f_i / fs_imu,
-            #     w_i / fs_imu,
-            #     degrees=False,
-            #     vel=v_i,
-            #     vel_var=vel_noise_std**2 * np.ones(3),
-            #     head=h_i,
-            #     head_var=compass_noise_std**2,
-            #     head_degrees=False,
-            # )
             if not (i % fs_ratio):  # with aiding
                 mekf.update(
                     f_i / fs_imu,

From a137e1817e28631952ad0b098ae40c055dc70a1f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 11:02:11 +0100
Subject: [PATCH 076/217] raise fix

---
 src/smsfusion/_ins_v2.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 604491e8..c20a354f 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -582,7 +582,7 @@ def _aiding_update_vel(
         """
 
         if vel_var is None:
-            raise ValueError("'vg_var' not provided.")
+            raise ValueError("'vel_var' not provided.")
 
         dz = vel_meas - self._v_n
         _kalman_update_sequential(self._dx, self._P, dz, vel_var, self._dhdx[0:3])

From 838988cfa92c08f25149e2b81fd9c03b9a0b038b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 11:36:39 +0100
Subject: [PATCH 077/217] refactor project ahead

---
 src/smsfusion/_ins_v2.py | 57 +++++++++++++++++++++++++++++-----------
 1 file changed, 41 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index c20a354f..5dd9eb3f 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -233,6 +233,40 @@ def _project_cov_ahead(
     P[:, :] = phi @ P @ phi.T + Q
 
 
+@njit  # type: ignore[misc]
+def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr):
+    """
+    Project the state estimate ahead.
+
+    Parameters
+    ----------
+    dvel : ndarray, shape (3,)
+        Velocity change vector measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude change vector measurement (coning integral).
+    v_n : ndarray, shape (3,)
+        Current velocity estimate expressed in the navigation frame. Will be projected
+        ahead (in place).
+    q_nb : ndarray, shape (4,)
+        Current attitude estimate parameterized as a unit quaternion. Will be projected
+        ahead (in place).
+    R_nb : ndarray, shape (3, 3)
+        Current rotation matrix from body to navigation frame.
+    dvel_g_corr : ndarray, shape (3,)
+        Gravity correction term for the velocity change vector.
+
+    Returns
+    -------
+    None
+        The state estimates are updated in place.
+    """
+    # Velocity state estimate
+    v_n[:] += R_nb @ dvel + dvel_g_corr
+
+    # Attitude state estimate
+    _update_quaternion_with_rotvec(q_nb, dtheta)
+
+
 def _state_transition(
     dt: float,
     dvel: NDArray[np.float64],
@@ -606,20 +640,6 @@ def _aiding_update_head(
         dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
         _kalman_update_scalar(self._dx, self._P, dz, head_var, self._dhdx[3])
 
-    def _project_ahead(self, dvel, dtheta) -> None:
-        """
-        Project state and covariance estimates ahead.
-        """
-
-        # Velocity (dead reckoning)
-        self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr
-
-        # Attitude (dead reckoning)
-        _update_quaternion_with_rotvec(self._q_nb, dtheta)
-
-        # Covariance
-        _project_cov_ahead(self._P, self._phi, self._Q)
-
     def update(
         self,
         dvel: ArrayLike,
@@ -668,8 +688,13 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        # Project (a priori) state and covariance estimates ahead
-        self._project_ahead(dvel, dtheta)
+        # Project state estimates ahead (a priori)
+        _project_state_ahead(
+            dvel, dtheta, self._v_n, self._q_nb, self._R_nb, self._dvel_g_corr
+        )
+
+        # Project error covariance matrix estimate ahead (a priori)
+        _project_cov_ahead(self._P, self._phi, self._Q)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if vel is not None:

From 3c50e8bc2d75dfcaf14c90f6474f749b196c0751 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 11:56:00 +0100
Subject: [PATCH 078/217] refactor reset

---
 src/smsfusion/_ins_v2.py | 46 +++++++++++++++++++++++-----------------
 1 file changed, 26 insertions(+), 20 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 5dd9eb3f..ce73f554 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -236,7 +236,7 @@ def _project_cov_ahead(
 @njit  # type: ignore[misc]
 def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr):
     """
-    Project the state estimate ahead.
+    Project the state estimate ahead (in place).
 
     Parameters
     ----------
@@ -254,11 +254,6 @@ def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr):
         Current rotation matrix from body to navigation frame.
     dvel_g_corr : ndarray, shape (3,)
         Gravity correction term for the velocity change vector.
-
-    Returns
-    -------
-    None
-        The state estimates are updated in place.
     """
     # Velocity state estimate
     v_n[:] += R_nb @ dvel + dvel_g_corr
@@ -427,6 +422,30 @@ def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
     return g_n
 
 
+@njit  # type: ignore[misc]
+def _reset(v_n, q_nb, bg_b, dx) -> None:
+    """
+    Reset state, moving information from the error-state estimate to the nominal
+    state estimates.
+
+    Parameters
+    ----------
+    v_n : ndarray, shape (3,)
+        Velocity state estimate to be reset in place.
+    q_nb : ndarray, shape (4,)
+        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,)
+        Error state vector containing the corrections to be applied to the state
+        estimates. Will be reset to zero after applying the corrections.
+    """
+    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
+    v_n[:] += dx[0:3]
+    bg_b[:] += dx[6:9]
+    dx[:] = 0.0
+
+
 class AHRSv2:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
@@ -595,19 +614,6 @@ def P(self) -> NDArray[np.float64]:
         """
         return self._P.copy()
 
-    def _reset(self) -> None:
-        """
-        Reset state.
-        """
-
-        if not self._dx.any():
-            return
-
-        _update_quaternion_with_gibbs2(self._q_nb, self._dx[3:6])
-        self._v_n[:] += self._dx[0:3]
-        self._bg_b[:] += self._dx[6:9]
-        self._dx[:] = 0.0
-
     def _aiding_update_vel(
         self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
     ) -> None:
@@ -703,7 +709,7 @@ def update(
             self._aiding_update_head(head, head_var, head_degrees)
 
         # Reset state
-        self._reset()
+        _reset(self._v_n, self._q_nb, self._bg_b, self._dx)
 
         # Update model
         self._dvel[:] = dvel

From 5fb27c7c92b103877438ae04096a7423a89d4892 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:03:50 +0100
Subject: [PATCH 079/217] small fix

---
 src/smsfusion/_ins_v2.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index ce73f554..f6f58a65 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -440,8 +440,8 @@ def _reset(v_n, q_nb, bg_b, dx) -> None:
         Error state vector containing the corrections to be applied to the state
         estimates. Will be reset to zero after applying the corrections.
     """
-    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
     v_n[:] += dx[0:3]
+    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
     bg_b[:] += dx[6:9]
     dx[:] = 0.0
 

From e0d68b044010dce06fc3023aa98c51a5fa3c83a1 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:14:38 +0100
Subject: [PATCH 080/217] some changes

---
 src/smsfusion/__init__.py |  2 +-
 src/smsfusion/_ins_v2.py  | 22 +++++++---------------
 2 files changed, 8 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 4e91acfa..1da80bfc 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,9 +1,9 @@
+from . import _ins_v2  # hidden
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg
 from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
-from . import _ins_v2  # hidden
 
 __all__ = [
     "AHRS",
diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index f6f58a65..09486e86 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -234,33 +234,24 @@ def _project_cov_ahead(
 
 
 @njit  # type: ignore[misc]
-def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr):
+def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr):
     """
-    Project the state estimate ahead (in place).
+    Project velocity estimate ahead (in place).
 
     Parameters
     ----------
     dvel : ndarray, shape (3,)
         Velocity change vector measurement (sculling integral).
-    dtheta : ndarray, shape (3,)
-        Attitude change vector measurement (coning integral).
     v_n : ndarray, shape (3,)
         Current velocity estimate expressed in the navigation frame. Will be projected
         ahead (in place).
-    q_nb : ndarray, shape (4,)
-        Current attitude estimate parameterized as a unit quaternion. Will be projected
-        ahead (in place).
     R_nb : ndarray, shape (3, 3)
         Current rotation matrix from body to navigation frame.
     dvel_g_corr : ndarray, shape (3,)
         Gravity correction term for the velocity change vector.
     """
-    # Velocity state estimate
     v_n[:] += R_nb @ dvel + dvel_g_corr
 
-    # Attitude state estimate
-    _update_quaternion_with_rotvec(q_nb, dtheta)
-
 
 def _state_transition(
     dt: float,
@@ -694,10 +685,11 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        # Project state estimates ahead (a priori)
-        _project_state_ahead(
-            dvel, dtheta, self._v_n, self._q_nb, self._R_nb, self._dvel_g_corr
-        )
+        # Project velocity estimate ahead (a priori)
+        _project_vel_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr)
+
+        # Project attitude estimate ahead (a priori)
+        _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project error covariance matrix estimate ahead (a priori)
         _project_cov_ahead(self._P, self._phi, self._Q)

From 98570e87c082a3e71918d894b9210fb1f7b147ec Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:23:35 +0100
Subject: [PATCH 081/217] rename functions

---
 src/smsfusion/_ins_v2.py | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 09486e86..cfb5f8fa 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -215,7 +215,7 @@ def _kalman_update_sequential(
 
 
 @njit  # type: ignore[misc]
-def _project_cov_ahead(
+def _project_covariance_ahead(
     P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
 ) -> None:
     """
@@ -234,7 +234,7 @@ def _project_cov_ahead(
 
 
 @njit  # type: ignore[misc]
-def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr):
+def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr):
     """
     Project velocity estimate ahead (in place).
 
@@ -253,7 +253,7 @@ def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr):
     v_n[:] += R_nb @ dvel + dvel_g_corr
 
 
-def _state_transition(
+def _state_transition_matrix(
     dt: float,
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
@@ -290,7 +290,7 @@ def _state_transition(
 
 
 @njit  # type: ignore[misc]
-def _update_state_transition(
+def _update_state_transition_matrix(
     phi: NDArray[np.float64],
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
@@ -337,7 +337,7 @@ def _update_state_transition(
     phi[2, 5] = -(dvy * r20 - dvx * r21)
 
 
-def _process_noise_cov(
+def _process_noise_covariance_matrix(
     dt: float, vrw: float, arw: float, gbs: float, gbc: float
 ) -> NDArray[np.float64]:
     """
@@ -521,10 +521,10 @@ def __init__(
         self._dx = np.zeros(9)
 
         # Discrete state-space model
-        self._phi = _state_transition(
+        self._phi = _state_transition_matrix(
             self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc
         )
-        self._Q = _process_noise_cov(
+        self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
         )
         self._dhdx = _measurement_matrix(self._q_nb)
@@ -686,13 +686,13 @@ def update(
         dtheta = dtheta - self._dt * self._bg_b
 
         # Project velocity estimate ahead (a priori)
-        _project_vel_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr)
+        _project_velocity_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr)
 
         # Project attitude estimate ahead (a priori)
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project error covariance matrix estimate ahead (a priori)
-        _project_cov_ahead(self._P, self._phi, self._Q)
+        _project_covariance_ahead(self._P, self._phi, self._Q)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if vel is not None:
@@ -707,6 +707,6 @@ def update(
         self._dvel[:] = dvel
         self._dtheta[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb)
+        _update_state_transition_matrix(self._phi, self._dvel, self._dtheta, self._R_nb)
 
         return self

From 14978421e097af25b4ab75b1ee0a25ef080a0d6c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:39:16 +0100
Subject: [PATCH 082/217] comment

---
 src/smsfusion/_ins_v2.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index cfb5f8fa..ae1f6104 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -703,7 +703,7 @@ def update(
         # Reset state
         _reset(self._v_n, self._q_nb, self._bg_b, self._dx)
 
-        # Update model
+        # Update state space model
         self._dvel[:] = dvel
         self._dtheta[:] = dtheta
         self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)

From c27890e1e0e7ef9a8cd3657f4d3df1160e5a76e9 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:43:46 +0100
Subject: [PATCH 083/217] docstring fixes

---
 src/smsfusion/_ins_v2.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index ae1f6104..8184c44d 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -127,12 +127,12 @@ def _covariance_update(
     r: float,
 ) -> NDArray[np.float64]:
     """
-    Compute the updated state error covariance matrix estimate (Joseph form).
+    Compute the updated error covariance matrix estimate (Joseph form).
 
     Parameters
     ----------
     P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
+        Error covariance matrix to be updated in place.
     k : ndarray, shape (n,)
         Kalman gain vector.
     h : ndarray, shape (n,)
@@ -166,7 +166,7 @@ def _kalman_update_scalar(
     x : ndarray, shape (n,)
         State estimate to be updated in place.
     P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
+        Error covariance matrix to be updated in place.
     z : float
         Scalar measurement.
     r : float
@@ -201,7 +201,7 @@ def _kalman_update_sequential(
     x : ndarray, shape (n,)
         State estimate to be updated in place.
     P : ndarray, shape (n, n)
-        State error covariance matrix to be updated in place.
+        Error covariance matrix to be updated in place.
     z : ndarray, shape (m,)
         Measurement vector.
     var : ndarray, shape (m,)
@@ -224,7 +224,7 @@ def _project_covariance_ahead(
     Parameters
     ----------
     P : ndarray, shape (n, n)
-        State error covariance matrix to be projected ahead (in place).
+        Error covariance matrix to be projected ahead (in place).
     phi : ndarray, shape (n, n)
         State transition matrix.
     Q : ndarray, shape (n, n)

From c297bb1188d4edbf648c459e9aea48f97b970108 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Fri, 27 Mar 2026 14:56:35 +0100
Subject: [PATCH 084/217] docstring fix

---
 src/smsfusion/_ins_v2.py | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 8184c44d..ca3be52e 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -241,14 +241,14 @@ def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr):
     Parameters
     ----------
     dvel : ndarray, shape (3,)
-        Velocity change vector measurement (sculling integral).
+        Velocity increment measurement (sculling integral).
     v_n : ndarray, shape (3,)
-        Current velocity estimate expressed in the navigation frame. Will be projected
-        ahead (in place).
+        Current velocity estimate expressed in the navigation frame (projected ahead
+        in place).
     R_nb : ndarray, shape (3, 3)
         Current rotation matrix from body to navigation frame.
     dvel_g_corr : ndarray, shape (3,)
-        Gravity correction term for the velocity change vector.
+        Gravity correction.
     """
     v_n[:] += R_nb @ dvel + dvel_g_corr
 
@@ -268,9 +268,9 @@ def _state_transition_matrix(
     dt : float
         Time step in seconds.
     dvel : ndarray, shape (3,)
-        Velocity change vector (sculling integral).
+        Velocity increment measurement (sculling integral).
     dtheta : ndarray, shape (3,)
-        Attitude change vector (coning integral).
+        Attitude increment measurement (coning integral).
     R_nb : ndarray, shape (3, 3)
         Rotation matrix from body to navigation frame.
     gbc : float
@@ -304,9 +304,9 @@ def _update_state_transition_matrix(
     phi : ndarray, shape (9, 9)
         State transition matrix to be updated in place.
     dvel : ndarray, shape (3,)
-        Velocity change vector (sculling integral).
+        Velocity increment measurement (sculling integral).
     dtheta : ndarray, shape (3,)
-        Attitude change vector (coning integral).
+        Attitude increment measurement (coning integral).
     R_nb : ndarray, shape (3, 3)
         Rotation matrix from body to navigation frame.
     """
@@ -454,9 +454,9 @@ class AHRSv2:
     bg : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
     dvel : array_like, shape (3,), optional
-        Initial velocity change vector measurement (sculling integral). Defaults to zero.
+        Initial velocity increment (sculling integral). Defaults to zero (stationary).
     dtheta : array_like, shape (3,), optional
-        Initial attitude change vector measurement (coning integral). Defaults to zero.
+        Initial attitude increment (coning integral). Defaults to zero (stationary).
     P : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
         a small diagonal matrix (1e-6 * np.eye(9)).
@@ -579,13 +579,13 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
 
     def dvel(self) -> NDArray[np.float64]:
         """
-        Previous velocity change vector measurement (sculling integral).
+        Previous velocity increment measurement (sculling integral).
         """
         return self._dvel.copy()
 
     def dtheta(self, degrees=False) -> NDArray[np.float64]:
         """
-        Previous attitude change vector measurement (coning integral).
+        Previous bias corrected attitude increment (coning integral).
 
         Parameters
         ----------
@@ -654,12 +654,12 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,), optional
-            Velocity change vector (sculling integral) in m/s.
+            Velocity increment (sculling integral) in m/s.
         dtheta : array_like, shape (3,), optional
-            Attitude change vector (coning integral) in radians.
+            Attitude increment (coning integral) in radians.
         degrees : bool, optional
-            Specifies whether the unit of the attitude change vector, ``dtheta``,
-            is degrees or radians. Defaults to radians.
+            Specifies whether the unit of the attitude increment, ``dtheta``, is
+            degrees or radians. Defaults to radians.
         head : float, optional
             Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
             assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.

From 9b4d080e4dda728143f44a863f870c94398bf505 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 28 Mar 2026 09:19:53 +0100
Subject: [PATCH 085/217] remove need for dvel and dtheta attributes

---
 src/smsfusion/_ins_v2.py | 43 ++++++----------------------------------
 1 file changed, 6 insertions(+), 37 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index ca3be52e..e5c0334b 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -453,10 +453,6 @@ class AHRSv2:
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
     bg : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    dvel : array_like, shape (3,), optional
-        Initial velocity increment (sculling integral). Defaults to zero (stationary).
-    dtheta : array_like, shape (3,), optional
-        Initial attitude increment (coning integral). Defaults to zero (stationary).
     P : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
         a small diagonal matrix (1e-6 * np.eye(9)).
@@ -487,8 +483,6 @@ def __init__(
         v: ArrayLike = (0.0, 0.0, 0.0),
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
-        dvel: ArrayLike = (0.0, 0.0, 0.0),
-        dtheta: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = 1e-6 * np.eye(9),
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
@@ -515,14 +509,12 @@ def __init__(
         self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
         self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
         self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy()
-        self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 
         # Discrete state-space model
         self._phi = _state_transition_matrix(
-            self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc
+            self._dt, np.zeros(3), np.zeros(3), self._R_nb, self._gbc
         )
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
@@ -577,27 +569,6 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
             bg_b = (180.0 / np.pi) * bg_b
         return bg_b
 
-    def dvel(self) -> NDArray[np.float64]:
-        """
-        Previous velocity increment measurement (sculling integral).
-        """
-        return self._dvel.copy()
-
-    def dtheta(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Previous bias corrected attitude increment (coning integral).
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the coning integral in degrees or radians. Defaults
-            to radians.
-        """
-        dtheta = self._dtheta.copy()
-        if degrees:
-            dtheta = (180.0 / np.pi) * dtheta
-        return dtheta
-
     @property
     def P(self) -> NDArray[np.float64]:
         """
@@ -685,8 +656,12 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
+        # Update state-space model
+        R_nb = _rot_matrix_from_quaternion(self._q_nb)
+        _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb)
+
         # Project velocity estimate ahead (a priori)
-        _project_velocity_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr)
+        _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr)
 
         # Project attitude estimate ahead (a priori)
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
@@ -703,10 +678,4 @@ def update(
         # Reset state
         _reset(self._v_n, self._q_nb, self._bg_b, self._dx)
 
-        # Update state space model
-        self._dvel[:] = dvel
-        self._dtheta[:] = dtheta
-        self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb)
-        _update_state_transition_matrix(self._phi, self._dvel, self._dtheta, self._R_nb)
-
         return self

From 7f17d488f06872a28a63640f96a64e8a2773e5f8 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Sat, 28 Mar 2026 09:21:21 +0100
Subject: [PATCH 086/217] small fix

---
 src/smsfusion/_ins_v2.py | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index e5c0334b..f56ac3b6 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -660,13 +660,11 @@ def update(
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
         _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb)
 
-        # Project velocity estimate ahead (a priori)
+        # Project (a priori) state estimates ahead
         _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr)
-
-        # Project attitude estimate ahead (a priori)
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
-        # Project error covariance matrix estimate ahead (a priori)
+        # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements

From f9dbdc63cdea547a2def226103dafb29f18b786a Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 30 Mar 2026 09:20:38 +0200
Subject: [PATCH 087/217] small fix

---
 src/smsfusion/_ins_v2.py | 21 ++++++++++-----------
 1 file changed, 10 insertions(+), 11 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index f56ac3b6..d0d1d6d3 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -326,15 +326,15 @@ def _update_state_transition_matrix(
     phi[5, 4] = -dtx
 
     # phi[0:3, 3:6] = -dt * R_nb @ S(f_b)
-    phi[0, 3] = -(dvz * r01 - dvy * r02)
-    phi[1, 3] = -(dvz * r11 - dvy * r12)
-    phi[2, 3] = -(dvz * r21 - dvy * r22)
-    phi[0, 4] = -(-dvz * r00 + dvx * r02)
-    phi[1, 4] = -(-dvz * r10 + dvx * r12)
-    phi[2, 4] = -(-dvz * r20 + dvx * r22)
-    phi[0, 5] = -(dvy * r00 - dvx * r01)
-    phi[1, 5] = -(dvy * r10 - dvx * r11)
-    phi[2, 5] = -(dvy * r20 - dvx * r21)
+    phi[0, 3] = -dvz * r01 + dvy * r02
+    phi[1, 3] = -dvz * r11 + dvy * r12
+    phi[2, 3] = -dvz * r21 + dvy * r22
+    phi[0, 4] = dvz * r00 - dvx * r02
+    phi[1, 4] = dvz * r10 - dvx * r12
+    phi[2, 4] = dvz * r20 - dvx * r22
+    phi[0, 5] = -dvy * r00 + dvx * r01
+    phi[1, 5] = -dvy * r10 + dvx * r11
+    phi[2, 5] = -dvy * r20 + dvx * r21
 
 
 def _process_noise_covariance_matrix(
@@ -416,8 +416,7 @@ def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
 @njit  # type: ignore[misc]
 def _reset(v_n, q_nb, bg_b, dx) -> None:
     """
-    Reset state, moving information from the error-state estimate to the nominal
-    state estimates.
+    Reset state.
 
     Parameters
     ----------

From 95900d80cc785f35a8fb64078029867668544f2c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 31 Mar 2026 08:40:45 +0200
Subject: [PATCH 088/217] remove obsolete rot m,atrix attribute

---
 src/smsfusion/_ins_v2.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index d0d1d6d3..25f76bd3 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -506,14 +506,17 @@ def __init__(
         # State and covariance estimates
         self._v_n = np.asarray_chkfinite(v).reshape(3).copy()
         self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
-        self._R_nb = _rot_matrix_from_quaternion(self._q_nb)
         self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 
         # Discrete state-space model
         self._phi = _state_transition_matrix(
-            self._dt, np.zeros(3), np.zeros(3), self._R_nb, self._gbc
+            self._dt,
+            np.zeros(3),
+            np.zeros(3),
+            _rot_matrix_from_quaternion(self._q_nb),
+            self._gbc,
         )
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc

From 70fc149458fa4f09db0230dad8874f2cb986cf27 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 1 Apr 2026 10:14:35 +0200
Subject: [PATCH 089/217] immutable default P0

---
 src/smsfusion/_ins_v2.py | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 25f76bd3..8bbf096d 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -9,6 +9,20 @@
 from ._vectorops import _normalize, _skew_symmetric
 
 
+P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
+)
+
+
+
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
@@ -482,7 +496,7 @@ def __init__(
         v: ArrayLike = (0.0, 0.0, 0.0),
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = 1e-6 * np.eye(9),
+        P: ArrayLike = P0,
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.0001,
         gyro_bias_stability: float = 0.00005,

From fca3026cdb44357327fbf206a106e5aa72191474 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 1 Apr 2026 10:21:39 +0200
Subject: [PATCH 090/217] delete volocity project ahead function

---
 src/smsfusion/_ins_v2.py | 24 +-----------------------
 1 file changed, 1 insertion(+), 23 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 8bbf096d..9831b2c4 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -8,7 +8,6 @@
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
-
 P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
@@ -22,7 +21,6 @@
 )
 
 
-
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
@@ -247,26 +245,6 @@ def _project_covariance_ahead(
     P[:, :] = phi @ P @ phi.T + Q
 
 
-@njit  # type: ignore[misc]
-def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr):
-    """
-    Project velocity estimate ahead (in place).
-
-    Parameters
-    ----------
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    v_n : ndarray, shape (3,)
-        Current velocity estimate expressed in the navigation frame (projected ahead
-        in place).
-    R_nb : ndarray, shape (3, 3)
-        Current rotation matrix from body to navigation frame.
-    dvel_g_corr : ndarray, shape (3,)
-        Gravity correction.
-    """
-    v_n[:] += R_nb @ dvel + dvel_g_corr
-
-
 def _state_transition_matrix(
     dt: float,
     dvel: NDArray[np.float64],
@@ -677,7 +655,7 @@ def update(
         _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb)
 
         # Project (a priori) state estimates ahead
-        _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr)
+        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr  # TODO: speed-up with njit?
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project (a priori) error covariance matrix estimate ahead

From 541a0068a6632d162cbb9a6e82fd9d39880d3afc Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 1 Apr 2026 12:11:13 +0200
Subject: [PATCH 091/217] innovation cov

---
 src/smsfusion/_ins_v2.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
index 9831b2c4..231152e9 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins_v2.py
@@ -121,12 +121,13 @@ def _kalman_gain(
         Kalman gain vector.
     """
 
-    # Innovation covariance (inverse)
     Ph = np.dot(P, h)
-    s_inv = 1.0 / (np.dot(h, Ph) + r)
+
+    # Innovation covariance
+    s = np.dot(h, Ph) + r
 
     # Kalman gain
-    k = Ph * s_inv
+    k = Ph / s
 
     return k
 

From 719e32606b75d08fa93ef512c48bc5efbeafedea Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Thu, 30 Apr 2026 14:02:15 +0200
Subject: [PATCH 092/217] add vruv2

---
 src/smsfusion/_vru_v2.py | 615 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 615 insertions(+)
 create mode 100644 src/smsfusion/_vru_v2.py

diff --git a/src/smsfusion/_vru_v2.py b/src/smsfusion/_vru_v2.py
new file mode 100644
index 00000000..96cb4e9e
--- /dev/null
+++ b/src/smsfusion/_vru_v2.py
@@ -0,0 +1,615 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from ._vectorops import _normalize, _skew_symmetric
+
+
+P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
+)
+
+
+
+@njit  # type: ignore[misc]
+def _update_quaternion_with_gibbs2(
+    q: NDArray[np.float64], da: NDArray[np.float64]
+) -> None:
+    """
+    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
+    as a scaled (2x) Gibbs vector.
+
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
+    da : ndarray, shape (3,)
+        Attitude error correction parameterized as a scaled (2x) Gibbs vector.
+
+    References
+    ----------
+    .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    and Control, Eq. (6.27)-(6.28).
+    """
+
+    qw, qx, qy, qz = q
+    dax, day, daz = da
+
+    q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
+    q[:] = _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _update_quaternion_with_rotvec(
+    q: NDArray[np.float64], dtheta: NDArray[np.float64]
+) -> None:
+    """
+    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
+    as a rotation vector.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
+    dtheta : ndarray, shape (3,)
+        Attitude increment (rotation vector).
+
+    References
+    ----------
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
+    """
+
+    qw, qx, qy, qz = q
+    rx, ry, rz = dtheta
+
+    gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2)
+    cos_gamma = np.cos(gamma)
+
+    if gamma >= 1e-5:
+        scale = np.sin(gamma) / (2.0 * gamma)
+    else:
+        scale = 0.5
+
+    # Psi
+    px = scale * rx
+    py = scale * ry
+    pz = scale * rz
+
+    q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz
+    q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz
+    q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
+    q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
+    q[:] = _normalize(q)
+
+
+@njit  # type: ignore[misc]
+def _kalman_gain(
+    P: NDArray[np.float64], h: NDArray[np.float64], r: float
+) -> NDArray[np.float64]:
+    """
+    Compute the Kalman gain for a scalar measurement.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n,)
+        Kalman gain vector.
+    """
+
+    Ph = np.dot(P, h)
+
+    # Innovation covariance
+    s = np.dot(h, Ph) + r
+
+    # Kalman gain
+    k = Ph / s
+
+    return k
+
+
+@njit  # type: ignore[misc]
+def _covariance_update(
+    P: NDArray[np.float64],
+    k: NDArray[np.float64],
+    h: NDArray[np.float64],
+    r: float,
+) -> NDArray[np.float64]:
+    """
+    Compute the updated error covariance matrix estimate (Joseph form).
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    k : ndarray, shape (n,)
+        Kalman gain vector.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Updated state error covariance matrix.
+    """
+    A = np.eye(k.size) - np.outer(k, h)
+    P = A @ P @ A.T + r * np.outer(k, k)
+    return P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_scalar(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: float,
+    r: float,
+    h: NDArray[np.float64],
+) -> None:
+    """
+    Scalar Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    z : float
+        Scalar measurement.
+    r : float
+        Scalar measurement noise variance.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    """
+
+    # Kalman gain
+    k = _kalman_gain(P, h, r)
+
+    # Updated (a posteriori) state estimate
+    x[:] += k * (z - np.dot(h, x))
+
+    # Updated (a posteriori) covariance estimate (Joseph form)
+    P[:, :] = _covariance_update(P, k, h, r)
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_sequential(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: NDArray[np.float64],
+    var: NDArray[np.float64],
+    H: NDArray[np.float64],
+) -> None:
+    """
+    Sequential (one-at-a-time) Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    z : ndarray, shape (m,)
+        Measurement vector.
+    var : ndarray, shape (m,)
+        Measurement noise variances corresponding to each scalar measurement.
+    H : ndarray, shape (m, n)
+        Measurement matrix where each row corresponds to a scalar measurement model.
+    """
+    m = z.shape[0]
+    for i in range(m):
+        _kalman_update_scalar(x, P, z[i], var[i], H[i])
+
+
+@njit  # type: ignore[misc]
+def _project_covariance_ahead(
+    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
+) -> None:
+    """
+    Project the error covariance matrix estimate ahead.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be projected ahead (in place).
+    phi : ndarray, shape (n, n)
+        State transition matrix.
+    Q : ndarray, shape (n, n)
+        Process noise covariance matrix.
+    """
+    P[:, :] = phi @ P @ phi.T + Q
+
+
+def _state_transition_matrix(
+    dt: float,
+    dtheta: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (6, 6)
+        State transition matrix.
+    """
+    phi = np.eye(6)
+    phi[0:3, 0:3] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[0:3, 3:6] -= dt * np.eye(3)
+    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition_matrix(
+    phi: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+) -> None:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (9, 9)
+        State transition matrix to be updated in place.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    """
+    dtx, dty, dtz = dtheta
+
+    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
+    phi[0, 1] = dtz
+    phi[0, 2] = -dty
+    phi[1, 0] = -dtz
+    phi[1, 2] = dtx
+    phi[2, 0] = dty
+    phi[2, 1] = -dtx
+
+
+def _process_noise_covariance_matrix(
+    dt: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (9, 9)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((6, 6))
+    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
+    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+@njit  # type: ignore[misc]
+def _reset(q_nb, bg_b, dx) -> None:
+    """
+    Reset state.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        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,)
+        Error state vector containing the corrections to be applied to the state
+        estimates. Will be reset to zero after applying the corrections.
+    """
+    _update_quaternion_with_gibbs2(q_nb, dx[0:3])
+    bg_b[:] += dx[3:6]
+    dx[:] = 0.0
+
+
+@njit  # type: ignore[misc]
+def _nz_b_from_quat(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Unit vector describing the z-axis of frame {n} expressed in frame {b}, computed
+    from a unit quaternion, q_nb.
+
+    Note that this vector corresponds to the third row of the rotation matrix which
+    transforms a vector from {b} to {n}.
+
+    Parameters
+    ----------
+    q_nb : numpy.ndarray, shape (4,)
+        Unit quaternion which transforms a vector from frame {b} to frame {n}.
+
+    Returns
+    -------
+    numpy.ndarray, shape (3,)
+        The z-axis (unit vector) of frame {n} expressed in frame {b}.
+    """
+
+    x = 2.0 * (q_nb[1] * q_nb[3] - q_nb[0] * q_nb[2])
+    y = 2.0 * (q_nb[2] * q_nb[3] + q_nb[0] * q_nb[1])
+    z = 1.0 - 2.0 * (q_nb[1] ** 2 + q_nb[2] ** 2)
+
+    return np.array([x, y, z])
+
+
+def _nz2vg(nav_frame: str) -> float:
+    """
+    Gravity direction along the navigation frame's z-axis. Transforms the z-axis
+    of the navigation frame to a gravity reference vector (unit vector).
+
+    Parameters
+    ----------
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame.
+
+    Returns
+    -------
+    float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
+    """
+    if nav_frame.lower() == "ned":
+        return 1.0
+    elif nav_frame.lower() == "enu":
+        return -1.0
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+
+
+class VRUv2:
+    """
+    Vertical Reference Unit (VRU) using a multiplicative extended
+    Kalman filter (MEKF). Uses only gravitational vector as aiding.
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    q : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix. Defaults to
+        a small diagonal matrix (1e-6 * np.eye(9)).
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
+        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
+        noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    nav_frame : {'NED', 'ENU'}, optional
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+    def __init__(
+        self,
+        fs: float,
+        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = P0,
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.0001,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(3, 3).copy()
+        self._dx = np.zeros(6)
+
+        # Discrete state-space model
+        self._phi = _state_transition_matrix(
+            self._dt,
+            np.zeros(3),
+            self._gbc,
+        )
+        self._Q = _process_noise_covariance_matrix(
+            self._dt, self._arw, self._gbs, self._gbc
+        )
+
+        self._dhdx = np.zeros((3, 3))
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    @njit  # type: ignore[misc]
+    def _aiding_update_gref(
+        self,
+        dvel: NDArray[np.float64],
+        gref_var: NDArray[np.float64] | None,
+        q_nb: NDArray[np.float64],
+    ) -> None:
+        """
+        Update state and covariance with gravity reference vector aiding measurement.
+        """
+
+        if gref_var is None:
+            raise ValueError("gref_var is not provided; required for gref aiding.")
+
+        vg_b = self._nz2vg * _nz_b_from_quat(q_nb)
+        dz = -_normalize(dvel) - vg_b
+        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
+
+        _kalman_update_sequential(self._dx, self._P, dz, gref_var, self._dhdx[0:3])
+
+
+    def update(
+        self,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
+        degrees: bool = False,
+        gref: bool = True,
+        gref_var: ArrayLike = (0.001, 0.001, 0.001),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        dvel : array_like, shape (3,), optional
+            Velocity increment (sculling integral) in m/s.
+        dtheta : array_like, shape (3,), optional
+            Attitude increment (coning integral) in radians.
+        degrees : bool, optional
+            Specifies whether the unit of the attitude increment, ``dtheta``, is
+            degrees or radians. Defaults to radians.
+        gref : bool, optional
+            Specifies whether to use accelerometer measurements (dv) and the known
+            direction of gravity as aiding. Defaults to ``True``.
+        gref_var : array_like, shape (3,), optional
+            Variance of gravity reference vector measurement noise (dimensionless).
+            Required for gravity reference vector aiding. Defaults to (0.001, 0.001, 0.001).
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
+
+        if degrees:
+            dtheta = np.radians(dtheta)
+
+        dtheta = dtheta - self._dt * self._bg_b
+
+        # Update state-space model
+        _update_state_transition_matrix(self._phi, dtheta)
+
+        # Project (a priori) state estimates ahead
+        _update_quaternion_with_rotvec(self._q_nb, dtheta)
+
+        # Project (a priori) error covariance matrix estimate ahead
+        _project_covariance_ahead(self._P, self._phi, self._Q)
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        if gref is True:
+            self._aiding_update_gref(
+                dvel,
+                gref_var,
+                self._q_nb
+            )
+
+        # Reset state
+        _reset(self._q_nb, self._bg_b, self._dx)
+
+        return self

From 4e94a9dca51464668fb3b95d2f95f6f59c497428 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Thu, 7 May 2026 13:40:12 +0200
Subject: [PATCH 093/217] fix vru

---
 src/smsfusion/_vru_v2.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_vru_v2.py b/src/smsfusion/_vru_v2.py
index 96cb4e9e..646dc2b2 100644
--- a/src/smsfusion/_vru_v2.py
+++ b/src/smsfusion/_vru_v2.py
@@ -466,7 +466,7 @@ def __init__(
         # State and covariance estimates
         self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
         self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(3, 3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
         self._dx = np.zeros(6)
 
         # Discrete state-space model
@@ -479,7 +479,7 @@ def __init__(
             self._dt, self._arw, self._gbs, self._gbc
         )
 
-        self._dhdx = np.zeros((3, 3))
+        self._dhdx = np.zeros((3, 6))
 
     def quaternion(self) -> NDArray[np.float64]:
         """
@@ -530,7 +530,6 @@ def P(self) -> NDArray[np.float64]:
         """
         return self._P.copy()
 
-    @njit  # type: ignore[misc]
     def _aiding_update_gref(
         self,
         dvel: NDArray[np.float64],

From cabdba70e26bdb20a01442813d87bca0b8632fbf Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 8 May 2026 13:52:34 +0200
Subject: [PATCH 094/217] refactor

---
 src/smsfusion/__init__.py                   |  13 +-
 src/smsfusion/_ins/__init__.py              |   3 +
 src/smsfusion/{_ins_v2.py => _ins/_ahrs.py} | 345 ++---------
 src/smsfusion/_ins/_aiding.py               |  80 +++
 src/smsfusion/{_ins.py => _ins/_ains.py}    |   7 +-
 src/smsfusion/_ins/_common.py               | 380 ++++++++++++
 src/smsfusion/_ins/_vru.py                  | 334 +++++++++++
 src/smsfusion/_vru_v2.py                    | 614 --------------------
 tests/test_ins_v2.py                        |   2 +-
 9 files changed, 872 insertions(+), 906 deletions(-)
 create mode 100644 src/smsfusion/_ins/__init__.py
 rename src/smsfusion/{_ins_v2.py => _ins/_ahrs.py} (60%)
 create mode 100644 src/smsfusion/_ins/_aiding.py
 rename src/smsfusion/{_ins.py => _ins/_ains.py} (99%)
 create mode 100644 src/smsfusion/_ins/_common.py
 create mode 100644 src/smsfusion/_ins/_vru.py
 delete mode 100644 src/smsfusion/_vru_v2.py

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 1da80bfc..bbddd973 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,21 +1,22 @@
-from . import _ins_v2  # hidden
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg
-from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
+
+# from ._ins import _ahrs  # hidden
+from ._ins import AHRS, VRU, AHRSv2, AidedINS, FixedNED, StrapdownINS, VRUv2, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
 
 __all__ = [
     "AHRS",
-    "AidedINS",
+    # "AidedINS",
     "benchmark",
     "constants",
     "calibrate",
     "FixedIntervalSmoother",
-    "FixedNED",
-    "gravity",
+    # "FixedNED",
+    # "gravity",
     "noise",
-    "StrapdownINS",
+    # "StrapdownINS",
     "VRU",
     "quaternion_from_euler",
     "ConingScullingAlg",
diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
new file mode 100644
index 00000000..4c367d3e
--- /dev/null
+++ b/src/smsfusion/_ins/__init__.py
@@ -0,0 +1,3 @@
+from ._ahrs import AHRS as AHRSv2
+from ._ains import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
+from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins/_ahrs.py
similarity index 60%
rename from src/smsfusion/_ins_v2.py
rename to src/smsfusion/_ins/_ahrs.py
index 231152e9..30e3bbb4 100644
--- a/src/smsfusion/_ins_v2.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -4,9 +4,16 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from ._ins import _dhda_head, _h_head, _signed_smallest_angle
-from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from ._vectorops import _normalize, _skew_symmetric
+from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
+
+from ._aiding import _aiding_update_head, _aiding_update_vel
+from ._common import (
+    _dhda_head,
+    _project_covariance_ahead,
+    _update_quaternion_with_gibbs2,
+    _update_quaternion_with_rotvec,
+)
 
 P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
@@ -21,232 +28,7 @@
 )
 
 
-@njit  # type: ignore[misc]
-def _update_quaternion_with_gibbs2(
-    q: NDArray[np.float64], da: NDArray[np.float64]
-) -> None:
-    """
-    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector.
-
-    As described in ref [1]_, this correction can be simplified by doing it in two
-    steps: first a correction, followed by renormalization. The scaling factor becomes
-    obsolete due to the renormalization step.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
-    da : ndarray, shape (3,)
-        Attitude error correction parameterized as a scaled (2x) Gibbs vector.
-
-    References
-    ----------
-    .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
-    and Control, Eq. (6.27)-(6.28).
-    """
-
-    qw, qx, qy, qz = q
-    dax, day, daz = da
-
-    q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz)
-    q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day)
-    q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
-    q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _update_quaternion_with_rotvec(
-    q: NDArray[np.float64], dtheta: NDArray[np.float64]
-) -> NDArray[np.float64]:
-    """
-    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
-    as a rotation vector.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
-    dtheta : ndarray, shape (3,)
-        Attitude increment (rotation vector).
-
-    References
-    ----------
-    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
-    """
-
-    qw, qx, qy, qz = q
-    rx, ry, rz = dtheta
-
-    gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2)
-    cos_gamma = np.cos(gamma)
-
-    if gamma >= 1e-5:
-        scale = np.sin(gamma) / (2.0 * gamma)
-    else:
-        scale = 0.5
-
-    # Psi
-    px = scale * rx
-    py = scale * ry
-    pz = scale * rz
-
-    q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz
-    q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz
-    q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
-    q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _kalman_gain(
-    P: NDArray[np.float64], h: NDArray[np.float64], r: float
-) -> NDArray[np.float64]:
-    """
-    Compute the Kalman gain for a scalar measurement.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n,)
-        Kalman gain vector.
-    """
-
-    Ph = np.dot(P, h)
-
-    # Innovation covariance
-    s = np.dot(h, Ph) + r
-
-    # Kalman gain
-    k = Ph / s
-
-    return k
-
-
-@njit  # type: ignore[misc]
-def _covariance_update(
-    P: NDArray[np.float64],
-    k: NDArray[np.float64],
-    h: NDArray[np.float64],
-    r: float,
-) -> NDArray[np.float64]:
-    """
-    Compute the updated error covariance matrix estimate (Joseph form).
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    k : ndarray, shape (n,)
-        Kalman gain vector.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n, n)
-        Updated state error covariance matrix.
-    """
-    A = np.eye(k.size) - np.outer(k, h)
-    P = A @ P @ A.T + r * np.outer(k, k)
-    return P
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_scalar(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: float,
-    r: float,
-    h: NDArray[np.float64],
-) -> None:
-    """
-    Scalar Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    z : float
-        Scalar measurement.
-    r : float
-        Scalar measurement noise variance.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    """
-
-    # Kalman gain
-    k = _kalman_gain(P, h, r)
-
-    # Updated (a posteriori) state estimate
-    x[:] += k * (z - np.dot(h, x))
-
-    # Updated (a posteriori) covariance estimate (Joseph form)
-    P[:, :] = _covariance_update(P, k, h, r)
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_sequential(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: NDArray[np.float64],
-    var: NDArray[np.float64],
-    H: NDArray[np.float64],
-) -> None:
-    """
-    Sequential (one-at-a-time) Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    z : ndarray, shape (m,)
-        Measurement vector.
-    var : ndarray, shape (m,)
-        Measurement noise variances corresponding to each scalar measurement.
-    H : ndarray, shape (m, n)
-        Measurement matrix where each row corresponds to a scalar measurement model.
-    """
-    m = z.shape[0]
-    for i in range(m):
-        _kalman_update_scalar(x, P, z[i], var[i], H[i])
-
-
-@njit  # type: ignore[misc]
-def _project_covariance_ahead(
-    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
-) -> None:
-    """
-    Project the error covariance matrix estimate ahead.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be projected ahead (in place).
-    phi : ndarray, shape (n, n)
-        State transition matrix.
-    Q : ndarray, shape (n, n)
-        Process noise covariance matrix.
-    """
-    P[:, :] = phi @ P @ phi.T + Q
-
-
-def _state_transition_matrix(
+def _state_transition_matrix_init(
     dt: float,
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
@@ -283,12 +65,12 @@ def _state_transition_matrix(
 
 
 @njit  # type: ignore[misc]
-def _update_state_transition_matrix(
+def _state_transition_matrix_update(
     phi: NDArray[np.float64],
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
     R_nb: NDArray[np.float64],
-) -> None:
+) -> NDArray[np.float64]:
     """
     Update the state transition matrix in place.
 
@@ -328,6 +110,7 @@ def _update_state_transition_matrix(
     phi[0, 5] = -dvy * r00 + dvx * r01
     phi[1, 5] = -dvy * r10 + dvx * r11
     phi[2, 5] = -dvy * r20 + dvx * r21
+    return phi
 
 
 def _process_noise_covariance_matrix(
@@ -361,7 +144,7 @@ def _process_noise_covariance_matrix(
     return Q
 
 
-def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+def _measurement_matrix_init(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
     """
     Measurement matrix.
 
@@ -407,7 +190,14 @@ def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
 
 
 @njit  # type: ignore[misc]
-def _reset(v_n, q_nb, bg_b, dx) -> None:
+def _reset(
+    dx: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    bg_b: NDArray[np.float64],
+) -> tuple[
+    NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]
+]:
     """
     Reset state.
 
@@ -424,12 +214,13 @@ def _reset(v_n, q_nb, bg_b, dx) -> None:
         estimates. Will be reset to zero after applying the corrections.
     """
     v_n[:] += dx[0:3]
-    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
+    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[3:6])
     bg_b[:] += dx[6:9]
     dx[:] = 0.0
+    return dx, v_n, q_nb, bg_b
 
 
-class AHRSv2:
+class AHRS:
     """
     Attitude and Heading Reference System (AHRS) using a multiplicative extended
     Kalman filter (MEKF).
@@ -477,7 +268,7 @@ def __init__(
         bg: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = P0,
         acc_noise_density: float = 0.0007,
-        gyro_noise_density: float = 0.0001,
+        gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
         gyro_bias_corr_time: float = 50.0,
         g: float = 9.80665,
@@ -504,7 +295,7 @@ def __init__(
         self._dx = np.zeros(9)
 
         # Discrete state-space model
-        self._phi = _state_transition_matrix(
+        self._phi = _state_transition_matrix_init(
             self._dt,
             np.zeros(3),
             np.zeros(3),
@@ -514,7 +305,7 @@ def __init__(
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
         )
-        self._dhdx = _measurement_matrix(self._q_nb)
+        self._H = _measurement_matrix_init(self._q_nb)
 
     def quaternion(self) -> NDArray[np.float64]:
         """
@@ -571,48 +362,16 @@ def P(self) -> NDArray[np.float64]:
         """
         return self._P.copy()
 
-    def _aiding_update_vel(
-        self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None
-    ) -> None:
-        """
-        Update with velocity aiding measurement.
-        """
-
-        if vel_var is None:
-            raise ValueError("'vel_var' not provided.")
-
-        dz = vel_meas - self._v_n
-        _kalman_update_sequential(self._dx, self._P, dz, vel_var, self._dhdx[0:3])
-
-    def _aiding_update_head(
-        self, head_meas: float | None, head_var: float | None, head_degrees: bool
-    ) -> None:
-        """
-        Update with heading aiding measurement.
-        """
-
-        if head_var is None:
-            raise ValueError("'head_var' not provided.")
-
-        if head_degrees:
-            head_meas = (np.pi / 180.0) * head_meas
-            head_var = (np.pi / 180.0) ** 2 * head_var
-
-        self._dhdx[3, 3:6] = _dhda_head(self._q_nb)
-
-        dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb))
-        _kalman_update_scalar(self._dx, self._P, dz, head_var, self._dhdx[3])
-
     def update(
         self,
         dvel: ArrayLike,
         dtheta: ArrayLike,
         degrees: bool = False,
+        vel: ArrayLike | None = (0.0, 0.0, 0.0),
+        vel_var: ArrayLike = (100.0, 100.0, 100.0),
         head: float | None = None,
-        head_var: float | None = None,
+        head_var: float = 0.001,
         head_degrees: bool = False,
-        vel: ArrayLike | None = (0.0, 0.0, 0.0),
-        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -626,13 +385,17 @@ def update(
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
+        vel : array-like, shape (3,), optional
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+        vel_var : array-like, shape (3,), optional
+            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel`` is ``None``.
         head : float, optional
             Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
             assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
             If ``None``, compass aiding is not used. See ``head_degrees`` for units.
         head_var : float, optional
             Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
+             See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
         head_degrees : bool, default False
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
             or radians and radians^2. Default is in radians and radians^2.
@@ -653,22 +416,42 @@ def update(
 
         # Update state-space model
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb)
+        self._phi = _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
 
         # Project (a priori) state estimates ahead
-        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr  # TODO: speed-up with njit?
-        _update_quaternion_with_rotvec(self._q_nb, dtheta)
+        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr
+        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project (a priori) error covariance matrix estimate ahead
-        _project_covariance_ahead(self._P, self._phi, self._Q)
+        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if vel is not None:
-            self._aiding_update_vel(vel, vel_var)
+            self._dx, self._P = _aiding_update_vel(
+                self._dx,
+                self._P,
+                self._H[0:3],
+                self._v_n,
+                np.asarray(vel),
+                np.asarray(vel_var),
+            )
+
         if head is not None:
-            self._aiding_update_head(head, head_var, head_degrees)
+            self._H[3, 3:6] = _dhda_head(self._q_nb)  # Update measurement matrix
+
+            self._dx, self._P = _aiding_update_head(
+                self._dx,
+                self._P,
+                self._H[3],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
 
         # Reset state
-        _reset(self._v_n, self._q_nb, self._bg_b, self._dx)
+        self._dx, self._v_n, self._q_nb, self._bg_b = _reset(
+            self._dx, self._v_n, self._q_nb, self._bg_b
+        )
 
         return self
diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
new file mode 100644
index 00000000..a4ea9090
--- /dev/null
+++ b/src/smsfusion/_ins/_aiding.py
@@ -0,0 +1,80 @@
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from smsfusion._vectorops import _normalize, _skew_symmetric
+
+from ._common import (
+    _dhda_head,
+    _h_head,
+    _kalman_update_scalar,
+    _kalman_update_sequential,
+    _signed_smallest_angle,
+)
+
+
+@njit  # type: ignore[misc]
+def _aiding_update_vel(
+    dx: NDArray[np.float64],
+    P: NDArray[np.float64],
+    H: NDArray[np.float64],
+    vel_n: NDArray[np.float64],
+    vel_meas: NDArray[np.float64],
+    vel_var: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Update with velocity aiding measurement.
+    """
+
+    if vel_var is None:
+        raise ValueError("'vel_var' not provided.")
+
+    dz = vel_meas - vel_n
+    dx, P = _kalman_update_sequential(dx, P, dz, vel_var, H)
+    return dx, P
+
+
+@njit  # type: ignore[misc]
+def _aiding_update_head(
+    dx: NDArray[np.float64],
+    P: NDArray[np.float64],
+    H: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    head_meas: float,
+    head_var: float,
+    head_degrees: bool,
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Update with heading aiding measurement.
+    """
+
+    if head_var is None:
+        raise ValueError("'head_var' not provided.")
+
+    if head_degrees:
+        head_meas = (np.pi / 180.0) * head_meas
+        head_var = (np.pi / 180.0) ** 2 * head_var
+
+    dz = _signed_smallest_angle(head_meas - _h_head(q_nb))
+    dx, P = _kalman_update_scalar(dx, P, dz, head_var, H)
+    return dx, P
+
+
+def _aiding_update_gref(
+    dx: NDArray[np.float64],
+    P: NDArray[np.float64],
+    H: NDArray[np.float64],
+    vg_b: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    gref_var: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Update state and covariance with gravity reference vector aiding measurement.
+    """
+
+    if gref_var is None:
+        raise ValueError("gref_var is not provided; required for gref aiding.")
+
+    dz = -_normalize(dvel) - vg_b
+    dx, P = _kalman_update_sequential(dx, P, dz, gref_var, H)
+    return dx, P
diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins/_ains.py
similarity index 99%
rename from src/smsfusion/_ins.py
rename to src/smsfusion/_ins/_ains.py
index 979e45df..f39423e4 100644
--- a/src/smsfusion/_ins.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -6,15 +6,14 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0
-
-from ._transforms import (
+from smsfusion._transforms import (
     _angular_matrix_from_quaternion,
     _euler_from_quaternion,
     _quaternion_from_euler,
     _rot_matrix_from_quaternion,
 )
-from ._vectorops import _normalize, _quaternion_product, _skew_symmetric
+from smsfusion._vectorops import _normalize, _quaternion_product, _skew_symmetric
+from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0
 
 
 def _roll_pitch_from_acc(f, nav_frame):
diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
new file mode 100644
index 00000000..8ffccb2a
--- /dev/null
+++ b/src/smsfusion/_ins/_common.py
@@ -0,0 +1,380 @@
+import numpy as np
+from numba import njit
+from numpy.typing import NDArray
+
+from smsfusion._vectorops import _normalize
+
+
+@njit  # type: ignore[misc]
+def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Compute yaw angle gradient wrt to the unit quaternion.
+
+    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
+    unit quaternion here to avoid singularities.
+
+    Parameters
+    ----------
+    q : numpy.ndarray, shape (3,)
+        Unit quaternion.
+
+    Returns
+    -------
+    numpy.ndarray, shape (3,)
+        Yaw angle gradient vector.
+
+    References
+    ----------
+    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
+    2nd Edition, equation 14.254, John Wiley & Sons, 2021.
+    """
+    q_w, q_x, q_y, q_z = q
+    u_y = 2.0 * (q_x * q_y + q_z * q_w)
+    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
+    u = u_y / u_x
+
+    duda_scale = 1.0 / u_x**2
+    duda_x = -(q_w * q_y) * (1.0 - 2.0 * q_w**2) - (2.0 * q_w**2 * q_x * q_z)
+    duda_y = (q_w * q_x) * (1.0 - 2.0 * q_z**2) + (2.0 * q_w**2 * q_y * q_z)
+    duda_z = q_w**2 * (1.0 - 2.0 * q_y**2) + (2.0 * q_w * q_x * q_y * q_z)
+    duda = duda_scale * np.array([duda_x, duda_y, duda_z])
+
+    dhda = 1.0 / (1.0 + u**2) * duda
+
+    return dhda  # type: ignore[no-any-return]
+
+
+@njit
+def _h_head(q: NDArray[np.float64]) -> float:
+    """
+    Compute yaw angle from unit quaternion.
+
+    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
+    unit quaternion here to avoid singularities.
+
+    Parameters
+    ----------
+    q : numpy.ndarray, shape (4,)
+        Unit quaternion.
+
+    Returns
+    -------
+    float
+        Yaw angle in the NED reference frame.
+
+    References
+    ----------
+    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
+    2nd Edition, equation 14.251, John Wiley & Sons, 2021.
+    """
+    q_w, q_x, q_y, q_z = q
+    u_y = 2.0 * (q_x * q_y + q_z * q_w)
+    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
+    return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
+
+
+@njit
+def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
+    """
+    Convert the given angle to the smallest angle between [-180., 180) degrees.
+
+    Parameters
+    ----------
+    angle : float
+        Value of angle.
+    degrees : bool, default True
+        Specify whether ``angle`` is given degrees or radians.
+
+    Returns
+    -------
+    float
+        The smallest angle between [-180., 180) degrees (or  [-pi, pi] radians).
+    """
+    base = 180.0 if degrees else np.pi
+    return (angle + base) % (2.0 * base) - base  # type: ignore[no-any-return]
+
+
+@njit  # type: ignore[misc]
+def _update_quaternion_with_rotvec(
+    q: NDArray[np.float64], dtheta: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Update (inplace) a unit quaternion, q, with a small attitude increment, dtheta,
+    parameterized as a rotation vector.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
+    dtheta : ndarray, shape (3,)
+        Attitude increment (rotation vector).
+
+    References
+    ----------
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
+    """
+
+    qw, qx, qy, qz = q
+    rx, ry, rz = dtheta
+
+    gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2)
+    cos_gamma = np.cos(gamma)
+
+    if gamma >= 1e-5:
+        scale = np.sin(gamma) / (2.0 * gamma)
+    else:
+        scale = 0.5
+
+    # Psi
+    px = scale * rx
+    py = scale * ry
+    pz = scale * rz
+
+    q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz
+    q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz
+    q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
+    q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
+    q[:] = _normalize(q)
+    return q
+
+
+@njit  # type: ignore[misc]
+def _update_quaternion_with_gibbs2(
+    q: NDArray[np.float64], da: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
+    as a scaled (2x) Gibbs vector.
+
+    As described in ref [1]_, this correction can be simplified by doing it in two
+    steps: first a correction, followed by renormalization. The scaling factor becomes
+    obsolete due to the renormalization step.
+
+    Parameters
+    ----------
+    q : ndarray, shape (4,)
+        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
+    da : ndarray, shape (3,)
+        Attitude error correction parameterized as a scaled (2x) Gibbs vector.
+
+    References
+    ----------
+    .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
+    and Control, Eq. (6.27)-(6.28).
+    """
+
+    qw, qx, qy, qz = q
+    dax, day, daz = da
+
+    q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz)
+    q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day)
+    q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
+    q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
+    q[:] = _normalize(q)
+    return q
+
+
+@njit  # type: ignore[misc]
+def _kalman_gain(
+    P: NDArray[np.float64], h: NDArray[np.float64], r: float
+) -> NDArray[np.float64]:
+    """
+    Compute the Kalman gain for a scalar measurement.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        State error covariance matrix.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n,)
+        Kalman gain vector.
+    """
+
+    Ph = np.dot(P, h)
+
+    # Innovation covariance
+    s = np.dot(h, Ph) + r
+
+    # Kalman gain
+    k = Ph / s
+
+    return k
+
+
+@njit  # type: ignore[misc]
+def _covariance_update(
+    P: NDArray[np.float64],
+    k: NDArray[np.float64],
+    h: NDArray[np.float64],
+    r: float,
+) -> NDArray[np.float64]:
+    """
+    Compute the updated error covariance matrix estimate (Joseph form).
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    k : ndarray, shape (n,)
+        Kalman gain vector.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    r : float
+        Scalar measurement noise variance.
+
+    Returns
+    -------
+    ndarray, shape (n, n)
+        Updated state error covariance matrix.
+    """
+    A = np.eye(k.size) - np.outer(k, h)
+    P = A @ P @ A.T + r * np.outer(k, k)
+    return P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_scalar(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: float,
+    r: float,
+    h: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Scalar Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    z : float
+        Scalar measurement.
+    r : float
+        Scalar measurement noise variance.
+    h : ndarray, shape (n,)
+        Measurement matrix (row vector).
+    """
+
+    # Kalman gain
+    k = _kalman_gain(P, h, r)
+
+    # Updated (a posteriori) state estimate
+    x[:] += k * (z - np.dot(h, x))
+
+    # Updated (a posteriori) covariance estimate (Joseph form)
+    P[:, :] = _covariance_update(P, k, h, r)
+    return x, P
+
+
+@njit  # type: ignore[misc]
+def _kalman_update_sequential(
+    x: NDArray[np.float64],
+    P: NDArray[np.float64],
+    z: NDArray[np.float64],
+    var: NDArray[np.float64],
+    H: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Sequential (one-at-a-time) Kalman filter measurement update.
+
+    Parameters
+    ----------
+    x : ndarray, shape (n,)
+        State estimate to be updated in place.
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be updated in place.
+    z : ndarray, shape (m,)
+        Measurement vector.
+    var : ndarray, shape (m,)
+        Measurement noise variances corresponding to each scalar measurement.
+    H : ndarray, shape (m, n)
+        Measurement matrix where each row corresponds to a scalar measurement model.
+    """
+    m = z.shape[0]
+    for i in range(m):
+        x, P = _kalman_update_scalar(x, P, z[i], var[i], H[i])
+    return x, P
+
+
+@njit  # type: ignore[misc]
+def _project_covariance_ahead(
+    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
+) -> NDArray[np.float64]:
+    """
+    Project the error covariance matrix estimate ahead.
+
+    Parameters
+    ----------
+    P : ndarray, shape (n, n)
+        Error covariance matrix to be projected ahead (in place).
+    phi : ndarray, shape (n, n)
+        State transition matrix.
+    Q : ndarray, shape (n, n)
+        Process noise covariance matrix.
+    """
+    P[:, :] = phi @ P @ phi.T + Q
+    return P
+
+
+def _nz2vg(nav_frame: str) -> float:
+    """
+    Gravity direction along the navigation frame's z-axis. Transforms the z-axis
+    of the navigation frame to a gravity reference vector (unit vector).
+
+    Parameters
+    ----------
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame.
+
+    Returns
+    -------
+    float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
+    """
+    if nav_frame.lower() == "ned":
+        return 1.0
+    elif nav_frame.lower() == "enu":
+        return -1.0
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+
+
+@njit  # type: ignore[misc]
+def _nz_b_from_quat(
+    q_nb: NDArray[np.float64], nav_frame_factor: float = 1.0
+) -> NDArray[np.float64]:
+    """
+    Unit vector describing the z-axis of frame {n} expressed in frame {b}, computed
+    from a unit quaternion, q_nb.
+
+    Note that this vector corresponds to the third row of the rotation matrix which
+    transforms a vector from {b} to {n}.
+
+    Parameters
+    ----------
+    q_nb : numpy.ndarray, shape (4,)
+        Unit quaternion which transforms a vector from frame {b} to frame {n}.
+    nav_frame_factor: float, default 1.0
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
+
+    Returns
+    -------
+    numpy.ndarray, shape (3,)
+        The z-axis (unit vector) of frame {n} expressed in frame {b}.
+    """
+
+    x = 2.0 * (q_nb[1] * q_nb[3] - q_nb[0] * q_nb[2])
+    y = 2.0 * (q_nb[2] * q_nb[3] + q_nb[0] * q_nb[1])
+    z = 1.0 - 2.0 * (q_nb[1] ** 2 + q_nb[2] ** 2)
+
+    return nav_frame_factor * np.array([x, y, z])
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
new file mode 100644
index 00000000..7f419638
--- /dev/null
+++ b/src/smsfusion/_ins/_vru.py
@@ -0,0 +1,334 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from smsfusion._transforms import _euler_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
+
+from ._aiding import _aiding_update_gref
+from ._common import (
+    _nz2vg,
+    _nz_b_from_quat,
+    _project_covariance_ahead,
+    _update_quaternion_with_gibbs2,
+    _update_quaternion_with_rotvec,
+)
+
+P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
+)
+
+
+def _state_transition_matrix_init(
+    dt: float,
+    dtheta: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (6, 6)
+        State transition matrix.
+    """
+    phi = np.eye(6)
+    phi[0:3, 0:3] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[0:3, 3:6] -= dt * np.eye(3)
+    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _state_transition_matrix_update(
+    phi: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+) -> NDArray[np.float64]:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (6, 6)
+        State transition matrix to be updated in place.
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    """
+    dtx, dty, dtz = dtheta
+
+    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
+    phi[0, 1] = dtz
+    phi[0, 2] = -dty
+    phi[1, 0] = -dtz
+    phi[1, 2] = dtx
+    phi[2, 0] = dty
+    phi[2, 1] = -dtx
+    return phi
+
+
+def _process_noise_covariance_matrix(
+    dt: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (9, 9)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((6, 6))
+    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
+    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+def _measurement_matrix_init() -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Returns
+    -------
+    ndarray, shape (3, 6)
+        Initial linearized measurement matrix.
+    """
+    return np.zeros((3, 6))
+
+
+@njit  # type: ignore[misc]
+def _reset(
+    dx: NDArray[np.float64], q_nb: NDArray[np.float64], bg_b: NDArray[np.float64]
+) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Reset state.
+
+    Parameters
+    ----------
+    dx : ndarray, shape (6,)
+        Error state vector containing the corrections to be applied to the state
+        estimates. Will be reset to zero after applying the corrections.
+    q_nb : ndarray, shape (4,)
+        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.
+    """
+    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[0:3])
+    bg_b[:] += dx[3:6]
+    dx[:] = 0.0
+    return dx, q_nb, bg_b
+
+
+class VRU:
+    """
+    Vertical Reference Unit (VRU) using a multiplicative extended
+    Kalman filter (MEKF). Uses only gravitational vector as aiding.
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    q : Attitude or array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix. Defaults to
+        a small diagonal matrix (1e-6 * np.eye(9)).
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
+        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
+        noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    nav_frame : {'NED', 'ENU'}, optional
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    """
+
+    def __init__(
+        self,
+        fs: float,
+        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = P0,
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.00005,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        nav_frame: str = "NED",
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
+        self._dx = np.zeros(6)
+
+        # Discrete state-space model
+        self._phi = _state_transition_matrix_init(
+            self._dt,
+            np.zeros(3),
+            self._gbc,
+        )
+        self._Q = _process_noise_covariance_matrix(
+            self._dt, self._arw, self._gbs, self._gbc
+        )
+        self._H = _measurement_matrix_init()
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    def update(
+        self,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
+        degrees: bool = False,
+        gref: bool = True,
+        gref_var: ArrayLike = (0.001, 0.001, 0.001),
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        dvel : array_like, shape (3,), optional
+            Velocity increment (sculling integral) in m/s.
+        dtheta : array_like, shape (3,), optional
+            Attitude increment (coning integral) in radians.
+        degrees : bool, optional
+            Specifies whether the unit of the attitude increment, ``dtheta``, is
+            degrees or radians. Defaults to radians.
+        gref : bool, optional
+            Specifies whether to use accelerometer measurements (dv) and the known
+            direction of gravity as aiding. Defaults to ``True``.
+        gref_var : array_like, shape (3,), optional
+            Variance of gravity reference vector measurement noise (dimensionless).
+            Required for gravity reference vector aiding. Defaults to (0.001, 0.001, 0.001).
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
+
+        if degrees:
+            dtheta = np.radians(dtheta)
+
+        dtheta = dtheta - self._dt * self._bg_b
+
+        # Update state-space model
+        self._phi = _state_transition_matrix_update(self._phi, dtheta)
+
+        # Project (a priori) state estimates ahead
+        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+
+        # Project (a priori) error covariance matrix estimate ahead
+        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        if gref is True:
+            vg_b = _nz_b_from_quat(self._q_nb, self._nz2vg)
+            self._H[0:3, 0:3] = _skew_symmetric(vg_b)  # Update measurement matrix
+
+            self._dx, self._P = _aiding_update_gref(
+                self._dx, self._P, self._H[0:3], vg_b, dvel, np.asarray(gref_var)
+            )
+
+        # Reset state
+        self._dx, self._q_nb, self._bg_b = _reset(self._dx, self._q_nb, self._bg_b)
+
+        return self
diff --git a/src/smsfusion/_vru_v2.py b/src/smsfusion/_vru_v2.py
deleted file mode 100644
index 646dc2b2..00000000
--- a/src/smsfusion/_vru_v2.py
+++ /dev/null
@@ -1,614 +0,0 @@
-from typing import Self
-
-import numpy as np
-from numba import njit
-from numpy.typing import ArrayLike, NDArray
-
-from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from ._vectorops import _normalize, _skew_symmetric
-
-
-P0 = (
-    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
-)
-
-
-
-@njit  # type: ignore[misc]
-def _update_quaternion_with_gibbs2(
-    q: NDArray[np.float64], da: NDArray[np.float64]
-) -> None:
-    """
-    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector.
-
-    As described in ref [1]_, this correction can be simplified by doing it in two
-    steps: first a correction, followed by renormalization. The scaling factor becomes
-    obsolete due to the renormalization step.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
-    da : ndarray, shape (3,)
-        Attitude error correction parameterized as a scaled (2x) Gibbs vector.
-
-    References
-    ----------
-    .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination
-    and Control, Eq. (6.27)-(6.28).
-    """
-
-    qw, qx, qy, qz = q
-    dax, day, daz = da
-
-    q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz)
-    q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day)
-    q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
-    q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _update_quaternion_with_rotvec(
-    q: NDArray[np.float64], dtheta: NDArray[np.float64]
-) -> None:
-    """
-    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
-    as a rotation vector.
-
-    Parameters
-    ----------
-    q : ndarray, shape (4,)
-        Unit quaternion (qw, qx, qy, qz) to be updated (in place).
-    dtheta : ndarray, shape (3,)
-        Attitude increment (rotation vector).
-
-    References
-    ----------
-    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
-    """
-
-    qw, qx, qy, qz = q
-    rx, ry, rz = dtheta
-
-    gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2)
-    cos_gamma = np.cos(gamma)
-
-    if gamma >= 1e-5:
-        scale = np.sin(gamma) / (2.0 * gamma)
-    else:
-        scale = 0.5
-
-    # Psi
-    px = scale * rx
-    py = scale * ry
-    pz = scale * rz
-
-    q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz
-    q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz
-    q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
-    q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
-    q[:] = _normalize(q)
-
-
-@njit  # type: ignore[misc]
-def _kalman_gain(
-    P: NDArray[np.float64], h: NDArray[np.float64], r: float
-) -> NDArray[np.float64]:
-    """
-    Compute the Kalman gain for a scalar measurement.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        State error covariance matrix.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n,)
-        Kalman gain vector.
-    """
-
-    Ph = np.dot(P, h)
-
-    # Innovation covariance
-    s = np.dot(h, Ph) + r
-
-    # Kalman gain
-    k = Ph / s
-
-    return k
-
-
-@njit  # type: ignore[misc]
-def _covariance_update(
-    P: NDArray[np.float64],
-    k: NDArray[np.float64],
-    h: NDArray[np.float64],
-    r: float,
-) -> NDArray[np.float64]:
-    """
-    Compute the updated error covariance matrix estimate (Joseph form).
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    k : ndarray, shape (n,)
-        Kalman gain vector.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    r : float
-        Scalar measurement noise variance.
-
-    Returns
-    -------
-    ndarray, shape (n, n)
-        Updated state error covariance matrix.
-    """
-    A = np.eye(k.size) - np.outer(k, h)
-    P = A @ P @ A.T + r * np.outer(k, k)
-    return P
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_scalar(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: float,
-    r: float,
-    h: NDArray[np.float64],
-) -> None:
-    """
-    Scalar Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    z : float
-        Scalar measurement.
-    r : float
-        Scalar measurement noise variance.
-    h : ndarray, shape (n,)
-        Measurement matrix (row vector).
-    """
-
-    # Kalman gain
-    k = _kalman_gain(P, h, r)
-
-    # Updated (a posteriori) state estimate
-    x[:] += k * (z - np.dot(h, x))
-
-    # Updated (a posteriori) covariance estimate (Joseph form)
-    P[:, :] = _covariance_update(P, k, h, r)
-
-
-@njit  # type: ignore[misc]
-def _kalman_update_sequential(
-    x: NDArray[np.float64],
-    P: NDArray[np.float64],
-    z: NDArray[np.float64],
-    var: NDArray[np.float64],
-    H: NDArray[np.float64],
-) -> None:
-    """
-    Sequential (one-at-a-time) Kalman filter measurement update.
-
-    Parameters
-    ----------
-    x : ndarray, shape (n,)
-        State estimate to be updated in place.
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be updated in place.
-    z : ndarray, shape (m,)
-        Measurement vector.
-    var : ndarray, shape (m,)
-        Measurement noise variances corresponding to each scalar measurement.
-    H : ndarray, shape (m, n)
-        Measurement matrix where each row corresponds to a scalar measurement model.
-    """
-    m = z.shape[0]
-    for i in range(m):
-        _kalman_update_scalar(x, P, z[i], var[i], H[i])
-
-
-@njit  # type: ignore[misc]
-def _project_covariance_ahead(
-    P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
-) -> None:
-    """
-    Project the error covariance matrix estimate ahead.
-
-    Parameters
-    ----------
-    P : ndarray, shape (n, n)
-        Error covariance matrix to be projected ahead (in place).
-    phi : ndarray, shape (n, n)
-        State transition matrix.
-    Q : ndarray, shape (n, n)
-        Process noise covariance matrix.
-    """
-    P[:, :] = phi @ P @ phi.T + Q
-
-
-def _state_transition_matrix(
-    dt: float,
-    dtheta: NDArray[np.float64],
-    gbc: float,
-) -> NDArray[np.float64]:
-    """
-    State transition matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    dtheta : ndarray, shape (3,)
-        Attitude increment measurement (coning integral).
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (6, 6)
-        State transition matrix.
-    """
-    phi = np.eye(6)
-    phi[0:3, 0:3] -= _skew_symmetric(dtheta)  # NB! update each time step
-    phi[0:3, 3:6] -= dt * np.eye(3)
-    phi[3:6, 3:6] -= dt * np.eye(3) / gbc
-    return phi
-
-
-@njit  # type: ignore[misc]
-def _update_state_transition_matrix(
-    phi: NDArray[np.float64],
-    dtheta: NDArray[np.float64],
-) -> None:
-    """
-    Update the state transition matrix in place.
-
-    Parameters
-    ----------
-    phi : ndarray, shape (9, 9)
-        State transition matrix to be updated in place.
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    dtheta : ndarray, shape (3,)
-        Attitude increment measurement (coning integral).
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    """
-    dtx, dty, dtz = dtheta
-
-    # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b)
-    phi[0, 1] = dtz
-    phi[0, 2] = -dty
-    phi[1, 0] = -dtz
-    phi[1, 2] = dtx
-    phi[2, 0] = dty
-    phi[2, 1] = -dtx
-
-
-def _process_noise_covariance_matrix(
-    dt: float, arw: float, gbs: float, gbc: float
-) -> NDArray[np.float64]:
-    """
-    Process noise covariance matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    arw : float
-        Angular random walk (gyroscope noise density) in rad/√Hz.
-    gbs : float
-        Gyro bias stability (bias instability) in rad/s.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    Q : ndarray, shape (9, 9)
-        Process noise covariance matrix.
-    """
-    Q = np.zeros((6, 6))
-    Q[0:3, 0:3] = dt * arw**2 * np.eye(3)
-    Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
-    return Q
-
-
-@njit  # type: ignore[misc]
-def _reset(q_nb, bg_b, dx) -> None:
-    """
-    Reset state.
-
-    Parameters
-    ----------
-    q_nb : ndarray, shape (4,)
-        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,)
-        Error state vector containing the corrections to be applied to the state
-        estimates. Will be reset to zero after applying the corrections.
-    """
-    _update_quaternion_with_gibbs2(q_nb, dx[0:3])
-    bg_b[:] += dx[3:6]
-    dx[:] = 0.0
-
-
-@njit  # type: ignore[misc]
-def _nz_b_from_quat(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
-    """
-    Unit vector describing the z-axis of frame {n} expressed in frame {b}, computed
-    from a unit quaternion, q_nb.
-
-    Note that this vector corresponds to the third row of the rotation matrix which
-    transforms a vector from {b} to {n}.
-
-    Parameters
-    ----------
-    q_nb : numpy.ndarray, shape (4,)
-        Unit quaternion which transforms a vector from frame {b} to frame {n}.
-
-    Returns
-    -------
-    numpy.ndarray, shape (3,)
-        The z-axis (unit vector) of frame {n} expressed in frame {b}.
-    """
-
-    x = 2.0 * (q_nb[1] * q_nb[3] - q_nb[0] * q_nb[2])
-    y = 2.0 * (q_nb[2] * q_nb[3] + q_nb[0] * q_nb[1])
-    z = 1.0 - 2.0 * (q_nb[1] ** 2 + q_nb[2] ** 2)
-
-    return np.array([x, y, z])
-
-
-def _nz2vg(nav_frame: str) -> float:
-    """
-    Gravity direction along the navigation frame's z-axis. Transforms the z-axis
-    of the navigation frame to a gravity reference vector (unit vector).
-
-    Parameters
-    ----------
-    nav_frame : {'NED', 'ENU'}
-        Navigation frame.
-
-    Returns
-    -------
-    float
-        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
-        -1.0 for 'ENU'.
-    """
-    if nav_frame.lower() == "ned":
-        return 1.0
-    elif nav_frame.lower() == "enu":
-        return -1.0
-    else:
-        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
-
-
-class VRUv2:
-    """
-    Vertical Reference Unit (VRU) using a multiplicative extended
-    Kalman filter (MEKF). Uses only gravitational vector as aiding.
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    q : Attitude or array_like, shape (4,), optional
-        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
-        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg : array_like, shape (3,), optional
-        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P : array_like, shape (6, 6), optional
-        Initial (a priori) estimate of the error covariance matrix. Defaults to
-        a small diagonal matrix (1e-6 * np.eye(9)).
-    acc_noise_density : float, optional
-        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
-        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
-    gyro_noise_density : float, optional
-        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
-    gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
-        noise level).
-    gyro_bias_corr_time : float, optional
-        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    nav_frame : {'NED', 'ENU'}, optional
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
-
-    """
-    def __init__(
-        self,
-        fs: float,
-        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = P0,
-        acc_noise_density: float = 0.0007,
-        gyro_noise_density: float = 0.0001,
-        gyro_bias_stability: float = 0.00005,
-        gyro_bias_corr_time: float = 50.0,
-        nav_frame: str = "NED",
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-        self._nav_frame = nav_frame.lower()
-        self._nz2vg = _nz2vg(self._nav_frame)
-
-        # IMU noise parameters
-        self._vrw = acc_noise_density  # velocity random walk
-        self._arw = gyro_noise_density  # angular random walk
-        self._gbs = gyro_bias_stability  # gyro bias stability
-        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
-
-        # State and covariance estimates
-        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
-        self._dx = np.zeros(6)
-
-        # Discrete state-space model
-        self._phi = _state_transition_matrix(
-            self._dt,
-            np.zeros(3),
-            self._gbc,
-        )
-        self._Q = _process_noise_covariance_matrix(
-            self._dt, self._arw, self._gbs, self._gbc
-        )
-
-        self._dhdx = np.zeros((3, 6))
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles (roll, pitch, yaw).
-        """
-
-        theta = _euler_from_quaternion(self._q_nb)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta
-
-    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
-        """
-        bg_b = self._bg_b.copy()
-        if degrees:
-            bg_b = (180.0 / np.pi) * bg_b
-        return bg_b
-
-    @property
-    def P(self) -> NDArray[np.float64]:
-        """
-        Copy of the error covariance matrix estimate.
-        """
-        return self._P.copy()
-
-    def _aiding_update_gref(
-        self,
-        dvel: NDArray[np.float64],
-        gref_var: NDArray[np.float64] | None,
-        q_nb: NDArray[np.float64],
-    ) -> None:
-        """
-        Update state and covariance with gravity reference vector aiding measurement.
-        """
-
-        if gref_var is None:
-            raise ValueError("gref_var is not provided; required for gref aiding.")
-
-        vg_b = self._nz2vg * _nz_b_from_quat(q_nb)
-        dz = -_normalize(dvel) - vg_b
-        self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b)
-
-        _kalman_update_sequential(self._dx, self._P, dz, gref_var, self._dhdx[0:3])
-
-
-    def update(
-        self,
-        dvel: ArrayLike,
-        dtheta: ArrayLike,
-        degrees: bool = False,
-        gref: bool = True,
-        gref_var: ArrayLike = (0.001, 0.001, 0.001),
-    ) -> Self:
-        """
-        Update state estimates with IMU and aiding measurements.
-
-        Parameters
-        ----------
-        dvel : array_like, shape (3,), optional
-            Velocity increment (sculling integral) in m/s.
-        dtheta : array_like, shape (3,), optional
-            Attitude increment (coning integral) in radians.
-        degrees : bool, optional
-            Specifies whether the unit of the attitude increment, ``dtheta``, is
-            degrees or radians. Defaults to radians.
-        gref : bool, optional
-            Specifies whether to use accelerometer measurements (dv) and the known
-            direction of gravity as aiding. Defaults to ``True``.
-        gref_var : array_like, shape (3,), optional
-            Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding. Defaults to (0.001, 0.001, 0.001).
-
-        Returns
-        -------
-        AHRS
-            A reference to the instance itself after the update.
-        """
-
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
-
-        if degrees:
-            dtheta = np.radians(dtheta)
-
-        dtheta = dtheta - self._dt * self._bg_b
-
-        # Update state-space model
-        _update_state_transition_matrix(self._phi, dtheta)
-
-        # Project (a priori) state estimates ahead
-        _update_quaternion_with_rotvec(self._q_nb, dtheta)
-
-        # Project (a priori) error covariance matrix estimate ahead
-        _project_covariance_ahead(self._P, self._phi, self._Q)
-
-        # Update (a posteriori) state and covariance estimates with aiding measurements
-        if gref is True:
-            self._aiding_update_gref(
-                dvel,
-                gref_var,
-                self._q_nb
-            )
-
-        # Reset state
-        _reset(self._q_nb, self._bg_b, self._dx)
-
-        return self
diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py
index a21ddc75..ab21d85f 100644
--- a/tests/test_ins_v2.py
+++ b/tests/test_ins_v2.py
@@ -3,7 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins_v2 import AHRSv2
+from smsfusion._ins._ahrs import AHRSv2
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
     benchmark_full_pva_chirp_202311A,

From 56cf9861415c02d762d2bcf6c7265a92ab63e6d1 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 8 May 2026 13:56:47 +0200
Subject: [PATCH 095/217] uncomment

---
 src/smsfusion/__init__.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index bbddd973..5c276e3f 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,23 +1,23 @@
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg
-
-# from ._ins import _ahrs  # hidden
 from ._ins import AHRS, VRU, AHRSv2, AidedINS, FixedNED, StrapdownINS, VRUv2, gravity
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
 
 __all__ = [
     "AHRS",
-    # "AidedINS",
+    "AHRSv2",
+    "AidedINS",
     "benchmark",
     "constants",
     "calibrate",
     "FixedIntervalSmoother",
-    # "FixedNED",
-    # "gravity",
+    "FixedNED",
+    "gravity",
     "noise",
-    # "StrapdownINS",
+    "StrapdownINS",
     "VRU",
+    "VRUv2",
     "quaternion_from_euler",
     "ConingScullingAlg",
 ]

From 4d360950d0cde474169f778ac40a58b5d577871c Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Tue, 12 May 2026 16:12:40 +0200
Subject: [PATCH 096/217] refactoring

---
 src/smsfusion/_ins/__init__.py |   3 +-
 src/smsfusion/_ins/_ahrs.py    |   2 +-
 src/smsfusion/_ins/_utils.py   | 194 ++++++++++++++++++++++++++++++++
 src/smsfusion/_ins/_vru.py     |   2 +-
 tests/test_ins_/__init__.py    |   0
 tests/test_ins_/test_common.py | 196 +++++++++++++++++++++++++++++++++
 tests/test_ins_/test_utils.py  | 121 ++++++++++++++++++++
 7 files changed, 515 insertions(+), 3 deletions(-)
 create mode 100644 src/smsfusion/_ins/_utils.py
 create mode 100644 tests/test_ins_/__init__.py
 create mode 100644 tests/test_ins_/test_common.py
 create mode 100644 tests/test_ins_/test_utils.py

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index 4c367d3e..4c927426 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,3 +1,4 @@
 from ._ahrs import AHRS as AHRSv2
-from ._ains import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity
+from ._ains import AHRS, VRU, AidedINS, StrapdownINS
+from ._utils import FixedNED, gravity, roll_pitch_from_acc
 from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 30e3bbb4..8bb51abd 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -410,7 +410,7 @@ def update(
         dtheta = np.asarray(dtheta)
 
         if degrees:
-            dtheta = np.radians(dtheta)
+            dtheta = (np.pi / 180.0) * dtheta
 
         dtheta = dtheta - self._dt * self._bg_b
 
diff --git a/src/smsfusion/_ins/_utils.py b/src/smsfusion/_ins/_utils.py
new file mode 100644
index 00000000..16c4f187
--- /dev/null
+++ b/src/smsfusion/_ins/_utils.py
@@ -0,0 +1,194 @@
+import numpy as np
+
+from ._common import _signed_smallest_angle
+
+
+def euler_from_acc(f, nav_frame, yaw=0.0, degrees=False):
+    """
+    Estimate roll and pitch angles from specific force (i.e., accelerometer) measurement.
+    As yaw cannot be determined from specific force alone, the keyword argument ``yaw``
+    is returned.
+
+    Parameters
+    ----------
+    f: array-like
+        Specific force (i.e., acceleration) measurement vector (fx, fy, fz).
+    nav_frame: {'NED', 'ENU'}
+        Navigation frame. Should be either 'NED' or 'ENU'.
+    yaw: float, optional
+        Yaw value in radians to be returned. Default is 0.0.
+    degrees : bool, optional
+        Whether to return the Euler angles in degrees or radians. Default is ``False``
+        (radians).
+
+    Returns
+    -------
+    numpy.ndarray, shape (3,)
+        Euler angles (roll, pitch, yaw).
+    """
+    fx, fy, fz = f
+
+    if nav_frame.lower() == "ned":
+        roll = np.arctan2(-fy, -fz)
+        pitch = np.arctan2(fx, np.sqrt(fy**2 + fz**2))
+    elif nav_frame.lower() == "enu":
+        roll = np.arctan2(fy, fz)
+        pitch = -np.arctan2(fx, np.sqrt(fy**2 + fz**2))
+    else:
+        raise ValueError("Invalid navigation frame. Should be 'NED' or 'ENU'.")
+
+    theta = np.array([roll, pitch, yaw])
+
+    if degrees:
+        theta = (180.0 / np.pi) * theta
+
+    return theta
+
+
+def gravity(lat: float | None = None, degrees: bool = True) -> float:
+    """
+    Calculates the gravitational acceleration based on the World Geodetic System
+    (1984) Ellipsoidal Gravity Formula (WGS-84).
+
+    The WGS-84 formula is given by::
+
+        g = g_e * (1 - k * sin(lat)^2) / sqrt(1 - e^2 * sin(lat)^2)
+
+    where, ::
+
+        g_e = 9.780325335903891718546
+        k = 0.00193185265245827352087
+        e^2 = 0.006694379990141316996137
+
+    and ``lat`` is the latitude.
+
+    If no latitude is provided, the 'standard gravity', ``g_0``, is returned instead.
+    The standard gravity is by definition of the ISO/IEC 8000 given as
+    ``g_0 = 9.80665``.
+
+    Parameters
+    ----------
+    lat : float, optional
+        Latitude. If none provided, the 'standard gravity' is returned.
+    degrees : bool, optional
+        Specify whether the latitude, ``lat``, is in degrees or radians.
+        Applicapble only if ``lat`` is provided.
+    """
+    if lat is None:
+        g_0 = 9.80665  # standard gravity in m/s^2
+        return g_0
+
+    g_e = 9.780325335903891718546  # gravity at equator
+    k = 0.00193185265245827352087  # formula constant
+    e_2 = 0.006694379990141316996137  # spheroid's squared eccentricity
+
+    if degrees:
+        lat = (np.pi / 180.0) * lat
+
+    g = g_e * (1.0 + k * np.sin(lat) ** 2.0) / np.sqrt(1.0 - e_2 * np.sin(lat) ** 2.0)
+    return g  # type: ignore[no-any-return]  # numpy funcs declare Any as return when given scalar-like
+
+
+class FixedNED:
+    """
+    Convert position coordinates between a fixed NED frame (x, y, z) and ECEF frame
+    (lattitude, longitude, height).
+
+    The fixed NED frame is a tangential plane on the WGS-84 ellipsoid with its origin
+    fixed at the provided reference coordinates. It is assumed that the tangential
+    plane is close to the ellipsoid surface.
+
+    Parameters
+    ----------
+    lat_ref: float
+        Reference latitude coordinate in decimal degrees.
+    lon_ref: float
+        Reference longitude coordinate in decimal degrees.
+    height_ref: ref
+        Reference height coordinate in decimal degrees.
+    """
+
+    def __init__(self, lat_ref: float, lon_ref: float, height_ref: float) -> None:
+        self._lat_ref = lat_ref
+        self._lon_ref = lon_ref
+        self._height_ref = height_ref
+
+        radius_eq = 6_378_137  # equatorial radius (WGS-84)
+        radius_polar = 6_356_752.314245  # polar radius (WGS-84)
+
+        radius_ratio_squared = (radius_polar / radius_eq) ** 2
+        denom = np.cos(
+            self._lat_ref * (np.pi / 180.0)
+        ) ** 2 + radius_ratio_squared * np.sin(self._lat_ref * (np.pi / 180.0))
+
+        self._Rn = radius_eq / np.sqrt(denom)  # radius prime vertical
+        self._Rm = self._Rn * radius_ratio_squared / denom  # radius meridian
+
+        self._Rm_h = self._Rm + height_ref
+        self._Rn_h_cos = (self._Rn + height_ref) * np.cos(
+            self._lat_ref * (np.pi / 180.0)
+        )
+
+    def to_llh(self, x: float, y: float, z: float) -> tuple[float, float, float]:
+        """
+        Compute longitude, latitude, and height coordinates (WGS-84) from local
+        Cartesian coordinates in the fixed NED frame.
+
+        Parameters
+        ----------
+        x: float
+            Local x-coordinate in meters in the fixed NED frame.
+        y: float
+            Local y-coordinate in meters in the fixed NED frame.
+        z: float
+            Local z-coordinate in meters in the fixed NED frame.
+
+        Returns
+        -------
+        lat: float
+            Latitude coordinate in decimal degrees.
+        lon: float
+            Longitude coordinate in decimal degrees.
+        height: float
+            Height coordinate in meters.
+        """
+        dlat = (180.0 / np.pi) * x / self._Rm_h
+        dlon = (180.0 / np.pi) * y / self._Rn_h_cos
+
+        lat = _signed_smallest_angle(self._lat_ref + dlat, degrees=True)
+        lon = _signed_smallest_angle(self._lon_ref + dlon, degrees=True)
+        h = self._height_ref - z
+        return lat, lon, h
+
+    def to_xyz(
+        self, lat: float, lon: float, height: float
+    ) -> tuple[float, float, float]:
+        """
+        Compute local Cartesian coordinates in the fixed NED frame from longitude,
+        latitude, and height coordinates (WGS-84).
+
+        Parameters
+        ----------
+        lat: float
+            Latitude coordinate in decimal degrees.
+        lon: float
+            Longitude coordinate in decimal degrees.
+        height: float
+            Height coordinate in meters.
+
+        Returns
+        -------
+        x: float
+            Local x-coordinate in meters in the fixed NED frame.
+        y: float
+            Local y-coordinate in meters in the fixed NED frame.
+        z: float
+            Local z-coordinate in meters in the fixed NED frame.
+        """
+        dlat = lat - self._lat_ref
+        dlon = lon - self._lon_ref
+
+        x = (np.pi / 180.0) * dlat * self._Rm_h
+        y = (np.pi / 180.0) * dlon * self._Rn_h_cos
+        z = self._height_ref - height
+        return x, y, z
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 7f419638..05542c94 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -306,7 +306,7 @@ def update(
         dtheta = np.asarray(dtheta)
 
         if degrees:
-            dtheta = np.radians(dtheta)
+            dtheta = (np.pi / 180.0) * dtheta
 
         dtheta = dtheta - self._dt * self._bg_b
 
diff --git a/tests/test_ins_/__init__.py b/tests/test_ins_/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py
new file mode 100644
index 00000000..5abd5d1c
--- /dev/null
+++ b/tests/test_ins_/test_common.py
@@ -0,0 +1,196 @@
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from smsfusion._ins import _common
+
+
+@pytest.mark.parametrize(
+    "quaternion, dhda_expect",
+    [
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([0.0, 0.0, 1.0]),
+        ),
+        (
+            np.array([0.89442719, 0.4472136, 0.0, 0.0]),  # gibbs -> [1.0, 0.0, 0.0]
+            np.array([0.0, 10.0, 20.0]) / (4.0 + 1.0) ** 2,
+        ),
+        (
+            np.array([0.89442719, 0.0, 0.4472136, 0.0]),  # gibbs -> [0.0, 1.0, 0.0]
+            np.array([6.0, 0.0, 12.0]) / (4.0 - 1.0) ** 2,
+        ),
+        (
+            np.array([0.89442719, 0.0, 0.0, 0.4472136]),  # gibbs -> [0.0, 0.0, 1.0]
+            np.array([0.0, 0.0, 20.0]) / ((4.0 - 1.0) ** 2 * (1 + (4.0 / 3.0) ** 2)),
+        ),
+        (
+            np.array(
+                [0.92387953, 0.22094238, 0.22094238, 0.22094238]
+            ),  # gibbs -> [0.47829262, 0.47829262, 0.47829262]
+            np.array([0.06751864, 0.29609696, 0.87452584]),
+        ),
+    ],
+)
+def test__dhda(quaternion, dhda_expect):
+    dhda_out = _common._dhda_head(quaternion)
+    np.testing.assert_allclose(dhda_out, dhda_expect)
+
+
+@pytest.mark.parametrize(
+    "angles",
+    [
+        np.radians([0.0, 0.0, 35.0]),
+        np.radians([25.0, 180.0, -125.0]),
+        np.radians([10.0, 95.0, 1.0]),
+    ],
+)
+def test__h_head(angles):
+    alpha, beta, gamma = np.radians((0.0, 0.0, 15.0))
+
+    quaternion = Rotation.from_euler(
+        "ZYX", (gamma, beta, alpha), degrees=False
+    ).as_quat()
+    quaternion = np.r_[quaternion[3], quaternion[:3]]
+
+    gamma_expect = _common._h_head(quaternion)
+    assert gamma_expect == pytest.approx(gamma)
+
+
+@pytest.mark.parametrize(
+    "angle, degrees, angle_expect",
+    [
+        (0.0, True, 0.0),
+        (-180.0, True, -180.0),
+        (180.0, True, -180.0),
+        (-np.pi, False, -np.pi),
+        (np.pi, False, -np.pi),
+        (90.0, True, 90.0),
+        (-90.0, True, -90.0),
+        (181, True, -179.0),
+        (-181, True, 179.0),
+    ],
+)
+def test__signed_smallest_angle(angle, degrees, angle_expect):
+    assert _common._signed_smallest_angle(angle, degrees=degrees) == pytest.approx(
+        angle_expect
+    )
+
+
+@pytest.mark.parametrize(
+    "quaternion, dtheta, quaternion_update_expected",
+    [
+        # Identity quaternion, zero rotation → unchanged
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([0.0, 0.0, 0.0]),
+            np.array([1.0, 0.0, 0.0, 0.0]),
+        ),
+        # Identity quaternion, 90° rotation around X-axis
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([np.pi / 2, 0.0, 0.0]),
+            np.array([np.cos(np.pi / 4), np.sin(np.pi / 4), 0.0, 0.0]),
+        ),
+        # Identity quaternion, 90° rotation around Y-axis
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([0.0, np.pi / 2, 0.0]),
+            np.array([np.cos(np.pi / 4), 0.0, np.sin(np.pi / 4), 0.0]),
+        ),
+        # Identity quaternion, 90° rotation around Z-axis
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([0.0, 0.0, np.pi / 2]),
+            np.array([np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)]),
+        ),
+        # Identity quaternion, 180° rotation around X-axis
+        (
+            np.array([1.0, 0.0, 0.0, 0.0]),
+            np.array([np.pi, 0.0, 0.0]),
+            np.array([0.0, 1.0, 0.0, 0.0]),
+        ),
+        # Non-identity quaternion (90° around Z), zero rotation → unchanged
+        (
+            np.array([np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)]),
+            np.array([0.0, 0.0, 0.0]),
+            np.array([np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)]),
+        ),
+    ],
+)
+def test__update_quaternion_with_rotvec(quaternion, dtheta, quaternion_update_expected):
+    quaternion_update = _common._update_quaternion_with_rotvec(quaternion, dtheta)
+
+    assert np.isclose(
+        np.linalg.norm(quaternion_update), 1.0
+    ), f"Output quaternion is not unit norm: {quaternion_update}"
+
+    np.testing.assert_allclose(
+        quaternion_update, quaternion_update_expected, atol=1e-16
+    )
+
+
+@pytest.mark.parametrize(
+    ("quaternion", "da", "quaternion_update_expected"),
+    [
+        # Identity quaternion, no correction
+        (
+            np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64),
+            np.array([0.0, 0.0, 0.0], dtype=np.float64),
+            np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64),
+        ),
+        # Small x-axis correction
+        (
+            np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64),
+            np.array([0.1, 0.0, 0.0], dtype=np.float64),
+            np.array(
+                [
+                    0.9987523388778446,
+                    0.04993761694389223,
+                    0.0,
+                    0.0,
+                ],
+                dtype=np.float64,
+            ),
+        ),
+        # Small y-axis correction
+        (
+            np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64),
+            np.array([0.0, 0.1, 0.0], dtype=np.float64),
+            np.array(
+                [
+                    0.9987523388778446,
+                    0.0,
+                    0.04993761694389223,
+                    0.0,
+                ],
+                dtype=np.float64,
+            ),
+        ),
+        # Small z-axis correction
+        (
+            np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64),
+            np.array([0.0, 0.0, 0.1], dtype=np.float64),
+            np.array(
+                [
+                    0.9987523388778446,
+                    0.0,
+                    0.0,
+                    0.04993761694389223,
+                ],
+                dtype=np.float64,
+            ),
+        ),
+    ],
+)
+def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expected):
+    quaternion_update = _common._update_quaternion_with_gibbs2(quaternion.copy(), da)
+
+    # Always assert unit norm
+    assert np.isclose(
+        np.linalg.norm(quaternion_update), 1.0
+    ), f"Output quaternion is not unit norm: {quaternion_update}"
+
+    np.testing.assert_allclose(
+        quaternion_update, quaternion_update_expected, atol=1e-10
+    )
diff --git a/tests/test_ins_/test_utils.py b/tests/test_ins_/test_utils.py
new file mode 100644
index 00000000..76182b14
--- /dev/null
+++ b/tests/test_ins_/test_utils.py
@@ -0,0 +1,121 @@
+import numpy as np
+import pytest
+
+from smsfusion._ins import _utils
+from smsfusion._transforms import _rot_matrix_from_quaternion, quaternion_from_euler
+
+
+@pytest.mark.parametrize(
+    "euler",
+    [
+        np.radians([10.0, 45.0, 0.0]),
+        np.radians([0.0, 0.0, 0.0]),
+        np.radians([90.0, 0.0, 0.0]),
+        np.radians([180.0, 0.0, 0.0]),
+        np.radians([130.0, -28.0, 90.0]),
+    ],
+)
+def test__roll_pitch_from_acc(euler):
+    R_nm = _rot_matrix_from_quaternion(quaternion_from_euler(euler))  # body-to-nav
+    g = _utils.gravity()
+
+    # North-East-Down (NED) frame
+    g_ned = np.array([0.0, 0.0, -g])
+    acc_ned = R_nm.T @ g_ned
+    roll_pitch_ned = _utils.roll_pitch_from_acc(acc_ned, nav_frame="NED")
+    np.testing.assert_allclose(roll_pitch_ned, euler[:2])
+
+    # North-East-Up (ENU) frame
+    g_enu = np.array([0.0, 0.0, g])
+    acc_enu = R_nm.T @ g_enu
+    roll_pitch_enu = _utils.roll_pitch_from_acc(acc_enu, nav_frame="ENU")
+    np.testing.assert_allclose(roll_pitch_enu, euler[:2])
+
+
+@pytest.mark.parametrize(
+    "mu, g_expect",
+    [
+        (None, 9.80665),
+        (0.0, 9.780325335903891718546),
+        (90.0, 9.8321849378634),
+        (59.91, 9.81910618638375),
+    ],
+)
+def test_gravity(mu, g_expect):
+    g_out = _utils.gravity(mu)
+    assert g_out == pytest.approx(g_expect)
+
+
+class Test_FixedNed:
+    def test_init(self):
+        _ = _utils.FixedNED(0.0, 0.0, 0.0)
+
+    @pytest.mark.parametrize(
+        "lat, lon, height, x, y, z",
+        [
+            (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+            (0.0, 0.0, 1.0, 0.0, 0.0, -1.0),
+            (0.1, 0.0, 0.0, pytest.approx(11057.4, abs=0.05), 0.0, 0.0),
+            (-0.1, 0.0, 0.0, pytest.approx(-11057.4, abs=0.05), 0.0, 0.0),
+            (0.0, 0.1, 0.0, 0.0, pytest.approx(11131.9, abs=0.05), 0.0),
+            (0.0, -0.1, 0.0, 0.0, pytest.approx(-11131.9, abs=0.05), 0.0),
+            (
+                0.1,
+                0.1,
+                0.0,
+                pytest.approx(11057.4, abs=0.05),
+                pytest.approx(11131.9, abs=0.05),
+                0.0,
+            ),
+            (
+                -0.1,
+                -0.1,
+                0.0,
+                pytest.approx(-11057.4, abs=0.05),
+                pytest.approx(-11131.9, abs=0.05),
+                0.0,
+            ),
+        ],
+    )
+    def test_to_xyz(self, lat, lon, height, x, y, z):
+        ned = _utils.FixedNED(0.0, 0.0, 0.0)
+
+        x_, y_, z_ = ned.to_xyz(lat, lon, height)
+        assert x_ == x
+        assert y_ == y
+        assert z_ == z
+
+    @pytest.mark.parametrize(
+        "lat, lon, height, x, y, z",
+        [
+            (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+            (0.0, 0.0, 1.0, 0.0, 0.0, -1.0),
+            (pytest.approx(0.1, abs=1e-4), 0.0, 0.0, 11057.4, 0.0, 0.0),
+            (pytest.approx(-0.1, abs=1e-4), 0.0, 0.0, -11057.4, 0.0, 0.0),
+            (0.0, pytest.approx(0.1, abs=1e-4), 0.0, 0.0, 11131.9, 0.0),
+            (0.0, pytest.approx(-0.1, abs=1e-4), 0.0, 0.0, -11131.9, 0.0),
+            (
+                pytest.approx(0.1, abs=1e-4),
+                pytest.approx(0.1, abs=1e-4),
+                0.0,
+                11057.4,
+                11131.9,
+                0.0,
+            ),
+            (
+                pytest.approx(-0.1, abs=1e-4),
+                pytest.approx(-0.1, abs=1e-4),
+                0.0,
+                -11057.4,
+                -11131.9,
+                0.0,
+            ),
+        ],
+    )
+    def test_to_llh(self, lat, lon, height, x, y, z):
+        ned = _utils.FixedNED(0.0, 0.0, 0.0)
+
+        lat_, lon_, height_ = ned.to_llh(x, y, z)
+        assert lat_ == lat
+        assert lon_ == lon
+        assert height_ == height

From dc5ee66a8b3d35d3dd5da4b8b4a1694ce8a22739 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Tue, 12 May 2026 16:20:22 +0200
Subject: [PATCH 097/217] fix euler_acc

---
 src/smsfusion/_ins/__init__.py |  2 +-
 tests/test_ins_/test_utils.py  | 24 ++++++++++++++++--------
 2 files changed, 17 insertions(+), 9 deletions(-)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index 4c927426..bc526a66 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,4 +1,4 @@
 from ._ahrs import AHRS as AHRSv2
 from ._ains import AHRS, VRU, AidedINS, StrapdownINS
-from ._utils import FixedNED, gravity, roll_pitch_from_acc
+from ._utils import FixedNED, gravity, euler_from_acc
 from ._vru import VRU as VRUv2
diff --git a/tests/test_ins_/test_utils.py b/tests/test_ins_/test_utils.py
index 76182b14..e8ee100e 100644
--- a/tests/test_ins_/test_utils.py
+++ b/tests/test_ins_/test_utils.py
@@ -9,27 +9,35 @@
     "euler",
     [
         np.radians([10.0, 45.0, 0.0]),
-        np.radians([0.0, 0.0, 0.0]),
-        np.radians([90.0, 0.0, 0.0]),
-        np.radians([180.0, 0.0, 0.0]),
+        np.radians([0.0, 0.0, -10.0]),
+        np.radians([90.0, 0.0, -45.0]),
+        np.radians([180.0, 0.0, 10.0]),
         np.radians([130.0, -28.0, 90.0]),
     ],
 )
-def test__roll_pitch_from_acc(euler):
+def test_euler_from_acc(euler):
     R_nm = _rot_matrix_from_quaternion(quaternion_from_euler(euler))  # body-to-nav
     g = _utils.gravity()
+    euler_degrees = np.degrees(euler)
 
     # North-East-Down (NED) frame
     g_ned = np.array([0.0, 0.0, -g])
     acc_ned = R_nm.T @ g_ned
-    roll_pitch_ned = _utils.roll_pitch_from_acc(acc_ned, nav_frame="NED")
-    np.testing.assert_allclose(roll_pitch_ned, euler[:2])
+    euler_ned = _utils.euler_from_acc(acc_ned, nav_frame="NED", yaw=euler[2])
+    np.testing.assert_allclose(euler_ned, euler)
+
+    euler_ned = _utils.euler_from_acc(acc_ned, nav_frame="NED", yaw=euler[2], degrees=True)
+    np.testing.assert_allclose(euler_ned, euler_degrees)
+
 
     # North-East-Up (ENU) frame
     g_enu = np.array([0.0, 0.0, g])
     acc_enu = R_nm.T @ g_enu
-    roll_pitch_enu = _utils.roll_pitch_from_acc(acc_enu, nav_frame="ENU")
-    np.testing.assert_allclose(roll_pitch_enu, euler[:2])
+    euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2])
+    np.testing.assert_allclose(euler_enu, euler)
+
+    euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2], degrees=True)
+    np.testing.assert_allclose(euler_enu, euler_degrees)
 
 
 @pytest.mark.parametrize(

From 32fff5e1e3963e8bb040e01f6a4414ae38d50a3f Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 12 Jun 2026 11:13:38 +0200
Subject: [PATCH 098/217] adding VRU + fixes + tests

---
 src/smsfusion/_ins/_vru.py                |   6 +-
 tests/{test_ins.py => _test_ins.py}       |  10 +-
 tests/{test_ins_v2.py => _test_ins_v2.py} |   0
 tests/test_ins_/test_vru.py               | 157 ++++++++++++++++++++++
 4 files changed, 164 insertions(+), 9 deletions(-)
 rename tests/{test_ins.py => _test_ins.py} (99%)
 rename tests/{test_ins_v2.py => _test_ins_v2.py} (100%)
 create mode 100644 tests/test_ins_/test_vru.py

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 05542c94..1a8b42f4 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -310,15 +310,13 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        # Update state-space model
+        # Update state-space model and project (a priori) error covariance matrix estimate ahead
         self._phi = _state_transition_matrix_update(self._phi, dtheta)
+        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
 
         # Project (a priori) state estimates ahead
         self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
-        # Project (a priori) error covariance matrix estimate ahead
-        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
-
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if gref is True:
             vg_b = _nz_b_from_quat(self._q_nb, self._nz2vg)
diff --git a/tests/test_ins.py b/tests/_test_ins.py
similarity index 99%
rename from tests/test_ins.py
rename to tests/_test_ins.py
index 375b40a2..a6e98910 100644
--- a/tests/test_ins.py
+++ b/tests/_test_ins.py
@@ -23,12 +23,12 @@
     VRU,
     AidedINS,
     FixedNED,
-    INSMixin,
+    # INSMixin,
     StrapdownINS,
-    _dhda_head,
-    _h_head,
-    _roll_pitch_from_acc,
-    _signed_smallest_angle,
+    # _dhda_head,
+    # _h_head,
+    # _roll_pitch_from_acc,
+    # _signed_smallest_angle,
     gravity,
 )
 from smsfusion._transforms import (
diff --git a/tests/test_ins_v2.py b/tests/_test_ins_v2.py
similarity index 100%
rename from tests/test_ins_v2.py
rename to tests/_test_ins_v2.py
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
new file mode 100644
index 00000000..d224975d
--- /dev/null
+++ b/tests/test_ins_/test_vru.py
@@ -0,0 +1,157 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion._ins._vru import VRU, _state_transition_matrix_init, _state_transition_matrix_update
+from smsfusion.benchmark import (
+    benchmark_pure_attitude_beat_202311A,
+    benchmark_pure_attitude_chirp_202311A
+)
+
+
+def test_state_transition_matrix_init():
+    dt = 0.1
+    dtheta = np.ones(3) * 0.02
+    gbc = 0.01
+
+    phi_out = _state_transition_matrix_init(dt, dtheta, gbc)
+    phi_expected = np.array([
+        [1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+        [-0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+        [0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_state_transition_matrix_update():
+    dt = 0.1
+    dtheta = np.ones(3) * 0.02
+    gbc = 0.01
+
+    phi_init = _state_transition_matrix_init(dt, dtheta, gbc)
+
+    dtheta_update = np.ones(3) * 0.01
+    phi_out = _state_transition_matrix_update(phi_init, dtheta=dtheta_update)
+
+    phi_expected = np.array([
+        [1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+        [-0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+        [0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_vru_init():
+    mekf = VRU(
+        10.0
+        )
+
+    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+    np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru.P0))
+
+
+@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+def test_vru_nav_frame(nav_frame, scale):
+    mekf = VRU(
+        10.0,
+        nav_frame=nav_frame
+        )
+
+    assert mekf._nz2vg == scale
+
+
+def test_vru_methods():
+    euler_init = np.array([10.0, 20.0, 30.0])
+    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
+    bg_init = np.array([0.01, -0.01, 0.02])
+
+    mekf = VRU(
+        10.0,
+        q=quaternion_init,
+        bg=bg_init
+        )
+
+    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
+    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
+    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen",
+    [benchmark_pure_attitude_beat_202311A, benchmark_pure_attitude_chirp_202311A],
+)
+def test_vru_benchmark(benchmark_gen):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = VRU(
+        fs_imu,
+        q=q0,
+        acc_noise_density=sf.constants.ERR_ACC_MOTION2["N"],
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i) in enumerate(
+        zip(acc_noise, gyro_noise)
+    ):
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(
+            dvel,
+            dtheta,
+            degrees=False,
+            gref=True
+        )
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005

From eeb4ab1bab2c554b978260bb2c5ae98494b87cd1 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 12 Jun 2026 11:19:16 +0200
Subject: [PATCH 099/217] skip test if tqdm installed

---
 tests/test_noise.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/tests/test_noise.py b/tests/test_noise.py
index 6154244f..625dd0b6 100644
--- a/tests/test_noise.py
+++ b/tests/test_noise.py
@@ -383,10 +383,14 @@ def test_different_seeds(self, err_acc_scalar):
 
 
 class Test_allan_var_overlapping:
-    def test_no_tqdm(self, monkeypatch):
-        import sys
 
-        monkeypatch.delitem(sys.modules, "tqdm", raising=False)
+    @staticmethod
+    def _tqdm_installed():
+        import importlib.util
+        return importlib.util.find_spec("tqdm") is not None
+
+    @pytest.mark.skipif(_tqdm_installed(), reason="tqdm is installed")
+    def test_no_tqdm(self):
         with pytest.raises(ImportError):
             y = np.random.random(1_000)
             tau, avar = allan_var(y, 10.0, progress=True)

From a9be028ec180ab4ba4479d4adf3b5a3f5d010cb0 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 12 Jun 2026 11:20:12 +0200
Subject: [PATCH 100/217] docstring fix

---
 src/smsfusion/_ins/_vru.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 1a8b42f4..d032a3a4 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -101,7 +101,7 @@ def _process_noise_covariance_matrix(
 
     Returns
     -------
-    Q : ndarray, shape (9, 9)
+    Q : ndarray, shape (6, 6)
         Process noise covariance matrix.
     """
     Q = np.zeros((6, 6))

From c977ecd27df6239884435f091d068f55e7e144c2 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 12 Jun 2026 12:33:02 +0200
Subject: [PATCH 101/217] add more vru tests

---
 src/smsfusion/_ins/_vru.py  |  5 -----
 tests/test_ins_/test_vru.py | 35 +++++++++++++++++++++++++++++++++--
 2 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index d032a3a4..d5b98100 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -162,9 +162,6 @@ class VRU:
     P : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
         a small diagonal matrix (1e-6 * np.eye(9)).
-    acc_noise_density : float, optional
-        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
-        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
     gyro_noise_density : float, optional
         Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
         0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
@@ -186,7 +183,6 @@ def __init__(
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = P0,
-        acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
         gyro_bias_corr_time: float = 50.0,
@@ -198,7 +194,6 @@ def __init__(
         self._nz2vg = _nz2vg(self._nav_frame)
 
         # IMU noise parameters
-        self._vrw = acc_noise_density  # velocity random walk
         self._arw = gyro_noise_density  # angular random walk
         self._gbs = gyro_bias_stability  # gyro bias stability
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
index d224975d..5d139e8a 100644
--- a/tests/test_ins_/test_vru.py
+++ b/tests/test_ins_/test_vru.py
@@ -3,7 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._vru import VRU, _state_transition_matrix_init, _state_transition_matrix_update
+from smsfusion._ins._vru import VRU, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
 from smsfusion.benchmark import (
     benchmark_pure_attitude_beat_202311A,
     benchmark_pure_attitude_chirp_202311A
@@ -50,6 +50,38 @@ def test_state_transition_matrix_update():
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
 
+def test_measurement_matrix_init():
+    np.testing.assert_array_equal(_measurement_matrix_init(), np.zeros((3, 6)))
+
+
+def test_process_noise_covariance_matrix():
+    dt = 0.1
+    arw = 0.00005
+    gbs = 0.00005
+    gbc = 50.0
+    Q_out = _process_noise_covariance_matrix(dt, arw, gbs, gbc)
+    Q_expect = np.array([
+        [dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+    ])
+
+
+def test_reset():
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    bg_b = np.zeros(3)
+    dx = np.array([0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+
+    dx, q_nb, bg_b = _reset(dx, q_nb, bg_b)
+
+    np.testing.assert_allclose(dx, np.zeros_like(dx))
+    np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
+    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+
+
 def test_vru_init():
     mekf = VRU(
         10.0
@@ -115,7 +147,6 @@ def test_vru_benchmark(benchmark_gen):
     mekf = VRU(
         fs_imu,
         q=q0,
-        acc_noise_density=sf.constants.ERR_ACC_MOTION2["N"],
         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],

From bd5d5cb9062125e916c36118483afdd8bcb7c848 Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 12 Jun 2026 13:14:13 +0200
Subject: [PATCH 102/217] fix test

---
 tests/test_ins_/test_vru.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
index 5d139e8a..bae5039e 100644
--- a/tests/test_ins_/test_vru.py
+++ b/tests/test_ins_/test_vru.py
@@ -69,6 +69,7 @@ def test_process_noise_covariance_matrix():
         [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
     ])
 
+    np.testing.assert_allclose(Q_out, Q_expect)
 
 def test_reset():
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])

From 986e313ce930c80409e72fd4bbcd0ac8caf545db Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Thu, 18 Jun 2026 14:47:17 +0200
Subject: [PATCH 103/217] tests

---
 src/smsfusion/_ins/_common.py  |  5 +++--
 src/smsfusion/_ins/_vru.py     |  3 +++
 tests/conftest.py              |  2 ++
 tests/test_ins_/test_common.py | 25 +++++++++++++++++++++++++
 tests/test_ins_/test_utils.py  |  6 ++++++
 tests/test_ins_/test_vru.py    | 12 ++++++++----
 6 files changed, 47 insertions(+), 6 deletions(-)
 create mode 100644 tests/conftest.py

diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index 8ffccb2a..3a985918 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -44,7 +44,7 @@ def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
     return dhda  # type: ignore[no-any-return]
 
 
-@njit
+@njit  # type: ignore[misc]
 def _h_head(q: NDArray[np.float64]) -> float:
     """
     Compute yaw angle from unit quaternion.
@@ -73,7 +73,7 @@ def _h_head(q: NDArray[np.float64]) -> float:
     return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
 
 
-@njit
+@njit  # type: ignore[misc]
 def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
     """
     Convert the given angle to the smallest angle between [-180., 180) degrees.
@@ -324,6 +324,7 @@ def _project_covariance_ahead(
     return P
 
 
+@njit  # type: ignore[misc]
 def _nz2vg(nav_frame: str) -> float:
     """
     Gravity direction along the navigation frame's z-axis. Transforms the z-axis
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index d5b98100..78214527 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -26,6 +26,7 @@
 )
 
 
+@njit  # type: ignore[misc]
 def _state_transition_matrix_init(
     dt: float,
     dtheta: NDArray[np.float64],
@@ -82,6 +83,7 @@ def _state_transition_matrix_update(
     return phi
 
 
+@njit  # type: ignore[misc]
 def _process_noise_covariance_matrix(
     dt: float, arw: float, gbs: float, gbc: float
 ) -> NDArray[np.float64]:
@@ -110,6 +112,7 @@ def _process_noise_covariance_matrix(
     return Q
 
 
+@njit  # type: ignore[misc]
 def _measurement_matrix_init() -> NDArray[np.float64]:
     """
     Measurement matrix.
diff --git a/tests/conftest.py b/tests/conftest.py
new file mode 100644
index 00000000..dd641f30
--- /dev/null
+++ b/tests/conftest.py
@@ -0,0 +1,2 @@
+import os
+os.environ["NUMBA_DISABLE_JIT"] = "1"
\ No newline at end of file
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py
index 5abd5d1c..4a916883 100644
--- a/tests/test_ins_/test_common.py
+++ b/tests/test_ins_/test_common.py
@@ -194,3 +194,28 @@ def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expect
     np.testing.assert_allclose(
         quaternion_update, quaternion_update_expected, atol=1e-10
     )
+
+
+@pytest.mark.parametrize(
+    "q_nb,nav_frame_factor",
+    [
+        (np.array([1.0, 0.0, 0.0, 0.0]), 1.0),
+        (np.array([1.0, 0.0, 0.0, 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), 1.0),
+        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), 1.0),
+        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), -1.0),
+        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), 1.0)
+    ]
+)
+def test__nz_b_from_quat(q_nb, nav_frame_factor):
+    out = _common._nz_b_from_quat(q_nb, nav_frame_factor=nav_frame_factor)
+
+    expect = Rotation.from_quat(q_nb, scalar_first=True).apply(nav_frame_factor * np.array([0.0, 0.0, 1.0]), inverse=True)
+    np.testing.assert_allclose(out, expect)
+
+
+def test__nz2vg():
+    assert _common._nz2vg("NED") == 1.0
+    assert _common._nz2vg("ENU") == -1.0
\ No newline at end of file
diff --git a/tests/test_ins_/test_utils.py b/tests/test_ins_/test_utils.py
index e8ee100e..957b2799 100644
--- a/tests/test_ins_/test_utils.py
+++ b/tests/test_ins_/test_utils.py
@@ -39,6 +39,12 @@ def test_euler_from_acc(euler):
     euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2], degrees=True)
     np.testing.assert_allclose(euler_enu, euler_degrees)
 
+def test_euler_from_acc_invalid_frame():
+ 
+    acc_ned = np.array([0.0, 0.0, 1])
+    with pytest.raises(ValueError):
+        _utils.euler_from_acc(acc_ned, nav_frame="STAR")
+
 
 @pytest.mark.parametrize(
     "mu, g_expect",
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
index bae5039e..d8197b0b 100644
--- a/tests/test_ins_/test_vru.py
+++ b/tests/test_ins_/test_vru.py
@@ -122,10 +122,10 @@ def test_vru_methods():
 
 
 @pytest.mark.parametrize(
-    "benchmark_gen",
-    [benchmark_pure_attitude_beat_202311A, benchmark_pure_attitude_chirp_202311A],
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
 )
-def test_vru_benchmark(benchmark_gen):
+def test_vru_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
@@ -143,6 +143,9 @@ def test_vru_benchmark(benchmark_gen):
     acc_noise = acc_ref + imu_noise[:, :3]
     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
 
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+
     # MEKF
     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
     mekf = VRU(
@@ -158,13 +161,14 @@ def test_vru_benchmark(benchmark_gen):
     for i, (f_i, w_i) in enumerate(
         zip(acc_noise, gyro_noise)
     ):
+
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
 
         mekf.update(
             dvel,
             dtheta,
-            degrees=False,
+            degrees=degrees,
             gref=True
         )
 

From 09cec81b4e423d63fb33a7ef09b9f7afdaef3c2d Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Thu, 2 Jul 2026 10:56:04 +0200
Subject: [PATCH 104/217] fix tests

---
 src/smsfusion/_ins/_ahrs.py  |   2 +-
 tests/test_ins_/test_ahrs.py | 308 +++++++++++++++++++++++++++++++++++
 2 files changed, 309 insertions(+), 1 deletion(-)
 create mode 100644 tests/test_ins_/test_ahrs.py

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 8bb51abd..8c3961a5 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -155,7 +155,7 @@ def _measurement_matrix_init(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
 
     Returns
     -------
-    ndarray, shape (4, 6)
+    ndarray, shape (4, 9)
         Linearized measurement matrix.
     """
     dhdx = np.zeros((4, 9))
diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
new file mode 100644
index 00000000..bcd2dfd1
--- /dev/null
+++ b/tests/test_ins_/test_ahrs.py
@@ -0,0 +1,308 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion._ins._ahrs import AHRS, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
+from smsfusion.benchmark import (
+    benchmark_pure_attitude_beat_202311A,
+    benchmark_pure_attitude_chirp_202311A
+)
+
+
+def test_state_transition_matrix_init():
+    dt = 0.1
+    dvel = np.ones(3) * 0.01
+    dtheta = np.ones(3) * 0.02
+    R_nb = np.eye(3)
+    gbc = 0.01
+
+    phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+    phi_expected = np.array([
+        [1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_state_transition_matrix_update():
+    dt = 0.1
+    dvel = np.ones(3) * 0.01
+    dtheta = np.ones(3) * 0.02
+    R_nb = np.eye(3)
+    gbc = 0.01
+
+    phi_init = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+
+    dtheta_update = np.ones(3) * 0.01
+    dvel_update = np.ones(3) * 0.1
+    phi_out = _state_transition_matrix_update(
+        phi_init,
+        dvel=dvel_update,
+        dtheta=dtheta_update,
+        R_nb=R_nb
+        )
+
+    phi_expected = np.array([
+        [1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_measurement_matrix_init():
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+
+    expect = np.zeros((4, 9))
+    expect[0:3, 0:3] = np.eye(3)
+    expect[3, 3:6] = np.array([0.0, 0.0, 1.0])
+    # kappa -> zero due to unit quat
+
+    np.testing.assert_array_equal(_measurement_matrix_init(q_nb), expect)
+
+
+def test_process_noise_covariance_matrix():
+    dt = 0.1
+    vrw = 0.0005
+    arw = 0.00005
+    gbs = 0.00005
+    gbc = 50.0
+    Q_out = _process_noise_covariance_matrix(dt, vrw, arw, gbs, gbc)
+    Q_expect = np.array([
+        [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+    ])
+
+    np.testing.assert_allclose(Q_out, Q_expect)
+
+def test_reset():
+    v_n = np.array([0.0, 0.1, 0.0])
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    bg_b = np.zeros(3)
+    dx = np.array([0.1, 0.0, 0.0, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+
+    dx, v_n, q_nb, bg_b = _reset(dx, v_n, q_nb, bg_b)
+
+    np.testing.assert_allclose(dx, np.zeros_like(dx))
+    np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
+    np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
+    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+
+
+def test_ahrs_init():
+    mekf = AHRS(
+        10.0
+        )
+    np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
+    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs.P0))
+    assert mekf._g == 9.80665
+    assert mekf._nav_frame == "ned"
+
+
+@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+def test_ahrs_nav_frame(nav_frame, scale):
+    mekf = AHRS(
+        10.0,
+        nav_frame=nav_frame
+        )
+
+    assert mekf._nav_frame == nav_frame.lower()
+    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
+
+
+def test_ahrs_methods():
+    vel_init = np.array([0.0, 0.1, -0.2])
+    euler_init = np.array([10.0, 20.0, 30.0])
+    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
+    bg_init = np.array([0.01, -0.01, 0.02])
+
+    mekf = AHRS(
+        10.0,
+        v=vel_init,
+        q=quaternion_init,
+        bg=bg_init
+        )
+
+    np.testing.assert_allclose(mekf.velocity(), vel_init)
+    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
+    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
+    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+)
+def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = AHRS(
+        fs_imu,
+        q=q0,
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i) in enumerate(
+        zip(acc_noise, gyro_noise)
+    ):
+
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(
+            dvel,
+            dtheta,
+            degrees=degrees,
+        )
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+)
+def test_ahrs_benchmark(benchmark_gen, degrees):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    head_std = np.radians(1.0)
+    head_noise = euler_ref[:, -1] + np.random.normal(0., head_std, len(euler_ref))
+
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+        head_noise = np.degrees(head_noise)
+        head_std = np.degrees(head_std)
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = AHRS(
+        fs_imu,
+        q=q0,
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i, head_i) in enumerate(
+        zip(acc_noise, gyro_noise, head_noise)
+    ):
+
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(
+            dvel,
+            dtheta,
+            degrees=degrees,
+            head=head_i,
+            head_degrees=degrees,
+            head_var=head_std**2,
+        )
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(yaw_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005
+    assert np.degrees(bias_gyro_z_rms) <= 0.005
\ No newline at end of file

From 5e840c8fa0c7c8c681467dba6e74490921ae9bea Mon Sep 17 00:00:00 2001
From: Ali Cetin <ali.cetin@4subsea.com>
Date: Fri, 3 Jul 2026 11:13:38 +0200
Subject: [PATCH 105/217] ains

---
 src/smsfusion/_ins/_ahrs.py   |  18 +-
 src/smsfusion/_ins/_aiding.py |  27 ++
 src/smsfusion/_ins/_ains_.py  | 511 ++++++++++++++++++++++++++++++++++
 tests/test_ins_/test_ains.py  | 315 +++++++++++++++++++++
 4 files changed, 862 insertions(+), 9 deletions(-)
 create mode 100644 src/smsfusion/_ins/_ains_.py
 create mode 100644 tests/test_ins_/test_ains.py

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 8c3961a5..41b359f7 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -229,7 +229,7 @@ class AHRS:
     ----------
     fs : float
         Sampling rate in Hz.
-    v : array_like, shape (3,), optional
+    vel : array_like, shape (3,), optional
         Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
     q : Attitude or array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
@@ -263,7 +263,7 @@ class AHRS:
     def __init__(
         self,
         fs: float,
-        v: ArrayLike = (0.0, 0.0, 0.0),
+        vel: ArrayLike = (0.0, 0.0, 0.0),
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = P0,
@@ -288,7 +288,7 @@ def __init__(
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._v_n = np.asarray_chkfinite(v).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(vel).reshape(3).copy()
         self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
         self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
@@ -307,18 +307,18 @@ def __init__(
         )
         self._H = _measurement_matrix_init(self._q_nb)
 
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
     def velocity(self) -> NDArray[np.float64]:
         """
         Velocity expressed in the navigation frame.
         """
         return self._v_n.copy()
 
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
         Attitude expressed as Euler angles (roll, pitch, yaw).
diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index a4ea9090..b6d94a13 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -13,6 +13,33 @@
 )
 
 
+@njit  # type: ignore[misc]
+def _aiding_update_pos(
+    dx: NDArray[np.float64],
+    P: NDArray[np.float64],
+    H: NDArray[np.float64],
+    pos_n: NDArray[np.float64],
+    pos_meas: NDArray[np.float64],
+    pos_var: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    lever_arm: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Update with position aiding measurement.
+    """
+
+    if pos_var is None:
+        raise ValueError("'pos_var' not provided.")
+
+    if not lever_arm.any():
+        dz = pos_meas - (pos_n + R_nb @ lever_arm)
+    else:
+        dz = pos_meas - pos_n
+
+    dx, P = _kalman_update_sequential(dx, P, dz, pos_var, H)
+    return dx, P
+
+
 @njit  # type: ignore[misc]
 def _aiding_update_vel(
     dx: NDArray[np.float64],
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
new file mode 100644
index 00000000..f03010b9
--- /dev/null
+++ b/src/smsfusion/_ins/_ains_.py
@@ -0,0 +1,511 @@
+from typing import Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
+
+from ._aiding import _aiding_update_head, _aiding_update_vel, _aiding_update_pos
+from ._common import (
+    _dhda_head,
+    _project_covariance_ahead,
+    _update_quaternion_with_gibbs2,
+    _update_quaternion_with_rotvec,
+)
+
+P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
+)
+
+
+def _state_transition_matrix_init(
+    dt: float,
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
+    """
+    State transition matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    ndarray, shape (12, 12)
+        State transition matrix.
+    """
+    phi = np.eye(12)
+    phi[0:3, 3:6] += dt * np.eye(3)
+    phi[3:6, 6:9] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
+    phi[6:9, 6:9] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[6:9, 9:12] -= dt * np.eye(3)
+    phi[9:12, 9:12] -= dt * np.eye(3) / gbc
+    return phi
+
+
+@njit  # type: ignore[misc]
+def _state_transition_matrix_update(
+    phi: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+) -> NDArray[np.float64]:
+    """
+    Update the state transition matrix in place.
+
+    Parameters
+    ----------
+    phi : ndarray, shape (12, 12)
+        State transition matrix to be updated in place.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    """
+    dtx, dty, dtz = dtheta
+    dvx, dvy, dvz = dvel
+
+    r00, r01, r02 = R_nb[0]
+    r10, r11, r12 = R_nb[1]
+    r20, r21, r22 = R_nb[2]
+
+    # phi[6:9, 6:9] = np.eye(3) - dt * S(w_b)
+    phi[6, 7] = dtz
+    phi[6, 8] = -dty
+    phi[7, 6] = -dtz
+    phi[7, 8] = dtx
+    phi[8, 6] = dty
+    phi[8, 7] = -dtx
+
+    # phi[3:6, 6:9] = -dt * R_nb @ S(f_b)
+    phi[3, 6] = -dvz * r01 + dvy * r02
+    phi[4, 6] = -dvz * r11 + dvy * r12
+    phi[5, 6] = -dvz * r21 + dvy * r22
+    phi[3, 7] = dvz * r00 - dvx * r02
+    phi[4, 7] = dvz * r10 - dvx * r12
+    phi[5, 7] = dvz * r20 - dvx * r22
+    phi[3, 8] = -dvy * r00 + dvx * r01
+    phi[4, 8] = -dvy * r10 + dvx * r11
+    phi[5, 8] = -dvy * r20 + dvx * r21
+    return phi
+
+
+def _process_noise_covariance_matrix(
+    dt: float, vrw: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
+    """
+    Process noise covariance matrix.
+
+    Parameters
+    ----------
+    dt : float
+        Time step in seconds.
+    vrw : float
+        Velocity random walk (accelerometer noise density) in m/s/√Hz.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
+
+    Returns
+    -------
+    Q : ndarray, shape (12, 12)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((12, 12))
+    Q[3:6, 0:3] = dt * vrw**2 * np.eye(3)
+    Q[6:9, 3:6] = dt * arw**2 * np.eye(3)
+    Q[9:12, 6:9] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
+
+
+
+def _measurement_matrix_init(
+        q_nb: NDArray[np.float64],
+        lever_arm: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Measurement matrix.
+
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    lever_arm : ndarray, shape(3,)
+        Lever-arm vector describing the location of position aiding (in meters) relative
+        to the IMU expressed in the IMU's measurement frame. For instance, the location
+        of the GNSS antenna relative to the IMU. By default it is assumed that the
+        aiding position coincides with the IMU's origin.
+
+    Returns
+    -------
+    ndarray, shape (7, 12)
+        Linearized measurement matrix.
+    """
+    dhdx = np.zeros((7, 12))
+    dhdx[0:3, 0:3] = np.eye(3)  # position
+    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)  # position lever arm
+    dhdx[3:6, 3:6] = np.eye(3)  # velocity
+    dhdx[6:7, 6:9] = _dhda_head(q_nb)  # heading
+    return dhdx
+
+
+def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
+    """
+    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
+
+    Parameters
+    ----------
+    g : float
+        Gravitational acceleration in m/s^2.
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame in which the gravity vector is expressed.
+
+    Returns
+    -------
+    ndarray, shape (3,)
+        Gravity vector expressed in the navigation frame.
+    """
+    if nav_frame.lower() == "ned":
+        g_n = np.array([0.0, 0.0, g])
+    elif nav_frame.lower() == "enu":
+        g_n = np.array([0.0, 0.0, -g])
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+    return g_n
+
+
+@njit  # type: ignore[misc]
+def _reset(
+    dx: NDArray[np.float64],
+    p_n: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    bg_b: NDArray[np.float64],
+) -> tuple[
+    NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]
+]:
+    """
+    Reset state.
+
+    Parameters
+    ----------
+    p_n : ndarray, shape (3,)
+        Position state estimate to be reset in place.
+    v_n : ndarray, shape (3,)
+        Velocity state estimate to be reset in place.
+    q_nb : ndarray, shape (4,)
+        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,)
+        Error state vector containing the corrections to be applied to the state
+        estimates. Will be reset to zero after applying the corrections.
+    """
+    p_n[:] += dx[0:3]
+    v_n[:] += dx[3:6]
+    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[6:9])
+    bg_b[:] += dx[9:12]
+    dx[:] = 0.0
+    return dx, p_n, v_n, q_nb, bg_b
+
+
+class AINS:
+    """
+    Aided inertial navigation system (AINS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    pos : array_like, shape (3,), optional
+        Initial position estimate in m from origin. Defaults to origin (0.0, 0.0, 0.0).
+    vel : array_like, shape (3,), optional
+        Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
+    q : array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    P : array_like, shape (6, 6), optional
+        Initial (a priori) estimate of the error covariance matrix. Defaults to
+        a small diagonal matrix (1e-6 * np.eye(9)).
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
+        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
+        noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    g : float, optional
+        The gravitational acceleration in m/s^2. Default is 'standard gravity' of
+        9.80665 m/s^2.
+    nav_frame : {'NED', 'ENU'}, optional
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+    lever_arm : array-like, shape (3,), default (0.0, 0.0, 0.0)
+        Lever-arm vector describing the location of position aiding (in meters) relative
+        to the IMU expressed in the IMU's measurement frame. For instance, the location
+        of the GNSS antenna relative to the IMU. By default it is assumed that the
+        aiding position coincides with the IMU's origin.
+
+    """
+
+    def __init__(
+        self,
+        fs: float,
+        pos: ArrayLike = (0.0, 0.0, 0.0),
+        vel: ArrayLike = (0.0, 0.0, 0.0),
+        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg: ArrayLike = (0.0, 0.0, 0.0),
+        P: ArrayLike = P0,
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.00005,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+        lever_arm: ArrayLike = (0.0, 0.0, 0.0)
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._g = g
+        self._nav_frame = nav_frame.lower()
+        self._g_n = _gravity_nav(self._g, self._nav_frame)
+        self._dvel_g_corr = self._dt * self._g_n
+        self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy()
+
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._p_n = np.asarray_chkfinite(pos).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(vel).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P).reshape(12, 12).copy()
+        self._dx = np.zeros(12)
+
+        # Discrete state-space model
+        self._phi = _state_transition_matrix_init(
+            self._dt,
+            np.zeros(3),
+            np.zeros(3),
+            _rot_matrix_from_quaternion(self._q_nb),
+            self._gbc,
+        )
+        self._Q = _process_noise_covariance_matrix(
+            self._dt, self._vrw, self._arw, self._gbs, self._gbc
+        )
+        self._H = _measurement_matrix_init(self._q_nb, self._lever_arm)
+
+    def position(self) -> NDArray[np.float64]:
+        """
+        Position expressed in the navigation frame.
+        """
+        return self._p_n.copy()
+
+    def velocity(self) -> NDArray[np.float64]:
+        """
+        Velocity expressed in the navigation frame.
+        """
+        return self._v_n.copy()
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Attitude expressed as Euler angles (roll, pitch, yaw).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
+
+        theta = _euler_from_quaternion(self._q_nb)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta
+
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
+        """
+        Gyroscope bias estimate (rad/s) expressed in the body frame.
+
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
+        """
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
+
+    def update(
+        self,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
+        degrees: bool = False,
+        pos: ArrayLike | None = None,
+        pos_var: ArrayLike = (1.0e6, 1.0e6, 1.0e6),
+        vel: ArrayLike | None = None,
+        vel_var: ArrayLike = (100.0, 100.0, 100.0),
+        head: float | None = None,
+        head_var: float = 0.001,
+        head_degrees: bool = False,
+    ) -> Self:
+        """
+        Update state estimates with IMU and aiding measurements.
+
+        Parameters
+        ----------
+        dvel : array_like, shape (3,), optional
+            Velocity increment (sculling integral) in m/s.
+        dtheta : array_like, shape (3,), optional
+            Attitude increment (coning integral) in radians.
+        degrees : bool, optional
+            Specifies whether the unit of the attitude increment, ``dtheta``, is
+            degrees or radians. Defaults to radians.
+        pos : array-like, shape (3,), optional
+            Position aiding measurement in m. If ``None``, position aiding ins not used.
+        pos_var : array-like, shape (3,), optional
+            Variance of position measurement noise in m^2. Ignored if ``pos`` is ``None``. 
+        vel : array-like, shape (3,), optional
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+        vel_var : array-like, shape (3,), optional
+            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel`` is ``None``.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+
+        Returns
+        -------
+        AINS
+            A reference to the instance itself after the update.
+        """
+
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
+
+        if degrees:
+            dtheta = (np.pi / 180.0) * dtheta
+
+        dtheta = dtheta - self._dt * self._bg_b
+
+        # Update state-space model
+        R_nb = _rot_matrix_from_quaternion(self._q_nb)
+        self._phi = _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
+
+        # Project (a priori) state estimates ahead
+        self._p_n[:] += self._dt * self._v_n
+        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr
+        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+
+        # Project (a priori) error covariance matrix estimate ahead
+        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
+
+        # Update (a posteriori) state and covariance estimates with aiding measurements
+        if pos is not None:
+            self._dx, self._P = _aiding_update_pos(
+                self._dx,
+                self._P,
+                self._H[0:3],
+                self._p_n,
+                np.asarray(vel),
+                np.asarray(vel_var),
+                R_nb,
+                self._lever_arm
+            )
+
+        if vel is not None:
+            self._dx, self._P = _aiding_update_vel(
+                self._dx,
+                self._P,
+                self._H[3:6],
+                self._v_n,
+                np.asarray(vel),
+                np.asarray(vel_var),
+            )
+
+        if head is not None:
+            self._H[3, 3:6] = _dhda_head(self._q_nb)  # Update measurement matrix
+
+            self._dx, self._P = _aiding_update_head(
+                self._dx,
+                self._P,
+                self._H[6],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
+
+        # Reset state
+        self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b = _reset(
+            self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b
+        )
+
+        return self
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
new file mode 100644
index 00000000..c55ed739
--- /dev/null
+++ b/tests/test_ins_/test_ains.py
@@ -0,0 +1,315 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion._ins._ains_ import AINS, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
+from smsfusion.benchmark import (
+    benchmark_pure_attitude_beat_202311A,
+    benchmark_pure_attitude_chirp_202311A
+)
+
+
+def test_state_transition_matrix_init():
+    dt = 0.1
+    dvel = np.ones(3) * 0.01
+    dtheta = np.ones(3) * 0.02
+    R_nb = np.eye(3)
+    gbc = 0.01
+
+    phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+    phi_expected = np.array([
+        [1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_state_transition_matrix_update():
+    dt = 0.1
+    dvel = np.ones(3) * 0.01
+    dtheta = np.ones(3) * 0.02
+    R_nb = np.eye(3)
+    gbc = 0.01
+
+    phi_init = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+
+    dtheta_update = np.ones(3) * 0.01
+    dvel_update = np.ones(3) * 0.1
+    phi_out = _state_transition_matrix_update(
+        phi_init,
+        dvel=dvel_update,
+        dtheta=dtheta_update,
+        R_nb=R_nb
+        )
+
+    phi_expected = np.array([
+        [1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_measurement_matrix_init():
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+
+    expect = np.zeros((4, 9))
+    expect[0:3, 0:3] = np.eye(3)
+    expect[3, 3:6] = np.array([0.0, 0.0, 1.0])
+    # kappa -> zero due to unit quat
+
+    np.testing.assert_array_equal(_measurement_matrix_init(q_nb), expect)
+
+
+def test_process_noise_covariance_matrix():
+    dt = 0.1
+    vrw = 0.0005
+    arw = 0.00005
+    gbs = 0.00005
+    gbc = 50.0
+    Q_out = _process_noise_covariance_matrix(dt, vrw, arw, gbs, gbc)
+    Q_expect = np.array([
+        [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+    ])
+
+    np.testing.assert_allclose(Q_out, Q_expect)
+
+def test_reset():
+    v_n = np.array([0.0, 0.1, 0.0])
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    bg_b = np.zeros(3)
+    dx = np.array([0.1, 0.0, 0.0, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+
+    dx, v_n, q_nb, bg_b = _reset(dx, v_n, q_nb, bg_b)
+
+    np.testing.assert_allclose(dx, np.zeros_like(dx))
+    np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
+    np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
+    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+
+
+def test_ahrs_init():
+    mekf = AHRS(
+        10.0
+        )
+    np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
+    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs.P0))
+    assert mekf._g == 9.80665
+    assert mekf._nav_frame == "ned"
+
+
+@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+def test_ahrs_nav_frame(nav_frame, scale):
+    mekf = AINS(
+        10.0,
+        nav_frame=nav_frame
+        )
+
+    assert mekf._nav_frame == nav_frame.lower()
+    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
+
+
+def test_ahrs_methods():
+    pos_init = np.array([0.1, 10.0, -0.2])
+    vel_init = np.array([0.0, 0.1, -0.2])
+    euler_init = np.array([10.0, 20.0, 30.0])
+    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
+    bg_init = np.array([0.01, -0.01, 0.02])
+
+    mekf = AINS(
+        10.0,
+        pos=pos_init,
+        vel=vel_init,
+        q=quaternion_init,
+        bg=bg_init
+        )
+
+    np.testing.assert_allclose(mekf.position(), vel_init)
+    np.testing.assert_allclose(mekf.velocity(), vel_init)
+    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
+    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
+    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+)
+def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = AHRS(
+        fs_imu,
+        q=q0,
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i) in enumerate(
+        zip(acc_noise, gyro_noise)
+    ):
+
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(
+            dvel,
+            dtheta,
+            degrees=degrees,
+        )
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+)
+def test_ains_attitude_benchmark(benchmark_gen, degrees):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    head_std = np.radians(1.0)
+    head_noise = euler_ref[:, -1] + np.random.normal(0., head_std, len(euler_ref))
+
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+        head_noise = np.degrees(head_noise)
+        head_std = np.degrees(head_std)
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = AINS(
+        fs_imu,
+        q=q0,
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    vel_aid = (0.0, 0.0, 0.0)
+    vel_var = (100.0, 100.0, 100.0)  # (10.0 m/s)^2
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i, head_i) in enumerate(
+        zip(acc_noise, gyro_noise, head_noise)
+    ):
+
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(
+            dvel,
+            dtheta,
+            degrees=degrees,
+            vel=vel_aid,
+            vel_var=vel_var,
+            head=head_i,
+            head_degrees=degrees,
+            head_var=head_std**2,
+        )
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(yaw_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005
+    assert np.degrees(bias_gyro_z_rms) <= 0.005
\ No newline at end of file

From c89a9437273dfb8e9e200fed788eed37f268b0e0 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:11:47 +0200
Subject: [PATCH 106/217] fix ahrs test

---
 tests/test_ins_/test_ahrs.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index bcd2dfd1..f6d64b15 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -143,7 +143,7 @@ def test_ahrs_methods():
 
     mekf = AHRS(
         10.0,
-        v=vel_init,
+        vel=vel_init,
         q=quaternion_init,
         bg=bg_init
         )

From fb800efd6d88426a8079e97d8b75c6f91c74571c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:22:02 +0200
Subject: [PATCH 107/217] add ains v2 to init

---
 src/smsfusion/_ins/__init__.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index bc526a66..8a4b4fc6 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -2,3 +2,4 @@
 from ._ains import AHRS, VRU, AidedINS, StrapdownINS
 from ._utils import FixedNED, gravity, euler_from_acc
 from ._vru import VRU as VRUv2
+from ._ains_ import AINS as AINSv2

From f15aa1342acbfddf8c26af528c68a205bb358fed Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:42:03 +0200
Subject: [PATCH 108/217] fix test aind state transition init

---
 tests/test_ins_/test_ains.py | 47 +++++++++++++++++++-----------------
 1 file changed, 25 insertions(+), 22 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index c55ed739..cf6f408a 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -19,15 +19,18 @@ def test_state_transition_matrix_init():
 
     phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
     phi_expected = np.array([
-        [1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        [1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
     ])
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
@@ -51,19 +54,19 @@ def test_state_transition_matrix_update():
         R_nb=R_nb
         )
 
-    phi_expected = np.array([
-        [1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
-
-    np.testing.assert_almost_equal(phi_out, phi_expected)
+    # phi_expected = np.array([
+    #     [1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #     [0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #     [0.0, 0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    #     [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+    #     [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+    #     [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+    # ])
+
+    np.testing.assert_almost_equal(phi_out[:9], phi_expected)
 
 
 def test_measurement_matrix_init():

From 1e43d99d1a256676a0cfd8172553126c0612cd48 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:42:58 +0200
Subject: [PATCH 109/217] small fix

---
 tests/test_ins_/test_ains.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index cf6f408a..16c66798 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -19,9 +19,9 @@ def test_state_transition_matrix_init():
 
     phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
     phi_expected = np.array([
-        [1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
         [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
         [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
         [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],

From 8daa857052bd1593986821c717255cfefe362763 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:45:36 +0200
Subject: [PATCH 110/217] fix test state transition update

---
 tests/test_ins_/test_ains.py | 29 ++++++++++++++++-------------
 1 file changed, 16 insertions(+), 13 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 16c66798..81bb9f04 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -54,19 +54,22 @@ def test_state_transition_matrix_update():
         R_nb=R_nb
         )
 
-    # phi_expected = np.array([
-    #     [1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    #     [0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    #     [0.0, 0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-    #     [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
-    #     [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
-    #     [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
-    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-    #     [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-    # ])
-
-    np.testing.assert_almost_equal(phi_out[:9], phi_expected)
+    phi_expected = np.array([
+        [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+    ])
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
 
 
 def test_measurement_matrix_init():

From 70dc931f859d5665dd86354dd0d3109e32514dd5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:55:50 +0200
Subject: [PATCH 111/217] fix test ains meas matrix

---
 tests/test_ins_/test_ains.py | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 81bb9f04..d734e5c3 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -2,6 +2,8 @@
 import pytest
 from scipy.signal import resample_poly
 
+from smsfusion._vectorops import _skew_symmetric
+from smsfusion._transforms import _rot_matrix_from_quaternion
 import smsfusion as sf
 from smsfusion._ins._ains_ import AINS, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
 from smsfusion.benchmark import (
@@ -74,13 +76,15 @@ def test_state_transition_matrix_update():
 
 def test_measurement_matrix_init():
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    lever_arm = np.array([2.0, 3.0, 4.0])
 
-    expect = np.zeros((4, 9))
+    expect = np.zeros((7, 12))
     expect[0:3, 0:3] = np.eye(3)
-    expect[3, 3:6] = np.array([0.0, 0.0, 1.0])
-    # kappa -> zero due to unit quat
+    expect[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
+    expect[3:6, 3:6] = np.eye(3)
+    expect[6, 6:9] = np.array([0.0, 0.0, 1.0]) # kappa -> zero due to unit quat
 
-    np.testing.assert_array_equal(_measurement_matrix_init(q_nb), expect)
+    np.testing.assert_array_equal(_measurement_matrix_init(q_nb, lever_arm), expect)
 
 
 def test_process_noise_covariance_matrix():

From 982f90b2c2941491dafbc1228fe137ecf75cfbb1 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 08:56:41 +0200
Subject: [PATCH 112/217] black + isort

---
 src/smsfusion/_ins/__init__.py |   4 +-
 src/smsfusion/_ins/_ains_.py   |  23 +++--
 tests/_test_ins.py             |   7 +-
 tests/conftest.py              |   3 +-
 tests/test_ins_/test_ahrs.py   | 133 +++++++++++++++--------------
 tests/test_ins_/test_ains.py   | 152 +++++++++++++++++----------------
 tests/test_ins_/test_common.py |  20 +++--
 tests/test_ins_/test_utils.py  |  12 ++-
 tests/test_ins_/test_vru.py    | 101 +++++++++++-----------
 tests/test_noise.py            |   1 +
 10 files changed, 238 insertions(+), 218 deletions(-)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index 8a4b4fc6..bf40489f 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,5 +1,5 @@
 from ._ahrs import AHRS as AHRSv2
 from ._ains import AHRS, VRU, AidedINS, StrapdownINS
-from ._utils import FixedNED, gravity, euler_from_acc
-from ._vru import VRU as VRUv2
 from ._ains_ import AINS as AINSv2
+from ._utils import FixedNED, euler_from_acc, gravity
+from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index f03010b9..f5ee4475 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -7,7 +7,7 @@
 from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 
-from ._aiding import _aiding_update_head, _aiding_update_vel, _aiding_update_pos
+from ._aiding import _aiding_update_head, _aiding_update_pos, _aiding_update_vel
 from ._common import (
     _dhda_head,
     _project_covariance_ahead,
@@ -148,10 +148,9 @@ def _process_noise_covariance_matrix(
     return Q
 
 
-
 def _measurement_matrix_init(
-        q_nb: NDArray[np.float64],
-        lever_arm: NDArray[np.float64]) -> NDArray[np.float64]:
+    q_nb: NDArray[np.float64], lever_arm: NDArray[np.float64]
+) -> NDArray[np.float64]:
     """
     Measurement matrix.
 
@@ -172,7 +171,9 @@ def _measurement_matrix_init(
     """
     dhdx = np.zeros((7, 12))
     dhdx[0:3, 0:3] = np.eye(3)  # position
-    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)  # position lever arm
+    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(
+        lever_arm
+    )  # position lever arm
     dhdx[3:6, 3:6] = np.eye(3)  # velocity
     dhdx[6:7, 6:9] = _dhda_head(q_nb)  # heading
     return dhdx
@@ -211,7 +212,11 @@ def _reset(
     q_nb: NDArray[np.float64],
     bg_b: NDArray[np.float64],
 ) -> tuple[
-    NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
+    NDArray[np.float64],
 ]:
     """
     Reset state.
@@ -299,7 +304,7 @@ def __init__(
         gyro_bias_corr_time: float = 50.0,
         g: float = 9.80665,
         nav_frame: str = "NED",
-        lever_arm: ArrayLike = (0.0, 0.0, 0.0)
+        lever_arm: ArrayLike = (0.0, 0.0, 0.0),
     ) -> None:
         self._fs = fs
         self._dt = 1.0 / fs
@@ -425,7 +430,7 @@ def update(
         pos : array-like, shape (3,), optional
             Position aiding measurement in m. If ``None``, position aiding ins not used.
         pos_var : array-like, shape (3,), optional
-            Variance of position measurement noise in m^2. Ignored if ``pos`` is ``None``. 
+            Variance of position measurement noise in m^2. Ignored if ``pos`` is ``None``.
         vel : array-like, shape (3,), optional
             Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
         vel_var : array-like, shape (3,), optional
@@ -477,7 +482,7 @@ def update(
                 np.asarray(vel),
                 np.asarray(vel_var),
                 R_nb,
-                self._lever_arm
+                self._lever_arm,
             )
 
         if vel is not None:
diff --git a/tests/_test_ins.py b/tests/_test_ins.py
index a6e98910..e34409af 100644
--- a/tests/_test_ins.py
+++ b/tests/_test_ins.py
@@ -18,17 +18,12 @@
 from scipy.spatial.transform import Rotation
 
 import smsfusion as sf
-from smsfusion._ins import (
+from smsfusion._ins import (  # INSMixin,; _dhda_head,; _h_head,; _roll_pitch_from_acc,; _signed_smallest_angle,
     AHRS,
     VRU,
     AidedINS,
     FixedNED,
-    # INSMixin,
     StrapdownINS,
-    # _dhda_head,
-    # _h_head,
-    # _roll_pitch_from_acc,
-    # _signed_smallest_angle,
     gravity,
 )
 from smsfusion._transforms import (
diff --git a/tests/conftest.py b/tests/conftest.py
index dd641f30..685d12a3 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -1,2 +1,3 @@
 import os
-os.environ["NUMBA_DISABLE_JIT"] = "1"
\ No newline at end of file
+
+os.environ["NUMBA_DISABLE_JIT"] = "1"
diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index f6d64b15..20646284 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -3,10 +3,17 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._ahrs import AHRS, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
+from smsfusion._ins._ahrs import (
+    AHRS,
+    _measurement_matrix_init,
+    _process_noise_covariance_matrix,
+    _reset,
+    _state_transition_matrix_init,
+    _state_transition_matrix_update,
+)
 from smsfusion.benchmark import (
     benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A
+    benchmark_pure_attitude_chirp_202311A,
 )
 
 
@@ -18,17 +25,19 @@ def test_state_transition_matrix_init():
     gbc = 0.01
 
     phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
-    phi_expected = np.array([
-        [1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+            [0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+            [0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -45,23 +54,22 @@ def test_state_transition_matrix_update():
     dtheta_update = np.ones(3) * 0.01
     dvel_update = np.ones(3) * 0.1
     phi_out = _state_transition_matrix_update(
-        phi_init,
-        dvel=dvel_update,
-        dtheta=dtheta_update,
-        R_nb=R_nb
-        )
+        phi_init, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
+    )
 
-    phi_expected = np.array([
-        [1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+            [0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+            [0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -84,20 +92,23 @@ def test_process_noise_covariance_matrix():
     gbs = 0.00005
     gbc = 50.0
     Q_out = _process_noise_covariance_matrix(dt, vrw, arw, gbs, gbc)
-    Q_expect = np.array([
-        [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
-    ])
+    Q_expect = np.array(
+        [
+            [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+        ]
+    )
 
     np.testing.assert_allclose(Q_out, Q_expect)
 
+
 def test_reset():
     v_n = np.array([0.0, 0.1, 0.0])
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
@@ -109,13 +120,13 @@ def test_reset():
     np.testing.assert_allclose(dx, np.zeros_like(dx))
     np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
-    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+    np.testing.assert_allclose(
+        q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
+    )
 
 
 def test_ahrs_init():
-    mekf = AHRS(
-        10.0
-        )
+    mekf = AHRS(10.0)
     np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
     np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
     np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
@@ -126,10 +137,7 @@ def test_ahrs_init():
 
 @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
 def test_ahrs_nav_frame(nav_frame, scale):
-    mekf = AHRS(
-        10.0,
-        nav_frame=nav_frame
-        )
+    mekf = AHRS(10.0, nav_frame=nav_frame)
 
     assert mekf._nav_frame == nav_frame.lower()
     np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
@@ -141,12 +149,7 @@ def test_ahrs_methods():
     quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
     bg_init = np.array([0.01, -0.01, 0.02])
 
-    mekf = AHRS(
-        10.0,
-        vel=vel_init,
-        q=quaternion_init,
-        bg=bg_init
-        )
+    mekf = AHRS(10.0, vel=vel_init, q=quaternion_init, bg=bg_init)
 
     np.testing.assert_allclose(mekf.velocity(), vel_init)
     np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
@@ -158,7 +161,10 @@ def test_ahrs_methods():
 
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
-    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
 )
 def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
@@ -193,9 +199,7 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
 
     # Apply filter
     euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(
-        zip(acc_noise, gyro_noise)
-    ):
+    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
 
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
@@ -229,7 +233,10 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
 
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
-    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
 )
 def test_ahrs_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
@@ -250,7 +257,7 @@ def test_ahrs_benchmark(benchmark_gen, degrees):
     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
 
     head_std = np.radians(1.0)
-    head_noise = euler_ref[:, -1] + np.random.normal(0., head_std, len(euler_ref))
+    head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
 
     if degrees:
         gyro_noise = np.degrees(gyro_noise)
@@ -269,9 +276,7 @@ def test_ahrs_benchmark(benchmark_gen, degrees):
 
     # Apply filter
     euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i, head_i) in enumerate(
-        zip(acc_noise, gyro_noise, head_noise)
-    ):
+    for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
 
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
@@ -305,4 +310,4 @@ def test_ahrs_benchmark(benchmark_gen, degrees):
     assert np.degrees(yaw_rms) <= 0.1
     assert np.degrees(bias_gyro_x_rms) <= 0.005
     assert np.degrees(bias_gyro_y_rms) <= 0.005
-    assert np.degrees(bias_gyro_z_rms) <= 0.005
\ No newline at end of file
+    assert np.degrees(bias_gyro_z_rms) <= 0.005
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index d734e5c3..a98a9c34 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -2,13 +2,20 @@
 import pytest
 from scipy.signal import resample_poly
 
-from smsfusion._vectorops import _skew_symmetric
-from smsfusion._transforms import _rot_matrix_from_quaternion
 import smsfusion as sf
-from smsfusion._ins._ains_ import AINS, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
+from smsfusion._ins._ains_ import (
+    AINS,
+    _measurement_matrix_init,
+    _process_noise_covariance_matrix,
+    _reset,
+    _state_transition_matrix_init,
+    _state_transition_matrix_update,
+)
+from smsfusion._transforms import _rot_matrix_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
     benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A
+    benchmark_pure_attitude_chirp_202311A,
 )
 
 
@@ -20,20 +27,22 @@ def test_state_transition_matrix_init():
     gbc = 0.01
 
     phi_out = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
-    phi_expected = np.array([
-        [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.01, -0.01, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.01, 0.0, 0.01, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -50,26 +59,25 @@ def test_state_transition_matrix_update():
     dtheta_update = np.ones(3) * 0.01
     dvel_update = np.ones(3) * 0.1
     phi_out = _state_transition_matrix_update(
-        phi_init,
-        dvel=dvel_update,
-        dtheta=dtheta_update,
-        R_nb=R_nb
-        )
+        phi_init, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
+    )
 
-    phi_expected = np.array([
-        [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -0.1, 0.0, 0.1, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -82,7 +90,7 @@ def test_measurement_matrix_init():
     expect[0:3, 0:3] = np.eye(3)
     expect[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     expect[3:6, 3:6] = np.eye(3)
-    expect[6, 6:9] = np.array([0.0, 0.0, 1.0]) # kappa -> zero due to unit quat
+    expect[6, 6:9] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
 
     np.testing.assert_array_equal(_measurement_matrix_init(q_nb, lever_arm), expect)
 
@@ -94,20 +102,23 @@ def test_process_noise_covariance_matrix():
     gbs = 0.00005
     gbc = 50.0
     Q_out = _process_noise_covariance_matrix(dt, vrw, arw, gbs, gbc)
-    Q_expect = np.array([
-        [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
-    ])
+    Q_expect = np.array(
+        [
+            [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+        ]
+    )
 
     np.testing.assert_allclose(Q_out, Q_expect)
 
+
 def test_reset():
     v_n = np.array([0.0, 0.1, 0.0])
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
@@ -119,13 +130,13 @@ def test_reset():
     np.testing.assert_allclose(dx, np.zeros_like(dx))
     np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
-    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+    np.testing.assert_allclose(
+        q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
+    )
 
 
 def test_ahrs_init():
-    mekf = AHRS(
-        10.0
-        )
+    mekf = AHRS(10.0)
     np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
     np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
     np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
@@ -136,10 +147,7 @@ def test_ahrs_init():
 
 @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
 def test_ahrs_nav_frame(nav_frame, scale):
-    mekf = AINS(
-        10.0,
-        nav_frame=nav_frame
-        )
+    mekf = AINS(10.0, nav_frame=nav_frame)
 
     assert mekf._nav_frame == nav_frame.lower()
     np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
@@ -152,13 +160,7 @@ def test_ahrs_methods():
     quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
     bg_init = np.array([0.01, -0.01, 0.02])
 
-    mekf = AINS(
-        10.0,
-        pos=pos_init,
-        vel=vel_init,
-        q=quaternion_init,
-        bg=bg_init
-        )
+    mekf = AINS(10.0, pos=pos_init, vel=vel_init, q=quaternion_init, bg=bg_init)
 
     np.testing.assert_allclose(mekf.position(), vel_init)
     np.testing.assert_allclose(mekf.velocity(), vel_init)
@@ -171,7 +173,10 @@ def test_ahrs_methods():
 
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
-    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
 )
 def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
@@ -206,9 +211,7 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
 
     # Apply filter
     euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(
-        zip(acc_noise, gyro_noise)
-    ):
+    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
 
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
@@ -242,7 +245,10 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
 
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
-    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
 )
 def test_ains_attitude_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
@@ -263,7 +269,7 @@ def test_ains_attitude_benchmark(benchmark_gen, degrees):
     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
 
     head_std = np.radians(1.0)
-    head_noise = euler_ref[:, -1] + np.random.normal(0., head_std, len(euler_ref))
+    head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
 
     if degrees:
         gyro_noise = np.degrees(gyro_noise)
@@ -284,9 +290,7 @@ def test_ains_attitude_benchmark(benchmark_gen, degrees):
     vel_var = (100.0, 100.0, 100.0)  # (10.0 m/s)^2
     # Apply filter
     euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i, head_i) in enumerate(
-        zip(acc_noise, gyro_noise, head_noise)
-    ):
+    for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
 
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
@@ -322,4 +326,4 @@ def test_ains_attitude_benchmark(benchmark_gen, degrees):
     assert np.degrees(yaw_rms) <= 0.1
     assert np.degrees(bias_gyro_x_rms) <= 0.005
     assert np.degrees(bias_gyro_y_rms) <= 0.005
-    assert np.degrees(bias_gyro_z_rms) <= 0.005
\ No newline at end of file
+    assert np.degrees(bias_gyro_z_rms) <= 0.005
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py
index 4a916883..1712dfa1 100644
--- a/tests/test_ins_/test_common.py
+++ b/tests/test_ins_/test_common.py
@@ -201,21 +201,23 @@ def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expect
     [
         (np.array([1.0, 0.0, 0.0, 0.0]), 1.0),
         (np.array([1.0, 0.0, 0.0, 0.0]), -1.0),
-        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), -1.0),
-        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), 1.0),
-        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), -1.0),
-        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), 1.0),
-        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), -1.0),
-        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), 1.0)
-    ]
+        (np.array([np.cos(0.1 / 2), np.sin(0.1 / 2), 0.0, 0.0]), -1.0),
+        (np.array([np.cos(0.1 / 2), np.sin(0.1 / 2), 0.0, 0.0]), 1.0),
+        (np.array([np.cos(0.1 / 2), 0.0, np.sin(0.1 / 2), 0.0]), -1.0),
+        (np.array([np.cos(0.1 / 2), 0.0, np.sin(0.1 / 2), 0.0]), 1.0),
+        (np.array([np.cos(0.1 / 2), 0.0, 0.0, np.sin(0.1 / 2)]), -1.0),
+        (np.array([np.cos(0.1 / 2), 0.0, 0.0, np.sin(0.1 / 2)]), 1.0),
+    ],
 )
 def test__nz_b_from_quat(q_nb, nav_frame_factor):
     out = _common._nz_b_from_quat(q_nb, nav_frame_factor=nav_frame_factor)
 
-    expect = Rotation.from_quat(q_nb, scalar_first=True).apply(nav_frame_factor * np.array([0.0, 0.0, 1.0]), inverse=True)
+    expect = Rotation.from_quat(q_nb, scalar_first=True).apply(
+        nav_frame_factor * np.array([0.0, 0.0, 1.0]), inverse=True
+    )
     np.testing.assert_allclose(out, expect)
 
 
 def test__nz2vg():
     assert _common._nz2vg("NED") == 1.0
-    assert _common._nz2vg("ENU") == -1.0
\ No newline at end of file
+    assert _common._nz2vg("ENU") == -1.0
diff --git a/tests/test_ins_/test_utils.py b/tests/test_ins_/test_utils.py
index 957b2799..4cdd1621 100644
--- a/tests/test_ins_/test_utils.py
+++ b/tests/test_ins_/test_utils.py
@@ -26,21 +26,25 @@ def test_euler_from_acc(euler):
     euler_ned = _utils.euler_from_acc(acc_ned, nav_frame="NED", yaw=euler[2])
     np.testing.assert_allclose(euler_ned, euler)
 
-    euler_ned = _utils.euler_from_acc(acc_ned, nav_frame="NED", yaw=euler[2], degrees=True)
+    euler_ned = _utils.euler_from_acc(
+        acc_ned, nav_frame="NED", yaw=euler[2], degrees=True
+    )
     np.testing.assert_allclose(euler_ned, euler_degrees)
 
-
     # North-East-Up (ENU) frame
     g_enu = np.array([0.0, 0.0, g])
     acc_enu = R_nm.T @ g_enu
     euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2])
     np.testing.assert_allclose(euler_enu, euler)
 
-    euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2], degrees=True)
+    euler_enu = _utils.euler_from_acc(
+        acc_enu, nav_frame="ENU", yaw=euler[2], degrees=True
+    )
     np.testing.assert_allclose(euler_enu, euler_degrees)
 
+
 def test_euler_from_acc_invalid_frame():
- 
+
     acc_ned = np.array([0.0, 0.0, 1])
     with pytest.raises(ValueError):
         _utils.euler_from_acc(acc_ned, nav_frame="STAR")
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
index d8197b0b..0e185960 100644
--- a/tests/test_ins_/test_vru.py
+++ b/tests/test_ins_/test_vru.py
@@ -3,10 +3,17 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._vru import VRU, _state_transition_matrix_init, _state_transition_matrix_update, _measurement_matrix_init, _reset, _process_noise_covariance_matrix
+from smsfusion._ins._vru import (
+    VRU,
+    _measurement_matrix_init,
+    _process_noise_covariance_matrix,
+    _reset,
+    _state_transition_matrix_init,
+    _state_transition_matrix_update,
+)
 from smsfusion.benchmark import (
     benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A
+    benchmark_pure_attitude_chirp_202311A,
 )
 
 
@@ -16,14 +23,16 @@ def test_state_transition_matrix_init():
     gbc = 0.01
 
     phi_out = _state_transition_matrix_init(dt, dtheta, gbc)
-    phi_expected = np.array([
-        [1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-        [-0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-        [0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+            [-0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+            [0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -38,14 +47,16 @@ def test_state_transition_matrix_update():
     dtheta_update = np.ones(3) * 0.01
     phi_out = _state_transition_matrix_update(phi_init, dtheta=dtheta_update)
 
-    phi_expected = np.array([
-        [1.0, 0.01, -0.01, -dt, 0.0, 0.0],
-        [-0.01, 1.0, 0.01, 0.0, -dt, 0.0],
-        [0.01, -0.01, 1.0, 0.0, 0.0, -dt],
-        [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-    ])
+    phi_expected = np.array(
+        [
+            [1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+            [-0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+            [0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
 
     np.testing.assert_almost_equal(phi_out, phi_expected)
 
@@ -60,17 +71,20 @@ def test_process_noise_covariance_matrix():
     gbs = 0.00005
     gbc = 50.0
     Q_out = _process_noise_covariance_matrix(dt, arw, gbs, gbc)
-    Q_expect = np.array([
-        [dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
-        [0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
-        [0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
-        [0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
-        [0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
-        [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
-    ])
+    Q_expect = np.array(
+        [
+            [dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+        ]
+    )
 
     np.testing.assert_allclose(Q_out, Q_expect)
 
+
 def test_reset():
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
     bg_b = np.zeros(3)
@@ -80,13 +94,13 @@ def test_reset():
 
     np.testing.assert_allclose(dx, np.zeros_like(dx))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
-    np.testing.assert_allclose(q_nb, np.array([np.cos(0.01/2), np.sin(0.01/2), 0.0, 0.0]), atol=1e-6)
+    np.testing.assert_allclose(
+        q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
+    )
 
 
 def test_vru_init():
-    mekf = VRU(
-        10.0
-        )
+    mekf = VRU(10.0)
 
     np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
     np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
@@ -95,10 +109,7 @@ def test_vru_init():
 
 @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
 def test_vru_nav_frame(nav_frame, scale):
-    mekf = VRU(
-        10.0,
-        nav_frame=nav_frame
-        )
+    mekf = VRU(10.0, nav_frame=nav_frame)
 
     assert mekf._nz2vg == scale
 
@@ -108,11 +119,7 @@ def test_vru_methods():
     quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
     bg_init = np.array([0.01, -0.01, 0.02])
 
-    mekf = VRU(
-        10.0,
-        q=quaternion_init,
-        bg=bg_init
-        )
+    mekf = VRU(10.0, q=quaternion_init, bg=bg_init)
 
     np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
     np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
@@ -123,7 +130,10 @@ def test_vru_methods():
 
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
-    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
 )
 def test_vru_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
@@ -158,19 +168,12 @@ def test_vru_benchmark(benchmark_gen, degrees):
 
     # Apply filter
     euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(
-        zip(acc_noise, gyro_noise)
-    ):
+    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
 
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
 
-        mekf.update(
-            dvel,
-            dtheta,
-            degrees=degrees,
-            gref=True
-        )
+        mekf.update(dvel, dtheta, degrees=degrees, gref=True)
 
         euler_out.append(mekf.euler(degrees=False))
         bias_gyro_out.append(mekf.bias_gyro(degrees=False))
diff --git a/tests/test_noise.py b/tests/test_noise.py
index 625dd0b6..5deb7d24 100644
--- a/tests/test_noise.py
+++ b/tests/test_noise.py
@@ -387,6 +387,7 @@ class Test_allan_var_overlapping:
     @staticmethod
     def _tqdm_installed():
         import importlib.util
+
         return importlib.util.find_spec("tqdm") is not None
 
     @pytest.mark.skipif(_tqdm_installed(), reason="tqdm is installed")

From 5abc5ba094d4bbf123f810f386222001567e3986 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:09:50 +0200
Subject: [PATCH 113/217] fix ains process noise cov matrix

---
 src/smsfusion/_ins/_ains_.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index f5ee4475..37055468 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -142,9 +142,9 @@ def _process_noise_covariance_matrix(
         Process noise covariance matrix.
     """
     Q = np.zeros((12, 12))
-    Q[3:6, 0:3] = dt * vrw**2 * np.eye(3)
-    Q[6:9, 3:6] = dt * arw**2 * np.eye(3)
-    Q[9:12, 6:9] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    Q[3:6, 3:6] = dt * vrw**2 * np.eye(3)
+    Q[6:9, 6:9] = dt * arw**2 * np.eye(3)
+    Q[9:12, 9:12] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
     return Q
 
 

From 776b2fd86365fe95a4d67926ba67a11d47d1b1d5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:14:25 +0200
Subject: [PATCH 114/217] fix test process noise cov matrix

---
 tests/test_ins_/test_ains.py | 60 ++++++++++++++++++++++++++++++------
 1 file changed, 51 insertions(+), 9 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index a98a9c34..d56a2507 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -104,15 +104,57 @@ def test_process_noise_covariance_matrix():
     Q_out = _process_noise_covariance_matrix(dt, vrw, arw, gbs, gbc)
     Q_expect = np.array(
         [
-            [dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-            [0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-            [0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
-            [0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, dt * vrw**2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                dt * (2.0 * gbs**2 / gbc),
+                0.0,
+                0.0,
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                dt * (2.0 * gbs**2 / gbc),
+                0.0,
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                dt * (2.0 * gbs**2 / gbc),
+            ],
         ]
     )
 

From 1455727b3cc1336d55466e9936959cddc0145a5f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:20:45 +0200
Subject: [PATCH 115/217] fix test ains reset

---
 tests/test_ins_/test_ains.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index d56a2507..652c5b39 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -162,15 +162,17 @@ def test_process_noise_covariance_matrix():
 
 
 def test_reset():
-    v_n = np.array([0.0, 0.1, 0.0])
+    p_n = np.array([1.0, 0.0, 0.0])
+    v_n = np.array([0.0, 2.0, 0.0])
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
     bg_b = np.zeros(3)
-    dx = np.array([0.1, 0.0, 0.0, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+    dx = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
 
-    dx, v_n, q_nb, bg_b = _reset(dx, v_n, q_nb, bg_b)
+    dx, p_n, v_n, q_nb, bg_b = _reset(dx, p_n, v_n, q_nb, bg_b)
 
     np.testing.assert_allclose(dx, np.zeros_like(dx))
-    np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
+    np.testing.assert_allclose(p_n, np.array([1.1, 0.2, 0.3]))
+    np.testing.assert_allclose(v_n, np.array([0.4, 2.5, 0.6]))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
     np.testing.assert_allclose(
         q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6

From 40631baae8992ea836014dbaeddc448296b718dd Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:23:53 +0200
Subject: [PATCH 116/217] fix test ains init

---
 tests/test_ins_/test_ains.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 652c5b39..c3bb9e1d 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -179,12 +179,13 @@ def test_reset():
     )
 
 
-def test_ahrs_init():
-    mekf = AHRS(10.0)
+def test_ains_init():
+    mekf = AINS(10.0)
+    np.testing.assert_allclose(mekf.position(), np.zeros(3))
     np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
     np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
     np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs.P0))
+    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains.P0))
     assert mekf._g == 9.80665
     assert mekf._nav_frame == "ned"
 

From a041b19e676f6ccf33b951355528d5bd986766f4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:26:13 +0200
Subject: [PATCH 117/217] fix test ains merthods

---
 tests/test_ins_/test_ains.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index c3bb9e1d..974c5b5c 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -191,14 +191,14 @@ def test_ains_init():
 
 
 @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-def test_ahrs_nav_frame(nav_frame, scale):
+def test_ains_nav_frame(nav_frame, scale):
     mekf = AINS(10.0, nav_frame=nav_frame)
 
     assert mekf._nav_frame == nav_frame.lower()
     np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
 
 
-def test_ahrs_methods():
+def test_ains_methods():
     pos_init = np.array([0.1, 10.0, -0.2])
     vel_init = np.array([0.0, 0.1, -0.2])
     euler_init = np.array([10.0, 20.0, 30.0])
@@ -207,7 +207,7 @@ def test_ahrs_methods():
 
     mekf = AINS(10.0, pos=pos_init, vel=vel_init, q=quaternion_init, bg=bg_init)
 
-    np.testing.assert_allclose(mekf.position(), vel_init)
+    np.testing.assert_allclose(mekf.position(), pos_init)
     np.testing.assert_allclose(mekf.velocity(), vel_init)
     np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
     np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)

From 84a756f73a2050cd45888b436ac98eae4184b587 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:34:22 +0200
Subject: [PATCH 118/217] fix tests ains bench no head

---
 tests/test_ins_/test_ains.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 974c5b5c..532e4e59 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -223,7 +223,7 @@ def test_ains_methods():
         (benchmark_pure_attitude_chirp_202311A, True),
     ],
 )
-def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
+def test_ains_no_head_aiding_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
@@ -246,7 +246,7 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
 
     # MEKF
     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = AHRS(
+    mekf = AINS(
         fs_imu,
         q=q0,
         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
@@ -265,6 +265,8 @@ def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
             dvel,
             dtheta,
             degrees=degrees,
+            pos=np.zeros(3),
+            vel=np.zeros(3),
         )
 
         euler_out.append(mekf.euler(degrees=False))

From 00d363b9e958aaa97d41ef0c4462ac4da24136dc Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 09:45:35 +0200
Subject: [PATCH 119/217] add ains to init

---
 src/smsfusion/__init__.py | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 0dac21df..f0afe5f0 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,6 +1,16 @@
 from . import benchmark, calibrate, constants, noise
 from ._coning_sculling import ConingScullingAlg, ConingScullingAlgCalibrated
-from ._ins import AHRS, VRU, AHRSv2, AidedINS, FixedNED, StrapdownINS, VRUv2, gravity
+from ._ins import (
+    AHRS,
+    VRU,
+    AHRSv2,
+    AidedINS,
+    AINSv2,
+    FixedNED,
+    StrapdownINS,
+    VRUv2,
+    gravity,
+)
 from ._smoothing import FixedIntervalSmoother
 from ._transforms import quaternion_from_euler
 
@@ -8,6 +18,7 @@
     "AHRS",
     "AHRSv2",
     "AidedINS",
+    "AINSv2",
     "benchmark",
     "constants",
     "calibrate",

From 29fcb598958f91cdd6ffa08f2b8bd3f0a5e527a3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 10:08:46 +0200
Subject: [PATCH 120/217] start on new bench test

---
 tests/test_ins_/test_ains.py | 318 +++++++++++++++++++++--------------
 1 file changed, 192 insertions(+), 126 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 532e4e59..e68c74ad 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -14,6 +14,8 @@
 from smsfusion._transforms import _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
+    benchmark_full_pva_beat_202311A,
+    benchmark_full_pva_chirp_202311A,
     benchmark_pure_attitude_beat_202311A,
     benchmark_pure_attitude_chirp_202311A,
 )
@@ -219,109 +221,36 @@ def test_ains_methods():
 @pytest.mark.parametrize(
     "benchmark_gen, degrees",
     [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
+        (benchmark_full_pva_beat_202311A, False),
+        # (benchmark_full_pva_chirp_202311A, True),
     ],
 )
-def test_ains_no_head_aiding_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
+def test_ains_benchmark(benchmark_gen, degrees):
+    fs_imu = 10.0
     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
     # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
+    t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU and aiding measurements (with noise)
+    pos_std = 0.1  # m
+    vel_std = 0.01  # m/s
+    head_std = np.radians(1.0)  # rad
+    err_acc = sf.constants.ERR_ACC_MOTION2
+    err_gyro = sf.constants.ERR_GYRO_MOTION2
+    noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
     bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
     imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+    acc_imu = acc_ref + imu_noise[:, :3]
+    gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+    pos_aid = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
+    vel_aid = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
+    head_aid = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
     if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-
-    # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = AINS(
-        fs_imu,
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-    )
-
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
-
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
-
-        mekf.update(
-            dvel,
-            dtheta,
-            degrees=degrees,
-            pos=np.zeros(3),
-            vel=np.zeros(3),
-        )
-
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
-
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
-    )
-
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, degrees",
-    [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
-    ],
-)
-def test_ains_attitude_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+        gyro_imu = np.degrees(gyro_imu)
 
-    head_std = np.radians(1.0)
-    head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
-
-    if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-        head_noise = np.degrees(head_noise)
-        head_std = np.degrees(head_std)
+    # Position and velocity aiding measurements
 
     # MEKF
     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
@@ -333,44 +262,181 @@ def test_ains_attitude_benchmark(benchmark_gen, degrees):
         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
     )
 
-    vel_aid = (0.0, 0.0, 0.0)
-    vel_var = (100.0, 100.0, 100.0)  # (10.0 m/s)^2
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
+    for i, (f_i, w_i, h_i, p_i, v_i) in enumerate(
+        zip(acc_imu, gyro_imu, head_aid, pos_aid, vel_aid)
+    ):
 
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
+        dvel_i = f_i / fs_imu
+        dtheta_i = w_i / fs_imu
 
         mekf.update(
-            dvel,
-            dtheta,
+            dvel_i,
+            dtheta_i,
             degrees=degrees,
-            vel=vel_aid,
-            vel_var=vel_var,
-            head=head_i,
-            head_degrees=degrees,
+            head=h_i,
             head_var=head_std**2,
+            pos=p_i,
+            pos_var=pos_std**2 * np.ones(3),
+            vel=v_i,
+            vel_var=vel_std**2 * np.ones(3),
         )
 
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
-
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
-    )
 
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(yaw_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005
-    assert np.degrees(bias_gyro_z_rms) <= 0.005
+# @pytest.mark.parametrize(
+#     "benchmark_gen, degrees",
+#     [
+#         (benchmark_pure_attitude_beat_202311A, False),
+#         (benchmark_pure_attitude_chirp_202311A, True),
+#     ],
+# )
+# def test_ains_no_head_aiding_benchmark(benchmark_gen, degrees):
+#     fs_imu = 100.0
+#     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+#     # Reference signals (without noise)
+#     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+#     # IMU measurements (with noise)
+#     bg = np.array([0.01, -0.02, 0.0])
+#     noise_model = sf.noise.IMUNoise(
+#         err_acc=sf.constants.ERR_ACC_MOTION2,
+#         err_gyro=sf.constants.ERR_GYRO_MOTION2,
+#         seed=0,
+#     )
+#     imu_noise = noise_model(fs_imu, len(t))
+#     acc_noise = acc_ref + imu_noise[:, :3]
+#     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+#     if degrees:
+#         gyro_noise = np.degrees(gyro_noise)
+
+#     # MEKF
+#     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+#     mekf = AINS(
+#         fs_imu,
+#         q=q0,
+#         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+#         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+#         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+#     )
+
+#     # Apply filter
+#     euler_out, bias_gyro_out = [], []
+#     for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
+
+#         dvel = f_i / fs_imu
+#         dtheta = w_i / fs_imu
+
+#         mekf.update(
+#             dvel,
+#             dtheta,
+#             degrees=degrees,
+#             pos=np.zeros(3),
+#             vel=np.zeros(3),
+#         )
+
+#         euler_out.append(mekf.euler(degrees=False))
+#         bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+#     euler_out = np.array(euler_out)
+#     bias_gyro_out = np.array(bias_gyro_out)
+
+#     # Half-sample shift (compensates for the delay introduced by Euler integration)
+#     euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+#     euler_ref = euler_ref[:-1, :]
+
+#     roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+#     bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+#         (bias_gyro_out - bg)[warmup:], axis=0
+#     )
+
+#     assert np.degrees(roll_rms) <= 0.1
+#     assert np.degrees(pitch_rms) <= 0.1
+#     assert np.degrees(bias_gyro_x_rms) <= 0.005
+#     assert np.degrees(bias_gyro_y_rms) <= 0.005
+
+
+# @pytest.mark.parametrize(
+#     "benchmark_gen, degrees",
+#     [
+#         (benchmark_pure_attitude_beat_202311A, False),
+#         (benchmark_pure_attitude_chirp_202311A, True),
+#     ],
+# )
+# def test_ains_attitude_benchmark(benchmark_gen, degrees):
+#     fs_imu = 100.0
+#     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+#     # Reference signals (without noise)
+#     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+#     # IMU measurements (with noise)
+#     bg = np.array([0.01, -0.02, 0.0])
+#     noise_model = sf.noise.IMUNoise(
+#         err_acc=sf.constants.ERR_ACC_MOTION2,
+#         err_gyro=sf.constants.ERR_GYRO_MOTION2,
+#         seed=0,
+#     )
+#     imu_noise = noise_model(fs_imu, len(t))
+#     acc_noise = acc_ref + imu_noise[:, :3]
+#     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+#     head_std = np.radians(1.0)
+#     head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
+
+#     if degrees:
+#         gyro_noise = np.degrees(gyro_noise)
+#         head_noise = np.degrees(head_noise)
+#         head_std = np.degrees(head_std)
+
+#     # MEKF
+#     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+#     mekf = AINS(
+#         fs_imu,
+#         q=q0,
+#         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+#         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+#         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+#     )
+
+#     vel_aid = (0.0, 0.0, 0.0)
+#     vel_var = (100.0, 100.0, 100.0)  # (10.0 m/s)^2
+#     # Apply filter
+#     euler_out, bias_gyro_out = [], []
+#     for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
+
+#         dvel = f_i / fs_imu
+#         dtheta = w_i / fs_imu
+
+#         mekf.update(
+#             dvel,
+#             dtheta,
+#             degrees=degrees,
+#             vel=vel_aid,
+#             vel_var=vel_var,
+#             head=head_i,
+#             head_degrees=degrees,
+#             head_var=head_std**2,
+#         )
+
+#         euler_out.append(mekf.euler(degrees=False))
+#         bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+#     euler_out = np.array(euler_out)
+#     bias_gyro_out = np.array(bias_gyro_out)
+
+#     # Half-sample shift (compensates for the delay introduced by Euler integration)
+#     euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+#     euler_ref = euler_ref[:-1, :]
+
+#     roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+#     bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+#         (bias_gyro_out - bg)[warmup:], axis=0
+#     )
+
+#     assert np.degrees(roll_rms) <= 0.1
+#     assert np.degrees(pitch_rms) <= 0.1
+#     assert np.degrees(yaw_rms) <= 0.1
+#     assert np.degrees(bias_gyro_x_rms) <= 0.005
+#     assert np.degrees(bias_gyro_y_rms) <= 0.005
+#     assert np.degrees(bias_gyro_z_rms) <= 0.005

From 9294a737b8e9fb5646a590ced44dc9ea140b6280 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 10:18:57 +0200
Subject: [PATCH 121/217] test ains bench

---
 tests/test_ins_/test_ains.py | 40 ++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index e68c74ad..8a51b1d0 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -256,12 +256,15 @@ def test_ains_benchmark(benchmark_gen, degrees):
     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
     mekf = AINS(
         fs_imu,
+        pos=pos_ref[0],
+        vel=vel_ref[0],
         q=q0,
         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
     )
 
+    pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
     for i, (f_i, w_i, h_i, p_i, v_i) in enumerate(
         zip(acc_imu, gyro_imu, head_aid, pos_aid, vel_aid)
     ):
@@ -280,6 +283,43 @@ def test_ains_benchmark(benchmark_gen, degrees):
             vel=v_i,
             vel_var=vel_std**2 * np.ones(3),
         )
+        pos_est.append(mekf.position())
+        vel_est.append(mekf.velocity())
+        euler_est.append(mekf.euler(degrees=False))
+        bias_gyro_est.append(mekf.bias_gyro())
+
+    pos_est = np.array(pos_est)
+    vel_est = np.array(vel_est)
+    euler_est = np.array(euler_est)
+    bias_gyro_est = np.array(bias_gyro_est)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    pos_est = resample_poly(pos_est, 2, 1)[1:-1:2]
+    vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
+    euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+    bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+    pos_ref = pos_ref[:-1, :]
+    vel_ref = vel_ref[:-1, :]
+    euler_ref = euler_ref[:-1, :]
+    bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+    px_std, py_std, pz_std = np.std((pos_est - pos_ref)[warmup:], axis=0)
+    vx_std, vy_std, vz_std = np.std((vel_est - vel_ref)[warmup:], axis=0)
+    roll_std, pitch_std, yaw_std = np.std((euler_est - euler_ref)[warmup:], axis=0)
+    bgx_std, bgy_std, bgz_std = np.std((bias_gyro_est - bias_gyro_ref)[warmup:], axis=0)
+
+    assert px_std <= 1.0
+    assert py_std <= 1.0
+    assert pz_std <= 1.0
+    assert vx_std <= 0.5
+    assert vy_std <= 0.5
+    assert vz_std <= 0.5
+    assert np.degrees(roll_std) <= 0.5
+    assert np.degrees(pitch_std) <= 0.5
+    assert np.degrees(yaw_std) <= 1.0
+    assert np.degrees(bgx_std) <= 0.1
+    assert np.degrees(bgy_std) <= 0.1
+    assert np.degrees(bgz_std) <= 0.1
 
 
 # @pytest.mark.parametrize(

From 16f189e41cd77f6ff3e46aa011724bdc8c3ed23f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 10:22:08 +0200
Subject: [PATCH 122/217] test fix

---
 tests/test_ins_/test_ains.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 8a51b1d0..8b1b43d3 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -265,9 +265,7 @@ def test_ains_benchmark(benchmark_gen, degrees):
     )
 
     pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
-    for i, (f_i, w_i, h_i, p_i, v_i) in enumerate(
-        zip(acc_imu, gyro_imu, head_aid, pos_aid, vel_aid)
-    ):
+    for f_i, w_i, h_i, p_i, v_i in zip(acc_imu, gyro_imu, head_aid, pos_aid, vel_aid):
 
         dvel_i = f_i / fs_imu
         dtheta_i = w_i / fs_imu

From 03936a7ddca1ab4bd7eefce19e0e44467e51064e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 10:56:41 +0200
Subject: [PATCH 123/217] bugfix pos update

---
 src/smsfusion/_ins/_ains_.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 37055468..9d517f4f 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -479,8 +479,8 @@ def update(
                 self._P,
                 self._H[0:3],
                 self._p_n,
-                np.asarray(vel),
-                np.asarray(vel_var),
+                np.asarray(pos),
+                np.asarray(pos_var),
                 R_nb,
                 self._lever_arm,
             )

From f069c139b7634f35804ce9cbc7c0b576579f69b4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 11:14:18 +0200
Subject: [PATCH 124/217] bugfix head aiding

---
 src/smsfusion/_ins/_ains_.py | 2 +-
 tests/test_ins_/test_ains.py | 7 ++++---
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 9d517f4f..f625f3a3 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -496,7 +496,7 @@ def update(
             )
 
         if head is not None:
-            self._H[3, 3:6] = _dhda_head(self._q_nb)  # Update measurement matrix
+            self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # Update measurement matrix
 
             self._dx, self._P = _aiding_update_head(
                 self._dx,
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 8b1b43d3..c1ecaf29 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -276,6 +276,7 @@ def test_ains_benchmark(benchmark_gen, degrees):
             degrees=degrees,
             head=h_i,
             head_var=head_std**2,
+            head_degrees=False,
             pos=p_i,
             pos_var=pos_std**2 * np.ones(3),
             vel=v_i,
@@ -306,9 +307,9 @@ def test_ains_benchmark(benchmark_gen, degrees):
     roll_std, pitch_std, yaw_std = np.std((euler_est - euler_ref)[warmup:], axis=0)
     bgx_std, bgy_std, bgz_std = np.std((bias_gyro_est - bias_gyro_ref)[warmup:], axis=0)
 
-    assert px_std <= 1.0
-    assert py_std <= 1.0
-    assert pz_std <= 1.0
+    assert px_std <= 0.5
+    assert py_std <= 0.5
+    assert pz_std <= 0.5
     assert vx_std <= 0.5
     assert vy_std <= 0.5
     assert vz_std <= 0.5

From 2f59669896db4c7c4ee99c03cbd6fa08e4b8dbdf Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:10:22 +0200
Subject: [PATCH 125/217] test bench fix

---
 tests/test_ins_/test_ains.py | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index c1ecaf29..6a679799 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -307,18 +307,18 @@ def test_ains_benchmark(benchmark_gen, degrees):
     roll_std, pitch_std, yaw_std = np.std((euler_est - euler_ref)[warmup:], axis=0)
     bgx_std, bgy_std, bgz_std = np.std((bias_gyro_est - bias_gyro_ref)[warmup:], axis=0)
 
-    assert px_std <= 0.5
-    assert py_std <= 0.5
-    assert pz_std <= 0.5
-    assert vx_std <= 0.5
-    assert vy_std <= 0.5
-    assert vz_std <= 0.5
-    assert np.degrees(roll_std) <= 0.5
-    assert np.degrees(pitch_std) <= 0.5
-    assert np.degrees(yaw_std) <= 1.0
-    assert np.degrees(bgx_std) <= 0.1
-    assert np.degrees(bgy_std) <= 0.1
-    assert np.degrees(bgz_std) <= 0.1
+    assert px_std <= 0.2
+    assert py_std <= 0.2
+    assert pz_std <= 0.2
+    assert vx_std <= 0.03
+    assert vy_std <= 0.03
+    assert vz_std <= 0.03
+    assert np.degrees(roll_std) <= 0.2
+    assert np.degrees(pitch_std) <= 0.2
+    assert np.degrees(yaw_std) <= 0.5
+    assert np.degrees(bgx_std) <= 0.01
+    assert np.degrees(bgy_std) <= 0.01
+    assert np.degrees(bgz_std) <= 0.01
 
 
 # @pytest.mark.parametrize(

From 1e806d08b361a7253412bcee0ba03c63c440a4b6 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:20:07 +0200
Subject: [PATCH 126/217] reduce head aid uncertainty

---
 tests/test_ins_/test_ains.py | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 6a679799..8045f34e 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -219,13 +219,13 @@ def test_ains_methods():
 
 
 @pytest.mark.parametrize(
-    "benchmark_gen, degrees",
+    "benchmark_gen, gyro_degrees",
     [
         (benchmark_full_pva_beat_202311A, False),
-        # (benchmark_full_pva_chirp_202311A, True),
+        (benchmark_full_pva_chirp_202311A, True),
     ],
 )
-def test_ains_benchmark(benchmark_gen, degrees):
+def test_ains_benchmark(benchmark_gen, gyro_degrees):
     fs_imu = 10.0
     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
@@ -235,7 +235,7 @@ def test_ains_benchmark(benchmark_gen, degrees):
     # IMU and aiding measurements (with noise)
     pos_std = 0.1  # m
     vel_std = 0.01  # m/s
-    head_std = np.radians(1.0)  # rad
+    head_std = np.radians(0.1)  # rad
     err_acc = sf.constants.ERR_ACC_MOTION2
     err_gyro = sf.constants.ERR_GYRO_MOTION2
     noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
@@ -247,7 +247,7 @@ def test_ains_benchmark(benchmark_gen, degrees):
     vel_aid = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
     head_aid = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
-    if degrees:
+    if gyro_degrees:
         gyro_imu = np.degrees(gyro_imu)
 
     # Position and velocity aiding measurements
@@ -273,7 +273,7 @@ def test_ains_benchmark(benchmark_gen, degrees):
         mekf.update(
             dvel_i,
             dtheta_i,
-            degrees=degrees,
+            degrees=gyro_degrees,
             head=h_i,
             head_var=head_std**2,
             head_degrees=False,
@@ -310,11 +310,11 @@ def test_ains_benchmark(benchmark_gen, degrees):
     assert px_std <= 0.2
     assert py_std <= 0.2
     assert pz_std <= 0.2
-    assert vx_std <= 0.03
-    assert vy_std <= 0.03
-    assert vz_std <= 0.03
-    assert np.degrees(roll_std) <= 0.2
-    assert np.degrees(pitch_std) <= 0.2
+    assert vx_std <= 0.1
+    assert vy_std <= 0.1
+    assert vz_std <= 0.1
+    assert np.degrees(roll_std) <= 0.5
+    assert np.degrees(pitch_std) <= 0.5
     assert np.degrees(yaw_std) <= 0.5
     assert np.degrees(bgx_std) <= 0.01
     assert np.degrees(bgy_std) <= 0.01

From e980980003168c7dd25b219486f393a630d6abc0 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:26:25 +0200
Subject: [PATCH 127/217] test fixes

---
 tests/test_ins_/test_ains.py | 19 ++++++++-----------
 1 file changed, 8 insertions(+), 11 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 8045f34e..ef85aec8 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -243,29 +243,26 @@ def test_ains_benchmark(benchmark_gen, gyro_degrees):
     imu_noise = noise_model(fs_imu, len(t))
     acc_imu = acc_ref + imu_noise[:, :3]
     gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
-    pos_aid = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
-    vel_aid = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
-    head_aid = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
+    pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
+    vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
+    head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
     if gyro_degrees:
         gyro_imu = np.degrees(gyro_imu)
 
-    # Position and velocity aiding measurements
-
     # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
     mekf = AINS(
         fs_imu,
         pos=pos_ref[0],
         vel=vel_ref[0],
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+        q=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+        gyro_noise_density=err_gyro["N"],
+        gyro_bias_stability=err_gyro["B"],
+        gyro_bias_corr_time=err_gyro["tau_cb"],
     )
 
     pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
-    for f_i, w_i, h_i, p_i, v_i in zip(acc_imu, gyro_imu, head_aid, pos_aid, vel_aid):
+    for f_i, w_i, h_i, p_i, v_i in zip(acc_imu, gyro_imu, head_meas, pos_meas, vel_meas):
 
         dvel_i = f_i / fs_imu
         dtheta_i = w_i / fs_imu

From 1040c9b19e9d5d9e38d6bcb1ac4675fb0d37ebdf Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:30:39 +0200
Subject: [PATCH 128/217] use rmse in bench test

---
 tests/test_ins_/test_ains.py | 41 ++++++++++++++++++++----------------
 1 file changed, 23 insertions(+), 18 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index ef85aec8..04184570 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -262,7 +262,9 @@ def test_ains_benchmark(benchmark_gen, gyro_degrees):
     )
 
     pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
-    for f_i, w_i, h_i, p_i, v_i in zip(acc_imu, gyro_imu, head_meas, pos_meas, vel_meas):
+    for f_i, w_i, h_i, p_i, v_i in zip(
+        acc_imu, gyro_imu, head_meas, pos_meas, vel_meas
+    ):
 
         dvel_i = f_i / fs_imu
         dtheta_i = w_i / fs_imu
@@ -299,23 +301,26 @@ def test_ains_benchmark(benchmark_gen, gyro_degrees):
     euler_ref = euler_ref[:-1, :]
     bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
-    px_std, py_std, pz_std = np.std((pos_est - pos_ref)[warmup:], axis=0)
-    vx_std, vy_std, vz_std = np.std((vel_est - vel_ref)[warmup:], axis=0)
-    roll_std, pitch_std, yaw_std = np.std((euler_est - euler_ref)[warmup:], axis=0)
-    bgx_std, bgy_std, bgz_std = np.std((bias_gyro_est - bias_gyro_ref)[warmup:], axis=0)
-
-    assert px_std <= 0.2
-    assert py_std <= 0.2
-    assert pz_std <= 0.2
-    assert vx_std <= 0.1
-    assert vy_std <= 0.1
-    assert vz_std <= 0.1
-    assert np.degrees(roll_std) <= 0.5
-    assert np.degrees(pitch_std) <= 0.5
-    assert np.degrees(yaw_std) <= 0.5
-    assert np.degrees(bgx_std) <= 0.01
-    assert np.degrees(bgy_std) <= 0.01
-    assert np.degrees(bgz_std) <= 0.01
+    def rmse(ref, est):
+        return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+    px_rmse, py_rmse, pz_rmse = rmse(pos_ref[warmup:], pos_est[warmup:])
+    vx_rmse, vy_rmse, vz_rmse = rmse(vel_ref[warmup:], vel_est[warmup:])
+    roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
+    bgx_rmse, bgy_rmse, bgz_rmse = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
+
+    assert px_rmse <= 0.2
+    assert py_rmse <= 0.2
+    assert pz_rmse <= 0.2
+    assert vx_rmse <= 0.1
+    assert vy_rmse <= 0.1
+    assert vz_rmse <= 0.1
+    assert np.degrees(roll_rmse) <= 0.5
+    assert np.degrees(pitch_rmse) <= 0.5
+    assert np.degrees(yaw_rmse) <= 0.5
+    assert np.degrees(bgx_rmse) <= 0.01
+    assert np.degrees(bgy_rmse) <= 0.01
+    assert np.degrees(bgz_rmse) <= 0.01
 
 
 # @pytest.mark.parametrize(

From 3d256e772790b3f6cb5af3f7f19a004610bece19 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:33:09 +0200
Subject: [PATCH 129/217] reduce pos error threshold

---
 tests/test_ins_/test_ains.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 04184570..a2cc0f3c 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -309,9 +309,9 @@ def rmse(ref, est):
     roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
     bgx_rmse, bgy_rmse, bgz_rmse = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
 
-    assert px_rmse <= 0.2
-    assert py_rmse <= 0.2
-    assert pz_rmse <= 0.2
+    assert px_rmse <= 0.1
+    assert py_rmse <= 0.1
+    assert pz_rmse <= 0.1
     assert vx_rmse <= 0.1
     assert vy_rmse <= 0.1
     assert vz_rmse <= 0.1

From 2aafee6b900798dc0a42779f8021ea38faf86a54 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:38:36 +0200
Subject: [PATCH 130/217] docstring fix

---
 src/smsfusion/_ins/_ains_.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index f625f3a3..9abe05bf 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -245,8 +245,8 @@ def _reset(
 
 class AINS:
     """
-    Aided inertial navigation system (AINS) using a multiplicative extended
-    Kalman filter (MEKF).
+    Aided inertial navigation system (AINS) using a multiplicative extended Kalman
+    filter (MEKF).
 
     Parameters
     ----------

From af92417bacb201f0f18a0e52086830458221ab4e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:39:06 +0200
Subject: [PATCH 131/217] delete old commented out tests

---
 tests/test_ins_/test_ains.py | 160 -----------------------------------
 1 file changed, 160 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index a2cc0f3c..e64d3a20 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -321,163 +321,3 @@ def rmse(ref, est):
     assert np.degrees(bgx_rmse) <= 0.01
     assert np.degrees(bgy_rmse) <= 0.01
     assert np.degrees(bgz_rmse) <= 0.01
-
-
-# @pytest.mark.parametrize(
-#     "benchmark_gen, degrees",
-#     [
-#         (benchmark_pure_attitude_beat_202311A, False),
-#         (benchmark_pure_attitude_chirp_202311A, True),
-#     ],
-# )
-# def test_ains_no_head_aiding_benchmark(benchmark_gen, degrees):
-#     fs_imu = 100.0
-#     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-#     # Reference signals (without noise)
-#     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-#     # IMU measurements (with noise)
-#     bg = np.array([0.01, -0.02, 0.0])
-#     noise_model = sf.noise.IMUNoise(
-#         err_acc=sf.constants.ERR_ACC_MOTION2,
-#         err_gyro=sf.constants.ERR_GYRO_MOTION2,
-#         seed=0,
-#     )
-#     imu_noise = noise_model(fs_imu, len(t))
-#     acc_noise = acc_ref + imu_noise[:, :3]
-#     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-#     if degrees:
-#         gyro_noise = np.degrees(gyro_noise)
-
-#     # MEKF
-#     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-#     mekf = AINS(
-#         fs_imu,
-#         q=q0,
-#         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-#         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-#         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-#     )
-
-#     # Apply filter
-#     euler_out, bias_gyro_out = [], []
-#     for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
-
-#         dvel = f_i / fs_imu
-#         dtheta = w_i / fs_imu
-
-#         mekf.update(
-#             dvel,
-#             dtheta,
-#             degrees=degrees,
-#             pos=np.zeros(3),
-#             vel=np.zeros(3),
-#         )
-
-#         euler_out.append(mekf.euler(degrees=False))
-#         bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-#     euler_out = np.array(euler_out)
-#     bias_gyro_out = np.array(bias_gyro_out)
-
-#     # Half-sample shift (compensates for the delay introduced by Euler integration)
-#     euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-#     euler_ref = euler_ref[:-1, :]
-
-#     roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-#     bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-#         (bias_gyro_out - bg)[warmup:], axis=0
-#     )
-
-#     assert np.degrees(roll_rms) <= 0.1
-#     assert np.degrees(pitch_rms) <= 0.1
-#     assert np.degrees(bias_gyro_x_rms) <= 0.005
-#     assert np.degrees(bias_gyro_y_rms) <= 0.005
-
-
-# @pytest.mark.parametrize(
-#     "benchmark_gen, degrees",
-#     [
-#         (benchmark_pure_attitude_beat_202311A, False),
-#         (benchmark_pure_attitude_chirp_202311A, True),
-#     ],
-# )
-# def test_ains_attitude_benchmark(benchmark_gen, degrees):
-#     fs_imu = 100.0
-#     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-#     # Reference signals (without noise)
-#     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-#     # IMU measurements (with noise)
-#     bg = np.array([0.01, -0.02, 0.0])
-#     noise_model = sf.noise.IMUNoise(
-#         err_acc=sf.constants.ERR_ACC_MOTION2,
-#         err_gyro=sf.constants.ERR_GYRO_MOTION2,
-#         seed=0,
-#     )
-#     imu_noise = noise_model(fs_imu, len(t))
-#     acc_noise = acc_ref + imu_noise[:, :3]
-#     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-#     head_std = np.radians(1.0)
-#     head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
-
-#     if degrees:
-#         gyro_noise = np.degrees(gyro_noise)
-#         head_noise = np.degrees(head_noise)
-#         head_std = np.degrees(head_std)
-
-#     # MEKF
-#     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-#     mekf = AINS(
-#         fs_imu,
-#         q=q0,
-#         gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-#         gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-#         gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-#     )
-
-#     vel_aid = (0.0, 0.0, 0.0)
-#     vel_var = (100.0, 100.0, 100.0)  # (10.0 m/s)^2
-#     # Apply filter
-#     euler_out, bias_gyro_out = [], []
-#     for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
-
-#         dvel = f_i / fs_imu
-#         dtheta = w_i / fs_imu
-
-#         mekf.update(
-#             dvel,
-#             dtheta,
-#             degrees=degrees,
-#             vel=vel_aid,
-#             vel_var=vel_var,
-#             head=head_i,
-#             head_degrees=degrees,
-#             head_var=head_std**2,
-#         )
-
-#         euler_out.append(mekf.euler(degrees=False))
-#         bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-#     euler_out = np.array(euler_out)
-#     bias_gyro_out = np.array(bias_gyro_out)
-
-#     # Half-sample shift (compensates for the delay introduced by Euler integration)
-#     euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-#     euler_ref = euler_ref[:-1, :]
-
-#     roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-#     bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-#         (bias_gyro_out - bg)[warmup:], axis=0
-#     )
-
-#     assert np.degrees(roll_rms) <= 0.1
-#     assert np.degrees(pitch_rms) <= 0.1
-#     assert np.degrees(yaw_rms) <= 0.1
-#     assert np.degrees(bias_gyro_x_rms) <= 0.005
-#     assert np.degrees(bias_gyro_y_rms) <= 0.005
-#     assert np.degrees(bias_gyro_z_rms) <= 0.005

From ea423dde5f0eda663d17555fd5c643c8e2f19d9f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:41:53 +0200
Subject: [PATCH 132/217] delete obsolete imports

---
 tests/test_ins_/test_ains.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index e64d3a20..26e357fc 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -16,8 +16,6 @@
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
     benchmark_full_pva_chirp_202311A,
-    benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A,
 )
 
 

From 358378fd71b93170a882a5730ad6d246d5a255ad Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:48:54 +0200
Subject: [PATCH 133/217] black python version to pyproject

---
 pyproject.toml               | 3 +++
 src/smsfusion/_ins/_ains_.py | 4 +---
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 30d3c9f3..0e2f8f5b 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -100,3 +100,6 @@ directory = "coverage_html_report"
 files = ["src/smsfusion"]
 strict = true
 ignore_missing_imports = true
+
+[tool.black]
+target-version = ["py312"]
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 9abe05bf..027880ae 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -171,9 +171,7 @@ def _measurement_matrix_init(
     """
     dhdx = np.zeros((7, 12))
     dhdx[0:3, 0:3] = np.eye(3)  # position
-    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(
-        lever_arm
-    )  # position lever arm
+    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     dhdx[3:6, 3:6] = np.eye(3)  # velocity
     dhdx[6:7, 6:9] = _dhda_head(q_nb)  # heading
     return dhdx

From a45d8dfab1d4de3ecdd236d1ac86c83b7ad81827 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 12:51:05 +0200
Subject: [PATCH 134/217] small docstring fix

---
 src/smsfusion/_ins/_ains_.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 027880ae..9d3b6774 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -138,7 +138,7 @@ def _process_noise_covariance_matrix(
 
     Returns
     -------
-    Q : ndarray, shape (12, 12)
+    ndarray, shape (12, 12)
         Process noise covariance matrix.
     """
     Q = np.zeros((12, 12))

From aae912f7e5544eb4006fa7ed0a1af9e3eb43204a Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 13:00:49 +0200
Subject: [PATCH 135/217] dont return arrays from reset

---
 src/smsfusion/_ins/_ains_.py | 11 +++++------
 tests/test_ins_/test_ains.py |  2 +-
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 9d3b6774..a5a65d17 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -235,10 +235,9 @@ def _reset(
     """
     p_n[:] += dx[0:3]
     v_n[:] += dx[3:6]
-    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[6:9])
+    _update_quaternion_with_gibbs2(q_nb, dx[6:9])
     bg_b[:] += dx[9:12]
     dx[:] = 0.0
-    return dx, p_n, v_n, q_nb, bg_b
 
 
 class AINS:
@@ -506,9 +505,9 @@ def update(
                 head_degrees,
             )
 
-        # Reset state
-        self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b = _reset(
-            self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b
-        )
+        # Reset state (in place)
+        # Moves information from the error state vector to the nominal state vectors,
+        # and resets the error state vector to zero
+        _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 26e357fc..7834bac6 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -168,7 +168,7 @@ def test_reset():
     bg_b = np.zeros(3)
     dx = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
 
-    dx, p_n, v_n, q_nb, bg_b = _reset(dx, p_n, v_n, q_nb, bg_b)
+    _reset(dx, p_n, v_n, q_nb, bg_b)
 
     np.testing.assert_allclose(dx, np.zeros_like(dx))
     np.testing.assert_allclose(p_n, np.array([1.1, 0.2, 0.3]))

From 5d6cdbdeac6fee23c34fb605dfc07b914bf8a468 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 13:10:42 +0200
Subject: [PATCH 136/217] in place methods dont return

---
 src/smsfusion/_ins/_ains_.py | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index a5a65d17..5ca2fc61 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -459,7 +459,9 @@ def update(
 
         # Update state-space model
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        self._phi = _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
+        _state_transition_matrix_update(
+            self._phi, dvel, dtheta, R_nb
+        )  # -> update phi (in place)
 
         # Project (a priori) state estimates ahead
         self._p_n[:] += self._dt * self._v_n
@@ -467,11 +469,11 @@ def update(
         self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project (a priori) error covariance matrix estimate ahead
-        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
+        _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if pos is not None:
-            self._dx, self._P = _aiding_update_pos(
+            _aiding_update_pos(
                 self._dx,
                 self._P,
                 self._H[0:3],
@@ -480,22 +482,22 @@ def update(
                 np.asarray(pos_var),
                 R_nb,
                 self._lever_arm,
-            )
+            )  # -> update dx and P (in place)
 
         if vel is not None:
-            self._dx, self._P = _aiding_update_vel(
+            _aiding_update_vel(
                 self._dx,
                 self._P,
                 self._H[3:6],
                 self._v_n,
                 np.asarray(vel),
                 np.asarray(vel_var),
-            )
+            )  # -> update dx and P (in place)
 
         if head is not None:
             self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # Update measurement matrix
 
-            self._dx, self._P = _aiding_update_head(
+            _aiding_update_head(
                 self._dx,
                 self._P,
                 self._H[6],
@@ -503,7 +505,7 @@ def update(
                 head,
                 head_var,
                 head_degrees,
-            )
+            )  # -> update dx and P (in place)
 
         # Reset state (in place)
         # Moves information from the error state vector to the nominal state vectors,

From 9024426bb4f99e8e28964e3bc8ded0f2ac16cabb Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 13:22:10 +0200
Subject: [PATCH 137/217] dont return array if inplace update

---
 src/smsfusion/_ins/_ains_.py | 14 +++++---------
 tests/test_ins_/test_ains.py |  8 ++++----
 2 files changed, 9 insertions(+), 13 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 5ca2fc61..19e26abc 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -114,7 +114,6 @@ def _state_transition_matrix_update(
     phi[3, 8] = -dvy * r00 + dvx * r01
     phi[4, 8] = -dvy * r10 + dvx * r11
     phi[5, 8] = -dvy * r20 + dvx * r21
-    return phi
 
 
 def _process_noise_covariance_matrix(
@@ -457,16 +456,15 @@ def update(
 
         dtheta = 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 (in place)
+
+        # Update state-space model -> update phi (in place)
+        _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
 
         # Project (a priori) state estimates ahead
         self._p_n[:] += self._dt * self._v_n
         self._v_n[:] += R_nb @ dvel + self._dvel_g_corr
-        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+        _update_quaternion_with_rotvec(self._q_nb, dtheta)  # -> update q_nb (in place)
 
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
@@ -507,9 +505,7 @@ def update(
                 head_degrees,
             )  # -> update dx and P (in place)
 
-        # Reset state (in place)
-        # Moves information from the error state vector to the nominal state vectors,
-        # and resets the error state vector to zero
+        # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 7834bac6..358c597f 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -54,12 +54,12 @@ def test_state_transition_matrix_update():
     R_nb = np.eye(3)
     gbc = 0.01
 
-    phi_init = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+    phi = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
 
     dtheta_update = np.ones(3) * 0.01
     dvel_update = np.ones(3) * 0.1
-    phi_out = _state_transition_matrix_update(
-        phi_init, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
+    _state_transition_matrix_update(
+        phi, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
     )
 
     phi_expected = np.array(
@@ -79,7 +79,7 @@ def test_state_transition_matrix_update():
         ]
     )
 
-    np.testing.assert_almost_equal(phi_out, phi_expected)
+    np.testing.assert_almost_equal(phi, phi_expected)
 
 
 def test_measurement_matrix_init():

From 7352a0a7b2d578138aa722eaed9d17073664cd89 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 13:29:03 +0200
Subject: [PATCH 138/217] small fix

---
 src/smsfusion/_ins/_ains_.py | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 19e26abc..f8a8a9e7 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -471,6 +471,8 @@ def update(
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if pos is not None:
+
+            # -> update dx and P (in place)
             _aiding_update_pos(
                 self._dx,
                 self._P,
@@ -483,6 +485,8 @@ def update(
             )  # -> update dx and P (in place)
 
         if vel is not None:
+
+            # -> update dx and P (in place)
             _aiding_update_vel(
                 self._dx,
                 self._P,
@@ -490,11 +494,12 @@ def update(
                 self._v_n,
                 np.asarray(vel),
                 np.asarray(vel_var),
-            )  # -> update dx and P (in place)
+            )
 
         if head is not None:
             self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # Update measurement matrix
 
+            # -> update dx and P (in place)
             _aiding_update_head(
                 self._dx,
                 self._P,
@@ -503,9 +508,10 @@ def update(
                 head,
                 head_var,
                 head_degrees,
-            )  # -> update dx and P (in place)
+            )
 
-        # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
+        # Reset state
+        # -> update p_n, v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self

From 275eb9827dc8d261c12712eebc390849e2ee6386 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:00:18 +0200
Subject: [PATCH 139/217] comments

---
 src/smsfusion/_ins/_ains_.py | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index f8a8a9e7..e14d62f3 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -472,8 +472,7 @@ def update(
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if pos is not None:
 
-            # -> update dx and P (in place)
-            _aiding_update_pos(
+            _aiding_update_pos(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
                 self._H[0:3],
@@ -482,12 +481,11 @@ def update(
                 np.asarray(pos_var),
                 R_nb,
                 self._lever_arm,
-            )  # -> update dx and P (in place)
+            )
 
         if vel is not None:
 
-            # -> update dx and P (in place)
-            _aiding_update_vel(
+            _aiding_update_vel(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
                 self._H[3:6],
@@ -497,10 +495,9 @@ def update(
             )
 
         if head is not None:
-            self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # Update measurement matrix
+            self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # update measurement matrix
 
-            # -> update dx and P (in place)
-            _aiding_update_head(
+            _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
                 self._H[6],
@@ -510,8 +507,7 @@ def update(
                 head_degrees,
             )
 
-        # Reset state
-        # -> update p_n, v_n, q_nb, bg_b and dx (in place)
+        # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self

From 744daf6099aee755edb6d433a7beaa8b9d2cb8d5 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:02:20 +0200
Subject: [PATCH 140/217] small fix

---
 src/smsfusion/_ins/_ains_.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index e14d62f3..d9f0c8d8 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -495,7 +495,7 @@ def update(
             )
 
         if head is not None:
-            self._H[6:7, 6:9] = _dhda_head(self._q_nb)  # update measurement matrix
+            self._H[6, 6:9] = _dhda_head(self._q_nb)  # update measurement matrix
 
             _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,

From 3604bfe024d2c3e9799f19c08ed819421b5d70d0 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:10:19 +0200
Subject: [PATCH 141/217] move raise exceptions out of aiding functions

---
 src/smsfusion/_ins/_aiding.py | 12 +-----------
 src/smsfusion/_ins/_ains_.py  |  7 +++++++
 2 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index b6d94a13..98f52a1c 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -28,10 +28,7 @@ def _aiding_update_pos(
     Update with position aiding measurement.
     """
 
-    if pos_var is None:
-        raise ValueError("'pos_var' not provided.")
-
-    if not lever_arm.any():
+    if lever_arm.any():
         dz = pos_meas - (pos_n + R_nb @ lever_arm)
     else:
         dz = pos_meas - pos_n
@@ -52,10 +49,6 @@ def _aiding_update_vel(
     """
     Update with velocity aiding measurement.
     """
-
-    if vel_var is None:
-        raise ValueError("'vel_var' not provided.")
-
     dz = vel_meas - vel_n
     dx, P = _kalman_update_sequential(dx, P, dz, vel_var, H)
     return dx, P
@@ -75,9 +68,6 @@ def _aiding_update_head(
     Update with heading aiding measurement.
     """
 
-    if head_var is None:
-        raise ValueError("'head_var' not provided.")
-
     if head_degrees:
         head_meas = (np.pi / 180.0) * head_meas
         head_var = (np.pi / 180.0) ** 2 * head_var
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index d9f0c8d8..29eaf749 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -471,6 +471,8 @@ def update(
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if pos is not None:
+            if pos_var is None:
+                raise ValueError("'pos_var' is required for position aiding.")
 
             _aiding_update_pos(  # -> update dx and P (in place)
                 self._dx,
@@ -484,6 +486,8 @@ def update(
             )
 
         if vel is not None:
+            if vel_var is None:
+                raise ValueError("'vel_var' is required for velocity aiding.")
 
             _aiding_update_vel(  # -> update dx and P (in place)
                 self._dx,
@@ -495,6 +499,9 @@ def update(
             )
 
         if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
             self._H[6, 6:9] = _dhda_head(self._q_nb)  # update measurement matrix
 
             _aiding_update_head(  # -> update dx and P (in place)

From 102b1938211db3cf6040099a0af641a288547812 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:17:30 +0200
Subject: [PATCH 142/217] rename initial value parameters

---
 src/smsfusion/_ins/_ains_.py | 35 +++++++++++++++++------------------
 tests/test_ins_/test_ains.py |  8 ++++----
 2 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 29eaf749..c96f47f2 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -248,16 +248,16 @@ class AINS:
     ----------
     fs : float
         Sampling rate in Hz.
-    pos : array_like, shape (3,), optional
-        Initial position estimate in m from origin. Defaults to origin (0.0, 0.0, 0.0).
-    vel : array_like, shape (3,), optional
+    p0 : array_like, shape (3,), optional
+        Initial position estimate in m. Defaults to origin (0.0, 0.0, 0.0).
+    v0 : array_like, shape (3,), optional
         Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
-    q : array_like, shape (4,), optional
+    q0 : array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg : array_like, shape (3,), optional
+    bg0 : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P : array_like, shape (6, 6), optional
+    P0 : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
         a small diagonal matrix (1e-6 * np.eye(9)).
     acc_noise_density : float, optional
@@ -278,22 +278,21 @@ class AINS:
         Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
         (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
         will be expressed relative to this frame.
-    lever_arm : array-like, shape (3,), default (0.0, 0.0, 0.0)
+    lever_arm : array-like, shape (3,), optional
         Lever-arm vector describing the location of position aiding (in meters) relative
         to the IMU expressed in the IMU's measurement frame. For instance, the location
         of the GNSS antenna relative to the IMU. By default it is assumed that the
         aiding position coincides with the IMU's origin.
-
     """
 
     def __init__(
         self,
         fs: float,
-        pos: ArrayLike = (0.0, 0.0, 0.0),
-        vel: ArrayLike = (0.0, 0.0, 0.0),
-        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = P0,
+        p0: ArrayLike = (0.0, 0.0, 0.0),
+        v0: ArrayLike = (0.0, 0.0, 0.0),
+        q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg0: ArrayLike = (0.0, 0.0, 0.0),
+        P0: ArrayLike = P0,
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
@@ -317,11 +316,11 @@ def __init__(
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._p_n = np.asarray_chkfinite(pos).reshape(3).copy()
-        self._v_n = np.asarray_chkfinite(vel).reshape(3).copy()
-        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(12, 12).copy()
+        self._p_n = np.asarray_chkfinite(p0).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(v0).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q0).reshape(4).copy()
+        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)
 
         # Discrete state-space model
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 358c597f..9319d174 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -205,7 +205,7 @@ def test_ains_methods():
     quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
     bg_init = np.array([0.01, -0.01, 0.02])
 
-    mekf = AINS(10.0, pos=pos_init, vel=vel_init, q=quaternion_init, bg=bg_init)
+    mekf = AINS(10.0, p0=pos_init, v0=vel_init, q0=quaternion_init, bg0=bg_init)
 
     np.testing.assert_allclose(mekf.position(), pos_init)
     np.testing.assert_allclose(mekf.velocity(), vel_init)
@@ -251,9 +251,9 @@ def test_ains_benchmark(benchmark_gen, gyro_degrees):
     # MEKF
     mekf = AINS(
         fs_imu,
-        pos=pos_ref[0],
-        vel=vel_ref[0],
-        q=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+        p0=pos_ref[0],
+        v0=vel_ref[0],
+        q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
         gyro_noise_density=err_gyro["N"],
         gyro_bias_stability=err_gyro["B"],
         gyro_bias_corr_time=err_gyro["tau_cb"],

From 642b6f8a719da14c152581b67178a5166ae8c120 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:24:49 +0200
Subject: [PATCH 143/217] few fixes

---
 src/smsfusion/_ins/_aiding.py |  4 ----
 src/smsfusion/_ins/_ains_.py  | 24 +++++++++---------------
 2 files changed, 9 insertions(+), 19 deletions(-)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index 98f52a1c..72ac98f6 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -88,10 +88,6 @@ def _aiding_update_gref(
     """
     Update state and covariance with gravity reference vector aiding measurement.
     """
-
-    if gref_var is None:
-        raise ValueError("gref_var is not provided; required for gref aiding.")
-
     dz = -_normalize(dvel) - vg_b
     dx, P = _kalman_update_sequential(dx, P, dz, gref_var, H)
     return dx, P
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index c96f47f2..9f85ecea 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -74,7 +74,7 @@ def _state_transition_matrix_update(
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
     R_nb: NDArray[np.float64],
-) -> NDArray[np.float64]:
+) -> None:
     """
     Update the state transition matrix in place.
 
@@ -168,12 +168,12 @@ def _measurement_matrix_init(
     ndarray, shape (7, 12)
         Linearized measurement matrix.
     """
-    dhdx = np.zeros((7, 12))
-    dhdx[0:3, 0:3] = np.eye(3)  # position
-    dhdx[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
-    dhdx[3:6, 3:6] = np.eye(3)  # velocity
-    dhdx[6:7, 6:9] = _dhda_head(q_nb)  # heading
-    return dhdx
+    H = np.zeros((7, 12))
+    H[0:3, 0:3] = np.eye(3)  # position
+    H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
+    H[3:6, 3:6] = np.eye(3)  # velocity
+    H[6:7, 6:9] = _dhda_head(q_nb)  # heading
+    return H
 
 
 def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
@@ -208,15 +208,9 @@ def _reset(
     v_n: NDArray[np.float64],
     q_nb: NDArray[np.float64],
     bg_b: NDArray[np.float64],
-) -> tuple[
-    NDArray[np.float64],
-    NDArray[np.float64],
-    NDArray[np.float64],
-    NDArray[np.float64],
-    NDArray[np.float64],
-]:
+) -> None:
     """
-    Reset state.
+    Reset state (in place).
 
     Parameters
     ----------

From ab8a43e93adea75470542816c96d54aa322af22d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:28:51 +0200
Subject: [PATCH 144/217] docstring

---
 src/smsfusion/_ins/_ains_.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 9f85ecea..80647683 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -420,17 +420,19 @@ def update(
             Position aiding measurement in m. If ``None``, position aiding ins not used.
         pos_var : array-like, shape (3,), optional
             Variance of position measurement noise in m^2. Ignored if ``pos`` is ``None``.
+            Defaults to (1e6, 1e6, 1e6) m^2.
         vel : array-like, shape (3,), optional
             Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
         vel_var : array-like, shape (3,), optional
             Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel`` is ``None``.
+            Defaults to (100.0, 100.0, 100.0) (m/s)^2.
         head : float, optional
             Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
             assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
             If ``None``, compass aiding is not used. See ``head_degrees`` for units.
         head_var : float, optional
             Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
+            See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
         head_degrees : bool, default False
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
             or radians and radians^2. Default is in radians and radians^2.

From dd527646b11ba9a49198efaa52f3dca08ed59af0 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:33:31 +0200
Subject: [PATCH 145/217] delete obsolete imports

---
 src/smsfusion/_ins/_aiding.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index 72ac98f6..0418de4f 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -1,11 +1,10 @@
 import numpy as np
 from numba import njit
-from numpy.typing import ArrayLike, NDArray
+from numpy.typing import NDArray
 
-from smsfusion._vectorops import _normalize, _skew_symmetric
+from smsfusion._vectorops import _normalize
 
 from ._common import (
-    _dhda_head,
     _h_head,
     _kalman_update_scalar,
     _kalman_update_sequential,

From ae32f0c304be41f1b242954090ef23348a5c7119 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Mon, 24 Aug 2026 15:37:50 +0200
Subject: [PATCH 146/217] small fix

---
 src/smsfusion/_ins/_ains_.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 80647683..58a5c268 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -451,9 +451,8 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        R_nb = _rot_matrix_from_quaternion(self._q_nb)
-
         # Update state-space model -> update phi (in place)
+        R_nb = _rot_matrix_from_quaternion(self._q_nb)
         _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
 
         # Project (a priori) state estimates ahead

From 28923c5b654df8d57214f9bbc31d24cbacf64fa3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 08:50:06 +0200
Subject: [PATCH 147/217] lever arm docstring fix

---
 src/smsfusion/_ins/_ains_.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 58a5c268..81f575e4 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -274,7 +274,7 @@ class AINS:
         will be expressed relative to this frame.
     lever_arm : array-like, shape (3,), optional
         Lever-arm vector describing the location of position aiding (in meters) relative
-        to the IMU expressed in the IMU's measurement frame. For instance, the location
+        to the IMU expressed in the IMU/body reference frame. For instance, the location
         of the GNSS antenna relative to the IMU. By default it is assumed that the
         aiding position coincides with the IMU's origin.
     """

From b698adcb4ec47daafd9c05909fcf2a67e1e7e382 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 08:58:41 +0200
Subject: [PATCH 148/217] docstring fixes

---
 src/smsfusion/_ins/_ains_.py | 52 ++++++++++++++++++++----------------
 1 file changed, 29 insertions(+), 23 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 81f575e4..09df217e 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -332,30 +332,31 @@ def __init__(
 
     def position(self) -> NDArray[np.float64]:
         """
-        Position expressed in the navigation frame.
+        Copy of the position estimate in meters.
         """
         return self._p_n.copy()
 
     def velocity(self) -> NDArray[np.float64]:
         """
-        Velocity expressed in the navigation frame.
+        Copy of the velocity estimate in m/s.
         """
         return self._v_n.copy()
 
     def quaternion(self) -> NDArray[np.float64]:
         """
-        Attitude expressed as a unit quaternion.
+        Copy of the attitude estimate expressed as a unit quaternion.
         """
         return self._q_nb.copy()
 
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
+        Copy of the attitude estimate expressed as Euler angles (roll, pitch, yaw).
 
         Parameters
         ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
+        degrees : bool, optional
+            Whether to return the Euler angles in degrees or radians. Defaults to
+            radians.
 
         Returns
         -------
@@ -372,7 +373,8 @@ def euler(self, degrees: bool = False) -> NDArray[np.float64]:
 
     def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
         """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
+        Copy of the gyroscope bias estimate in rad/s or deg/s depending on the
+        ``degrees`` flag.
 
         Parameters
         ----------
@@ -409,33 +411,37 @@ def update(
 
         Parameters
         ----------
-        dvel : array_like, shape (3,), optional
+        dvel : array_like, shape (3,)
             Velocity increment (sculling integral) in m/s.
-        dtheta : array_like, shape (3,), optional
-            Attitude increment (coning integral) in radians.
+        dtheta : array_like, shape (3,)
+            Attitude increment (coning integral) in radians or degrees depending
+            on the ``degrees`` flag.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
         pos : array-like, shape (3,), optional
-            Position aiding measurement in m. If ``None``, position aiding ins not used.
+            Position aiding measurement in meters. If ``None``, position aiding
+            is not used.
         pos_var : array-like, shape (3,), optional
-            Variance of position measurement noise in m^2. Ignored if ``pos`` is ``None``.
-            Defaults to (1e6, 1e6, 1e6) m^2.
+            Variance of position measurement noise in m^2. Ignored if ``pos`` is
+            ``None``. Defaults to (1e6, 1e6, 1e6) m^2.
         vel : array-like, shape (3,), optional
-            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is
+            not used.
         vel_var : array-like, shape (3,), optional
-            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel`` is ``None``.
-            Defaults to (100.0, 100.0, 100.0) (m/s)^2.
+            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
+            is ``None``. Defaults to (100.0, 100.0, 100.0) (m/s)^2.
         head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+            Heading measurement in radians or degrees depending on the ``head_degrees``
+            flag. I.e., the yaw angle of the 'body' frame relative to the assumed
+            'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used.
         head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-            See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
+            Variance of heading measurement noise in radians^2 or degrees^2 depending
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
         head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees
+            and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
 
         Returns
         -------

From 80b7d5fb5b1ebef6085c48f241b26f7a9691ffc9 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 09:02:17 +0200
Subject: [PATCH 149/217] remove default aiding variances AINS

---
 src/smsfusion/_ins/_ains_.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 09df217e..b53c5dac 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -399,11 +399,11 @@ def update(
         dtheta: ArrayLike,
         degrees: bool = False,
         pos: ArrayLike | None = None,
-        pos_var: ArrayLike = (1.0e6, 1.0e6, 1.0e6),
+        pos_var: ArrayLike | None = None,
         vel: ArrayLike | None = None,
-        vel_var: ArrayLike = (100.0, 100.0, 100.0),
+        vel_var: ArrayLike | None = None,
         head: float | None = None,
-        head_var: float = 0.001,
+        head_var: float | None = None,
         head_degrees: bool = False,
     ) -> Self:
         """
@@ -424,13 +424,13 @@ def update(
             is not used.
         pos_var : array-like, shape (3,), optional
             Variance of position measurement noise in m^2. Ignored if ``pos`` is
-            ``None``. Defaults to (1e6, 1e6, 1e6) m^2.
+            ``None``.
         vel : array-like, shape (3,), optional
             Velocity aiding measurement in m/s. If ``None``, velocity aiding is
             not used.
         vel_var : array-like, shape (3,), optional
             Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
-            is ``None``. Defaults to (100.0, 100.0, 100.0) (m/s)^2.
+            is ``None``.
         head : float, optional
             Heading measurement in radians or degrees depending on the ``head_degrees``
             flag. I.e., the yaw angle of the 'body' frame relative to the assumed

From 5b6436ae886a97ba3192348575f55f6aec4b6891 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 09:06:59 +0200
Subject: [PATCH 150/217] comments adjustment

---
 src/smsfusion/_ins/_ains_.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index b53c5dac..4ea9bf06 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -457,9 +457,9 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        # Update state-space model -> update phi (in place)
+        # Update state-space model
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
+        _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)  # -> update phi
 
         # Project (a priori) state estimates ahead
         self._p_n[:] += self._dt * self._v_n
@@ -502,7 +502,7 @@ def update(
             if head_var is None:
                 raise ValueError("'head_var' is required for heading aiding.")
 
-            self._H[6, 6:9] = _dhda_head(self._q_nb)  # update measurement matrix
+            self._H[6, 6:9] = _dhda_head(self._q_nb)
 
             _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,

From e13a470b50ab9a15c2a5cf1b9ac1c9a4ccae79f9 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 09:26:14 +0200
Subject: [PATCH 151/217] docstring init

---
 src/smsfusion/_ins/_ains_.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 4ea9bf06..b5970223 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -235,8 +235,10 @@ def _reset(
 
 class AINS:
     """
-    Aided inertial navigation system (AINS) using a multiplicative extended Kalman
-    filter (MEKF).
+    Aided inertial navigation system (AINS).
+    
+    This class provides position, velocity, attitude and gyro bias estimation using
+    a multiplicative extended Kalman filter (MEKF).
 
     Parameters
     ----------

From c72a41f7b2ae088c03f30e54b521248d8d84c110 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 09:36:28 +0200
Subject: [PATCH 152/217] docstring fix lever arm

---
 src/smsfusion/_ins/_ains_.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index b5970223..150e1104 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -159,9 +159,8 @@ def _measurement_matrix_init(
         Unit quaternion.
     lever_arm : ndarray, shape(3,)
         Lever-arm vector describing the location of position aiding (in meters) relative
-        to the IMU expressed in the IMU's measurement frame. For instance, the location
-        of the GNSS antenna relative to the IMU. By default it is assumed that the
-        aiding position coincides with the IMU's origin.
+        to the IMU expressed in the IMU/body reference frame. For instance, the location
+        of the GNSS antenna relative to the IMU.
 
     Returns
     -------

From 54123d4788fd7c1b6c2b10d7f09cdd72680e2899 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 09:38:01 +0200
Subject: [PATCH 153/217] black

---
 src/smsfusion/_ins/_ains_.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 150e1104..2eda544a 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -235,7 +235,7 @@ def _reset(
 class AINS:
     """
     Aided inertial navigation system (AINS).
-    
+
     This class provides position, velocity, attitude and gyro bias estimation using
     a multiplicative extended Kalman filter (MEKF).
 

From 98d9aafc8470cf2de657d62e05ba95ca01b33338 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:19:58 +0200
Subject: [PATCH 154/217] extend test mekf init default

---
 tests/test_ins_/test_ains.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 9319d174..282f29de 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -179,15 +179,23 @@ def test_reset():
     )
 
 
-def test_ains_init():
+def test_ains_init_default():
     mekf = AINS(10.0)
+    assert mekf._fs == pytest.approx(10.0)
+    assert mekf._dt == pytest.approx(0.1)
+    assert mekf._g == 9.80665
+    assert mekf._nav_frame == "ned"
+    assert mekf._vrw == pytest.approx(0.0007)
+    assert mekf._arw == pytest.approx(0.00005)
+    assert mekf._gbs == pytest.approx(0.00005)
+    assert mekf._gbc == pytest.approx(50.0)
+    np.testing.assert_allclose(mekf._lever_arm, np.zeros(3))
     np.testing.assert_allclose(mekf.position(), np.zeros(3))
     np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
     np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
     np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
     np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains.P0))
-    assert mekf._g == 9.80665
-    assert mekf._nav_frame == "ned"
+    np.testing.assert_allclose(mekf._dx, np.zeros(12))
 
 
 @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))

From 6bd0b3cead1aa7bb128aa7c476cd5be0a8563413 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:26:27 +0200
Subject: [PATCH 155/217] test ains init

---
 tests/test_ins_/test_ains.py | 44 ++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 282f29de..5c7ecda3 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -179,6 +179,50 @@ def test_reset():
     )
 
 
+def test_ains_init():
+
+    p0 = np.random.random(3)
+    v0 = np.random.random(3)
+    q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
+    bg0 = np.random.random(3)
+    vrw = 0.0001
+    arw = 0.0002
+    gbs = 0.0003
+    gbc = 123.0
+
+    mekf = AINS(
+        51.2,
+        p0=p0,
+        v0=v0,
+        q0=q0,
+        bg0=bg0,
+        P0=0.1 * np.eye(12),
+        acc_noise_density=vrw,
+        gyro_noise_density=arw,
+        gyro_bias_stability=gbs,
+        gyro_bias_corr_time=gbc,
+        g=9.81,
+        nav_frame="enu",
+        lever_arm=np.array([0.1, 0.2, 0.3]),
+    )
+
+    assert mekf._fs == pytest.approx(51.2)
+    assert mekf._dt == pytest.approx(1.0 / 51.2)
+    assert mekf._g == 9.81
+    assert mekf._nav_frame == "enu"
+    assert mekf._vrw == pytest.approx(vrw)
+    assert mekf._arw == pytest.approx(arw)
+    assert mekf._gbs == pytest.approx(gbs)
+    assert mekf._gbc == pytest.approx(gbc)
+    np.testing.assert_allclose(mekf._lever_arm, np.array([0.1, 0.2, 0.3]))
+    np.testing.assert_allclose(mekf.position(), p0)
+    np.testing.assert_allclose(mekf.velocity(), v0)
+    np.testing.assert_allclose(mekf.quaternion(), q0)
+    np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+    np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
+    np.testing.assert_allclose(mekf._dx, np.zeros(12))
+
+
 def test_ains_init_default():
     mekf = AINS(10.0)
     assert mekf._fs == pytest.approx(10.0)

From 76b942aa8214be043a69e08b6f533e20306ee1f7 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:27:26 +0200
Subject: [PATCH 156/217] test fix

---
 tests/test_ins_/test_ains.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 5c7ecda3..b351bf4b 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -214,6 +214,7 @@ def test_ains_init():
     assert mekf._arw == pytest.approx(arw)
     assert mekf._gbs == pytest.approx(gbs)
     assert mekf._gbc == pytest.approx(gbc)
+    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, -9.81]))
     np.testing.assert_allclose(mekf._lever_arm, np.array([0.1, 0.2, 0.3]))
     np.testing.assert_allclose(mekf.position(), p0)
     np.testing.assert_allclose(mekf.velocity(), v0)
@@ -233,6 +234,7 @@ def test_ains_init_default():
     assert mekf._arw == pytest.approx(0.00005)
     assert mekf._gbs == pytest.approx(0.00005)
     assert mekf._gbc == pytest.approx(50.0)
+    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, 9.80665]))
     np.testing.assert_allclose(mekf._lever_arm, np.zeros(3))
     np.testing.assert_allclose(mekf.position(), np.zeros(3))
     np.testing.assert_allclose(mekf.velocity(), np.zeros(3))

From 7ee01edad06f39da802d83371d46b7419a39f754 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:37:37 +0200
Subject: [PATCH 157/217] extend test

---
 tests/test_ins_/test_ains.py | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index b351bf4b..b6aaf234 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -259,15 +259,25 @@ def test_ains_methods():
     quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
     bg_init = np.array([0.01, -0.01, 0.02])
 
-    mekf = AINS(10.0, p0=pos_init, v0=vel_init, q0=quaternion_init, bg0=bg_init)
+    mekf = AINS(
+        10.0,
+        p0=pos_init,
+        v0=vel_init,
+        q0=quaternion_init,
+        bg0=bg_init,
+        P0=0.1 * np.eye(12),
+    )
 
     np.testing.assert_allclose(mekf.position(), pos_init)
     np.testing.assert_allclose(mekf.velocity(), vel_init)
     np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+    np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler_init))
     np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
     np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
     np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+    np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg_init)
     np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+    np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
 
 
 @pytest.mark.parametrize(

From 1374dcc3671903f713be1c01d4f5f5a31a61ffe4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:44:42 +0200
Subject: [PATCH 158/217] docstring fixes

---
 src/smsfusion/_ins/_aiding.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index 0418de4f..0b214ca7 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -24,7 +24,7 @@ def _aiding_update_pos(
     lever_arm: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
     """
-    Update with position aiding measurement.
+    Update error state (dx) and error covariance (P) with position aiding measurement.
     """
 
     if lever_arm.any():
@@ -46,7 +46,7 @@ def _aiding_update_vel(
     vel_var: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
     """
-    Update with velocity aiding measurement.
+    Update error state (dx) and error covariance (P) with velocity aiding measurement.
     """
     dz = vel_meas - vel_n
     dx, P = _kalman_update_sequential(dx, P, dz, vel_var, H)
@@ -64,7 +64,7 @@ def _aiding_update_head(
     head_degrees: bool,
 ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
     """
-    Update with heading aiding measurement.
+    Update error state (dx) and error covariance (P) with heading aiding measurement.
     """
 
     if head_degrees:
@@ -85,7 +85,8 @@ def _aiding_update_gref(
     gref_var: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
     """
-    Update state and covariance with gravity reference vector aiding measurement.
+    Update error state (dx) and error covariance (P) with gravity reference vector
+    aiding measurement.
     """
     dz = -_normalize(dvel) - vg_b
     dx, P = _kalman_update_sequential(dx, P, dz, gref_var, H)

From fa41761295b6021d7ccd2f2fb222762ef948e924 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:48:15 +0200
Subject: [PATCH 159/217] small fix

---
 src/smsfusion/_ins/_common.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index 3a985918..d7afc490 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -99,8 +99,8 @@ def _update_quaternion_with_rotvec(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
 ) -> NDArray[np.float64]:
     """
-    Update (inplace) a unit quaternion, q, with a small attitude increment, dtheta,
-    parameterized as a rotation vector.
+    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
+    as a rotation vector.
 
     Parameters
     ----------

From 9ce9a591275d5bc795a53a46c7f8a559f5d22034 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 10:56:00 +0200
Subject: [PATCH 160/217] fix gref_b_from_quat

---
 src/smsfusion/_ins/_common.py  | 11 ++++-------
 src/smsfusion/_ins/_vru.py     |  4 ++--
 tests/test_ins_/test_common.py |  4 ++--
 3 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index d7afc490..dbf1aa22 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -350,15 +350,12 @@ def _nz2vg(nav_frame: str) -> float:
 
 
 @njit  # type: ignore[misc]
-def _nz_b_from_quat(
+def _gref_b_from_quat(
     q_nb: NDArray[np.float64], nav_frame_factor: float = 1.0
 ) -> NDArray[np.float64]:
     """
-    Unit vector describing the z-axis of frame {n} expressed in frame {b}, computed
-    from a unit quaternion, q_nb.
-
-    Note that this vector corresponds to the third row of the rotation matrix which
-    transforms a vector from {b} to {n}.
+    Compute the gravity reference vector (unit vector) expressed in the body frame
+    from a unit quaternion.
 
     Parameters
     ----------
@@ -371,7 +368,7 @@ def _nz_b_from_quat(
     Returns
     -------
     numpy.ndarray, shape (3,)
-        The z-axis (unit vector) of frame {n} expressed in frame {b}.
+        Gravity reference vector expressed in the body frame (unit vector).
     """
 
     x = 2.0 * (q_nb[1] * q_nb[3] - q_nb[0] * q_nb[2])
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 78214527..f7a245e9 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -9,8 +9,8 @@
 
 from ._aiding import _aiding_update_gref
 from ._common import (
+    _gref_b_from_quat,
     _nz2vg,
-    _nz_b_from_quat,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
@@ -317,7 +317,7 @@ def update(
 
         # Update (a posteriori) state and covariance estimates with aiding measurements
         if gref is True:
-            vg_b = _nz_b_from_quat(self._q_nb, self._nz2vg)
+            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
             self._H[0:3, 0:3] = _skew_symmetric(vg_b)  # Update measurement matrix
 
             self._dx, self._P = _aiding_update_gref(
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py
index 1712dfa1..04c0a777 100644
--- a/tests/test_ins_/test_common.py
+++ b/tests/test_ins_/test_common.py
@@ -209,8 +209,8 @@ def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expect
         (np.array([np.cos(0.1 / 2), 0.0, 0.0, np.sin(0.1 / 2)]), 1.0),
     ],
 )
-def test__nz_b_from_quat(q_nb, nav_frame_factor):
-    out = _common._nz_b_from_quat(q_nb, nav_frame_factor=nav_frame_factor)
+def test__gref_b_from_quat(q_nb, nav_frame_factor):
+    out = _common._gref_b_from_quat(q_nb, nav_frame_factor=nav_frame_factor)
 
     expect = Rotation.from_quat(q_nb, scalar_first=True).apply(
         nav_frame_factor * np.array([0.0, 0.0, 1.0]), inverse=True

From d44a2a7d18a68dd7b86912d800e6dc7515db87a1 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:05:00 +0200
Subject: [PATCH 161/217] test ains bench gz bias

---
 tests/test_ins_/test_ahrs.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index 20646284..5e10f8a0 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -246,7 +246,7 @@ def test_ahrs_benchmark(benchmark_gen, degrees):
     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
     # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
+    bg = np.array([0.01, -0.02, 0.03])
     noise_model = sf.noise.IMUNoise(
         err_acc=sf.constants.ERR_ACC_MOTION2,
         err_gyro=sf.constants.ERR_GYRO_MOTION2,

From 7030dc98603753b71a8514d92844e648fc65bb9f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:05:57 +0200
Subject: [PATCH 162/217] revert gz bias

---
 tests/test_ins_/test_ahrs.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index 5e10f8a0..20646284 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -246,7 +246,7 @@ def test_ahrs_benchmark(benchmark_gen, degrees):
     t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
     # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.03])
+    bg = np.array([0.01, -0.02, 0.0])
     noise_model = sf.noise.IMUNoise(
         err_acc=sf.constants.ERR_ACC_MOTION2,
         err_gyro=sf.constants.ERR_GYRO_MOTION2,

From b4e9fb5a8fedb36fa66413aa4cc9b418f85e0015 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:08:01 +0200
Subject: [PATCH 163/217] test aind bench add gz bias

---
 tests/test_ins_/test_ains.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index b6aaf234..65b95ca7 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -301,7 +301,7 @@ def test_ains_benchmark(benchmark_gen, gyro_degrees):
     err_acc = sf.constants.ERR_ACC_MOTION2
     err_gyro = sf.constants.ERR_GYRO_MOTION2
     noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
-    bg = np.array([0.01, -0.02, 0.0])
+    bg = np.array([0.01, -0.02, 0.03])  # rad/s
     imu_noise = noise_model(fs_imu, len(t))
     acc_imu = acc_ref + imu_noise[:, :3]
     gyro_imu = gyro_ref + imu_noise[:, 3:] + bg

From 34591e058dceaee7e3b2117fd003c1f0d2f437a7 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:16:09 +0200
Subject: [PATCH 164/217] collect ains tests in class

---
 tests/test_ins_/test_ains.py | 432 +++++++++++++++++++----------------
 1 file changed, 233 insertions(+), 199 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 65b95ca7..7f704420 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -179,207 +179,241 @@ def test_reset():
     )
 
 
-def test_ains_init():
-
-    p0 = np.random.random(3)
-    v0 = np.random.random(3)
-    q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
-    bg0 = np.random.random(3)
-    vrw = 0.0001
-    arw = 0.0002
-    gbs = 0.0003
-    gbc = 123.0
-
-    mekf = AINS(
-        51.2,
-        p0=p0,
-        v0=v0,
-        q0=q0,
-        bg0=bg0,
-        P0=0.1 * np.eye(12),
-        acc_noise_density=vrw,
-        gyro_noise_density=arw,
-        gyro_bias_stability=gbs,
-        gyro_bias_corr_time=gbc,
-        g=9.81,
-        nav_frame="enu",
-        lever_arm=np.array([0.1, 0.2, 0.3]),
-    )
+class Test_AINS:
+
+    def test_init(self):
+
+        p0 = np.random.random(3)
+        v0 = np.random.random(3)
+        q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
+        bg0 = np.random.random(3)
+        vrw = 0.0001
+        arw = 0.0002
+        gbs = 0.0003
+        gbc = 123.0
+
+        mekf = AINS(
+            51.2,
+            p0=p0,
+            v0=v0,
+            q0=q0,
+            bg0=bg0,
+            P0=0.1 * np.eye(12),
+            acc_noise_density=vrw,
+            gyro_noise_density=arw,
+            gyro_bias_stability=gbs,
+            gyro_bias_corr_time=gbc,
+            g=9.81,
+            nav_frame="enu",
+            lever_arm=np.array([0.1, 0.2, 0.3]),
+        )
 
-    assert mekf._fs == pytest.approx(51.2)
-    assert mekf._dt == pytest.approx(1.0 / 51.2)
-    assert mekf._g == 9.81
-    assert mekf._nav_frame == "enu"
-    assert mekf._vrw == pytest.approx(vrw)
-    assert mekf._arw == pytest.approx(arw)
-    assert mekf._gbs == pytest.approx(gbs)
-    assert mekf._gbc == pytest.approx(gbc)
-    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, -9.81]))
-    np.testing.assert_allclose(mekf._lever_arm, np.array([0.1, 0.2, 0.3]))
-    np.testing.assert_allclose(mekf.position(), p0)
-    np.testing.assert_allclose(mekf.velocity(), v0)
-    np.testing.assert_allclose(mekf.quaternion(), q0)
-    np.testing.assert_allclose(mekf.bias_gyro(), bg0)
-    np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
-    np.testing.assert_allclose(mekf._dx, np.zeros(12))
-
-
-def test_ains_init_default():
-    mekf = AINS(10.0)
-    assert mekf._fs == pytest.approx(10.0)
-    assert mekf._dt == pytest.approx(0.1)
-    assert mekf._g == 9.80665
-    assert mekf._nav_frame == "ned"
-    assert mekf._vrw == pytest.approx(0.0007)
-    assert mekf._arw == pytest.approx(0.00005)
-    assert mekf._gbs == pytest.approx(0.00005)
-    assert mekf._gbc == pytest.approx(50.0)
-    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, 9.80665]))
-    np.testing.assert_allclose(mekf._lever_arm, np.zeros(3))
-    np.testing.assert_allclose(mekf.position(), np.zeros(3))
-    np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
-    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
-    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains.P0))
-    np.testing.assert_allclose(mekf._dx, np.zeros(12))
-
-
-@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-def test_ains_nav_frame(nav_frame, scale):
-    mekf = AINS(10.0, nav_frame=nav_frame)
-
-    assert mekf._nav_frame == nav_frame.lower()
-    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
-
-
-def test_ains_methods():
-    pos_init = np.array([0.1, 10.0, -0.2])
-    vel_init = np.array([0.0, 0.1, -0.2])
-    euler_init = np.array([10.0, 20.0, 30.0])
-    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
-    bg_init = np.array([0.01, -0.01, 0.02])
-
-    mekf = AINS(
-        10.0,
-        p0=pos_init,
-        v0=vel_init,
-        q0=quaternion_init,
-        bg0=bg_init,
-        P0=0.1 * np.eye(12),
-    )
+        assert mekf._fs == pytest.approx(51.2)
+        assert mekf._dt == pytest.approx(1.0 / 51.2)
+        assert mekf._g == 9.81
+        assert mekf._nav_frame == "enu"
+        assert mekf._vrw == pytest.approx(vrw)
+        assert mekf._arw == pytest.approx(arw)
+        assert mekf._gbs == pytest.approx(gbs)
+        assert mekf._gbc == pytest.approx(gbc)
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, -9.81]))
+        np.testing.assert_allclose(mekf._lever_arm, np.array([0.1, 0.2, 0.3]))
+        np.testing.assert_allclose(mekf.position(), p0)
+        np.testing.assert_allclose(mekf.velocity(), v0)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
+        np.testing.assert_allclose(mekf._dx, np.zeros(12))
+
+    def test_init_default(self):
+        mekf = AINS(10.0)
+        assert mekf._fs == pytest.approx(10.0)
+        assert mekf._dt == pytest.approx(0.1)
+        assert mekf._g == 9.80665
+        assert mekf._nav_frame == "ned"
+        assert mekf._vrw == pytest.approx(0.0007)
+        assert mekf._arw == pytest.approx(0.00005)
+        assert mekf._gbs == pytest.approx(0.00005)
+        assert mekf._gbc == pytest.approx(50.0)
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, 9.80665]))
+        np.testing.assert_allclose(mekf._lever_arm, np.zeros(3))
+        np.testing.assert_allclose(mekf.position(), np.zeros(3))
+        np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
+        np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+        np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains.P0))
+        np.testing.assert_allclose(mekf._dx, np.zeros(12))
+
+    @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+    def test_nav_frame(self, nav_frame, scale):
+        mekf = AINS(10.0, nav_frame=nav_frame)
+        assert mekf._nav_frame == nav_frame.lower()
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
+
+    def test_position(self):
+        p0 = np.array([1.0, 2.0, 3.0])
+        mekf = AINS(10.0, p0=p0)
+        np.testing.assert_allclose(mekf.position(), p0)
+
+    def test_velocity(self):
+        v0 = np.array([1.0, 2.0, 3.0])
+        mekf = AINS(10.0, v0=v0)
+        np.testing.assert_allclose(mekf.velocity(), v0)
+
+    def test_quaternion(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        q0 = sf.quaternion_from_euler(euler, degrees=True)
+        mekf = AINS(10.0, q0=q0)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+
+    def test_euler(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        mekf = AINS(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        np.testing.assert_allclose(mekf.euler(), np.radians(euler))
+        np.testing.assert_allclose(mekf.euler(degrees=True), euler)
+        np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
+
+    def test_bias_gyro(self):
+        bg0 = np.array([0.01, -0.02, 0.03])
+        mekf = AINS(10.0, bg0=bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
+
+    def test_P(self):
+        P0 = 0.1 * np.eye(12)
+        mekf = AINS(10.0, P0=P0)
+        np.testing.assert_allclose(mekf.P, P0)
+
+    def test_methods(self):
+        pos_init = np.array([0.1, 10.0, -0.2])
+        vel_init = np.array([0.0, 0.1, -0.2])
+        euler_init = np.array([10.0, 20.0, 30.0])
+        quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
+        bg_init = np.array([0.01, -0.01, 0.02])
+
+        mekf = AINS(
+            10.0,
+            p0=pos_init,
+            v0=vel_init,
+            q0=quaternion_init,
+            bg0=bg_init,
+            P0=0.1 * np.eye(12),
+        )
 
-    np.testing.assert_allclose(mekf.position(), pos_init)
-    np.testing.assert_allclose(mekf.velocity(), vel_init)
-    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
-    np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler_init))
-    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
-    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
-    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
-    np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg_init)
-    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
-    np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, gyro_degrees",
-    [
-        (benchmark_full_pva_beat_202311A, False),
-        (benchmark_full_pva_chirp_202311A, True),
-    ],
-)
-def test_ains_benchmark(benchmark_gen, gyro_degrees):
-    fs_imu = 10.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU and aiding measurements (with noise)
-    pos_std = 0.1  # m
-    vel_std = 0.01  # m/s
-    head_std = np.radians(0.1)  # rad
-    err_acc = sf.constants.ERR_ACC_MOTION2
-    err_gyro = sf.constants.ERR_GYRO_MOTION2
-    noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
-    bg = np.array([0.01, -0.02, 0.03])  # rad/s
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_imu = acc_ref + imu_noise[:, :3]
-    gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
-    pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
-    vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
-    head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
-
-    if gyro_degrees:
-        gyro_imu = np.degrees(gyro_imu)
-
-    # MEKF
-    mekf = AINS(
-        fs_imu,
-        p0=pos_ref[0],
-        v0=vel_ref[0],
-        q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
-        gyro_noise_density=err_gyro["N"],
-        gyro_bias_stability=err_gyro["B"],
-        gyro_bias_corr_time=err_gyro["tau_cb"],
+        np.testing.assert_allclose(mekf.position(), pos_init)
+        np.testing.assert_allclose(mekf.velocity(), vel_init)
+        np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+        np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler_init))
+        np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
+        np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg_init)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+        np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
     )
+    def test_benchmark(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        pos_std = 0.1  # m
+        vel_std = 0.01  # m/s
+        head_std = np.radians(0.1)  # rad
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.03])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_imu = acc_ref + imu_noise[:, :3]
+        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
+        vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
+        head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
+
+        if gyro_degrees:
+            gyro_imu = np.degrees(gyro_imu)
+
+        # MEKF
+        mekf = AINS(
+            fs_imu,
+            p0=pos_ref[0],
+            v0=vel_ref[0],
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
+        )
 
-    pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
-    for f_i, w_i, h_i, p_i, v_i in zip(
-        acc_imu, gyro_imu, head_meas, pos_meas, vel_meas
-    ):
-
-        dvel_i = f_i / fs_imu
-        dtheta_i = w_i / fs_imu
-
-        mekf.update(
-            dvel_i,
-            dtheta_i,
-            degrees=gyro_degrees,
-            head=h_i,
-            head_var=head_std**2,
-            head_degrees=False,
-            pos=p_i,
-            pos_var=pos_std**2 * np.ones(3),
-            vel=v_i,
-            vel_var=vel_std**2 * np.ones(3),
+        pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
+        for f_i, w_i, h_i, p_i, v_i in zip(
+            acc_imu, gyro_imu, head_meas, pos_meas, vel_meas
+        ):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(
+                dvel_i,
+                dtheta_i,
+                degrees=gyro_degrees,
+                head=h_i,
+                head_var=head_std**2,
+                head_degrees=False,
+                pos=p_i,
+                pos_var=pos_std**2 * np.ones(3),
+                vel=v_i,
+                vel_var=vel_std**2 * np.ones(3),
+            )
+            pos_est.append(mekf.position())
+            vel_est.append(mekf.velocity())
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        pos_est = np.array(pos_est)
+        vel_est = np.array(vel_est)
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        pos_est = resample_poly(pos_est, 2, 1)[1:-1:2]
+        vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        pos_ref = pos_ref[:-1, :]
+        vel_ref = vel_ref[:-1, :]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        px_rmse, py_rmse, pz_rmse = rmse(pos_ref[warmup:], pos_est[warmup:])
+        vx_rmse, vy_rmse, vz_rmse = rmse(vel_ref[warmup:], vel_est[warmup:])
+        roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, bgz_rmse = rmse(
+            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
         )
-        pos_est.append(mekf.position())
-        vel_est.append(mekf.velocity())
-        euler_est.append(mekf.euler(degrees=False))
-        bias_gyro_est.append(mekf.bias_gyro())
-
-    pos_est = np.array(pos_est)
-    vel_est = np.array(vel_est)
-    euler_est = np.array(euler_est)
-    bias_gyro_est = np.array(bias_gyro_est)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    pos_est = resample_poly(pos_est, 2, 1)[1:-1:2]
-    vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
-    euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
-    bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-    pos_ref = pos_ref[:-1, :]
-    vel_ref = vel_ref[:-1, :]
-    euler_ref = euler_ref[:-1, :]
-    bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
-
-    def rmse(ref, est):
-        return np.sqrt(np.mean((ref - est) ** 2, axis=0))
-
-    px_rmse, py_rmse, pz_rmse = rmse(pos_ref[warmup:], pos_est[warmup:])
-    vx_rmse, vy_rmse, vz_rmse = rmse(vel_ref[warmup:], vel_est[warmup:])
-    roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
-    bgx_rmse, bgy_rmse, bgz_rmse = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
-
-    assert px_rmse <= 0.1
-    assert py_rmse <= 0.1
-    assert pz_rmse <= 0.1
-    assert vx_rmse <= 0.1
-    assert vy_rmse <= 0.1
-    assert vz_rmse <= 0.1
-    assert np.degrees(roll_rmse) <= 0.5
-    assert np.degrees(pitch_rmse) <= 0.5
-    assert np.degrees(yaw_rmse) <= 0.5
-    assert np.degrees(bgx_rmse) <= 0.01
-    assert np.degrees(bgy_rmse) <= 0.01
-    assert np.degrees(bgz_rmse) <= 0.01
+
+        assert px_rmse <= 0.1
+        assert py_rmse <= 0.1
+        assert pz_rmse <= 0.1
+        assert vx_rmse <= 0.1
+        assert vy_rmse <= 0.1
+        assert vz_rmse <= 0.1
+        assert np.degrees(roll_rmse) <= 0.5
+        assert np.degrees(pitch_rmse) <= 0.5
+        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01
+        assert np.degrees(bgz_rmse) <= 0.01

From c1dfadfb31c6a94be41a7c8f089bcafe6ff9f072 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:16:36 +0200
Subject: [PATCH 165/217] delete old obsolete test

---
 tests/test_ins_/test_ains.py | 27 ---------------------------
 1 file changed, 27 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 7f704420..446519a7 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -285,33 +285,6 @@ def test_P(self):
         mekf = AINS(10.0, P0=P0)
         np.testing.assert_allclose(mekf.P, P0)
 
-    def test_methods(self):
-        pos_init = np.array([0.1, 10.0, -0.2])
-        vel_init = np.array([0.0, 0.1, -0.2])
-        euler_init = np.array([10.0, 20.0, 30.0])
-        quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
-        bg_init = np.array([0.01, -0.01, 0.02])
-
-        mekf = AINS(
-            10.0,
-            p0=pos_init,
-            v0=vel_init,
-            q0=quaternion_init,
-            bg0=bg_init,
-            P0=0.1 * np.eye(12),
-        )
-
-        np.testing.assert_allclose(mekf.position(), pos_init)
-        np.testing.assert_allclose(mekf.velocity(), vel_init)
-        np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
-        np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler_init))
-        np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
-        np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
-        np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
-        np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg_init)
-        np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
-        np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
-
     @pytest.mark.parametrize(
         "benchmark_gen, gyro_degrees",
         [

From bdb92380d82f5faaf80a3baea05c03b648fa5d9d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:22:25 +0200
Subject: [PATCH 166/217] check that initial values are copied

---
 tests/test_ins_/test_ains.py | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 446519a7..938ddb28 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -187,6 +187,7 @@ def test_init(self):
         v0 = np.random.random(3)
         q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
         bg0 = np.random.random(3)
+        P0 = 0.1 * np.eye(12)
         vrw = 0.0001
         arw = 0.0002
         gbs = 0.0003
@@ -198,7 +199,7 @@ def test_init(self):
             v0=v0,
             q0=q0,
             bg0=bg0,
-            P0=0.1 * np.eye(12),
+            P0=P0,
             acc_noise_density=vrw,
             gyro_noise_density=arw,
             gyro_bias_stability=gbs,
@@ -222,9 +223,16 @@ def test_init(self):
         np.testing.assert_allclose(mekf.velocity(), v0)
         np.testing.assert_allclose(mekf.quaternion(), q0)
         np.testing.assert_allclose(mekf.bias_gyro(), bg0)
-        np.testing.assert_allclose(mekf.P, 0.1 * np.eye(12))
+        np.testing.assert_allclose(mekf.P, P0)
         np.testing.assert_allclose(mekf._dx, np.zeros(12))
 
+        # Check that the initial values are copied
+        assert mekf._p_n is not p0  # copy
+        assert mekf._v_n is not v0  # copy
+        assert mekf._q_nb is not q0  # copy
+        assert mekf._bg_b is not bg0  # copy
+        assert mekf._P is not P0  # copy
+
     def test_init_default(self):
         mekf = AINS(10.0)
         assert mekf._fs == pytest.approx(10.0)
@@ -254,17 +262,20 @@ def test_position(self):
         p0 = np.array([1.0, 2.0, 3.0])
         mekf = AINS(10.0, p0=p0)
         np.testing.assert_allclose(mekf.position(), p0)
+        assert mekf.position() is not mekf._p_n  # copy
 
     def test_velocity(self):
         v0 = np.array([1.0, 2.0, 3.0])
         mekf = AINS(10.0, v0=v0)
         np.testing.assert_allclose(mekf.velocity(), v0)
+        assert mekf.velocity() is not mekf._v_n  # copy
 
     def test_quaternion(self):
         euler = np.array([10.0, 20.0, 30.0])
         q0 = sf.quaternion_from_euler(euler, degrees=True)
         mekf = AINS(10.0, q0=q0)
         np.testing.assert_allclose(mekf.quaternion(), q0)
+        assert mekf.quaternion() is not mekf._q_nb  # copy
 
     def test_euler(self):
         euler = np.array([10.0, 20.0, 30.0])
@@ -279,11 +290,13 @@ def test_bias_gyro(self):
         np.testing.assert_allclose(mekf.bias_gyro(), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
+        assert mekf.bias_gyro(degrees=False) is not mekf._bg_b  # copy
 
     def test_P(self):
         P0 = 0.1 * np.eye(12)
         mekf = AINS(10.0, P0=P0)
         np.testing.assert_allclose(mekf.P, P0)
+        assert mekf.P is not mekf._P  # copy
 
     @pytest.mark.parametrize(
         "benchmark_gen, gyro_degrees",

From 1f71962e362c6e98bbcc106c462570ea0f6f63bb Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 11:29:18 +0200
Subject: [PATCH 167/217] private ains P0

---
 src/smsfusion/_ins/_ains_.py | 4 ++--
 tests/test_ins_/test_ains.py | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 2eda544a..fb4ed2ca 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -15,7 +15,7 @@
     _update_quaternion_with_rotvec,
 )
 
-P0 = (
+_P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
@@ -287,7 +287,7 @@ def __init__(
         v0: ArrayLike = (0.0, 0.0, 0.0),
         q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg0: ArrayLike = (0.0, 0.0, 0.0),
-        P0: ArrayLike = P0,
+        P0: ArrayLike = _P0,
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 938ddb28..4b442aef 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -249,7 +249,7 @@ def test_init_default(self):
         np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
         np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
         np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains.P0))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains_._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(12))
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))

From 43ce94de4b539211ebe2fec7fcb46fe3614c6c8f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 12:52:35 +0200
Subject: [PATCH 168/217] add gref aiding to ains

---
 src/smsfusion/_ins/_ains_.py | 52 +++++++++++++++++++---
 tests/test_ins_/test_ains.py | 86 +++++++++++++++++++++++++++++++++++-
 2 files changed, 130 insertions(+), 8 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index fb4ed2ca..b8a2d9ba 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -7,9 +7,16 @@
 from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 
-from ._aiding import _aiding_update_head, _aiding_update_pos, _aiding_update_vel
+from ._aiding import (
+    _aiding_update_gref,
+    _aiding_update_head,
+    _aiding_update_pos,
+    _aiding_update_vel,
+)
 from ._common import (
     _dhda_head,
+    _gref_b_from_quat,
+    _nz2vg,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
@@ -148,7 +155,7 @@ def _process_noise_covariance_matrix(
 
 
 def _measurement_matrix_init(
-    q_nb: NDArray[np.float64], lever_arm: NDArray[np.float64]
+    q_nb: NDArray[np.float64], lever_arm: NDArray[np.float64], nav_frame_factor: float
 ) -> NDArray[np.float64]:
     """
     Measurement matrix.
@@ -161,17 +168,22 @@ def _measurement_matrix_init(
         Lever-arm vector describing the location of position aiding (in meters) relative
         to the IMU expressed in the IMU/body reference frame. For instance, the location
         of the GNSS antenna relative to the IMU.
+    nav_frame_factor: float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
 
     Returns
     -------
     ndarray, shape (7, 12)
         Linearized measurement matrix.
     """
-    H = np.zeros((7, 12))
+    vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
+    H = np.zeros((10, 12))
     H[0:3, 0:3] = np.eye(3)  # position
     H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     H[3:6, 3:6] = np.eye(3)  # velocity
     H[6:7, 6:9] = _dhda_head(q_nb)  # heading
+    H[7:10, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
     return H
 
 
@@ -298,8 +310,9 @@ def __init__(
     ) -> None:
         self._fs = fs
         self._dt = 1.0 / fs
-        self._g = g
         self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+        self._g = g
         self._g_n = _gravity_nav(self._g, self._nav_frame)
         self._dvel_g_corr = self._dt * self._g_n
         self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy()
@@ -329,7 +342,7 @@ def __init__(
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
         )
-        self._H = _measurement_matrix_init(self._q_nb, self._lever_arm)
+        self._H = _measurement_matrix_init(self._q_nb, self._lever_arm, self._nz2vg)
 
     def position(self) -> NDArray[np.float64]:
         """
@@ -406,6 +419,8 @@ def update(
         head: float | None = None,
         head_var: float | None = None,
         head_degrees: bool = False,
+        gref: bool = False,
+        gref_var: ArrayLike | None = None,
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -443,6 +458,12 @@ def update(
         head_degrees : bool, default False
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees
             and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
+        gref : bool, optional
+            Specifies whether to use accelerometer measurements (dvel) and the known
+            direction of gravity as aiding. Defaults to ``False``.
+        gref_var : array_like, shape (3,), optional
+            Variance of gravity reference vector measurement noise (dimensionless).
+            Required for gravity reference vector aiding.
 
         Returns
         -------
@@ -470,7 +491,7 @@ def update(
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        # Update (a posteriori) state and covariance estimates with aiding measurements
+        # Update (a posteriori) estimates with position aiding
         if pos is not None:
             if pos_var is None:
                 raise ValueError("'pos_var' is required for position aiding.")
@@ -486,6 +507,7 @@ def update(
                 self._lever_arm,
             )
 
+        # Update (a posteriori) estimates with velocity aiding
         if vel is not None:
             if vel_var is None:
                 raise ValueError("'vel_var' is required for velocity aiding.")
@@ -499,6 +521,7 @@ def update(
                 np.asarray(vel_var),
             )
 
+        # Update (a posteriori) estimates with heading aiding
         if head is not None:
             if head_var is None:
                 raise ValueError("'head_var' is required for heading aiding.")
@@ -515,6 +538,23 @@ def update(
                 head_degrees,
             )
 
+        # Update (a posteriori) estimates with gravity reference vector aiding
+        if gref is True:
+            if gref_var is None:
+                raise ValueError("'gref_var' is required for gravity reference aiding.")
+
+            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
+            self._H[7:10, 6:9] = _skew_symmetric(vg_b)
+
+            _aiding_update_gref(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[7:10],
+                vg_b,
+                dvel,
+                np.asarray(gref_var),
+            )
+
         # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 4b442aef..38580e12 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -11,6 +11,7 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
+from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._transforms import _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
@@ -85,14 +86,18 @@ def test_state_transition_matrix_update():
 def test_measurement_matrix_init():
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
     lever_arm = np.array([2.0, 3.0, 4.0])
+    nz2vg = 1.0
 
-    expect = np.zeros((7, 12))
+    expect = np.zeros((10, 12))
     expect[0:3, 0:3] = np.eye(3)
     expect[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     expect[3:6, 3:6] = np.eye(3)
     expect[6, 6:9] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
+    expect[7:10, 6:9] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
 
-    np.testing.assert_array_equal(_measurement_matrix_init(q_nb, lever_arm), expect)
+    np.testing.assert_array_equal(
+        _measurement_matrix_init(q_nb, lever_arm, nz2vg), expect
+    )
 
 
 def test_process_noise_covariance_matrix():
@@ -403,3 +408,80 @@ def rmse(ref, est):
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
+    )
+    def test_benchmark_no_aiding(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.0])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_imu = acc_ref + imu_noise[:, :3]
+        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+
+        if gyro_degrees:
+            gyro_imu = np.degrees(gyro_imu)
+
+        # MEKF
+        mekf = AINS(
+            fs_imu,
+            p0=pos_ref[0],
+            v0=vel_ref[0],
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
+        )
+
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i in zip(acc_imu, gyro_imu):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(
+                dvel_i,
+                dtheta_i,
+                degrees=gyro_degrees,
+                pos=np.zeros(3),
+                pos_var=1_000_000.0 * np.ones(3),
+                vel=np.zeros(3),
+                vel_var=100.0 * np.ones(3),
+                gref=True,
+                gref_var=(0.001, 0.001, 0.001),
+            )
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
+
+        assert np.degrees(roll_rmse) <= 0.5
+        assert np.degrees(pitch_rmse) <= 0.5
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01

From ea1cb6a69108fba1a477722f744234a9cd1c878e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 13:05:36 +0200
Subject: [PATCH 169/217] improve position integration

---
 src/smsfusion/_ins/_ains_.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index b8a2d9ba..7e256748 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -484,8 +484,9 @@ def update(
         _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)  # -> update phi
 
         # Project (a priori) state estimates ahead
-        self._p_n[:] += self._dt * self._v_n
-        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr
+        dv_n_corr = R_nb @ dvel + self._dvel_g_corr  # corrected velocity increment
+        self._p_n[:] += self._dt * self._v_n + 0.5 * self._dt * dv_n_corr
+        self._v_n[:] += dv_n_corr
         _update_quaternion_with_rotvec(self._q_nb, dtheta)  # -> update q_nb (in place)
 
         # Project (a priori) error covariance matrix estimate ahead

From 7ac63893ce03981aacde41c94ba51d66ab7e2d3f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 13:19:01 +0200
Subject: [PATCH 170/217] njit project state ahead

---
 src/smsfusion/_ins/_ains_.py  | 38 +++++++++++++++++++++++++++++++----
 src/smsfusion/_ins/_common.py |  2 +-
 2 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 7e256748..241dcf01 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -244,6 +244,30 @@ def _reset(
     dx[:] = 0.0
 
 
+@njit  # type: ignore[misc]
+def _project_state_ahead(
+    p_n: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    dt: float,
+    dvel_g_corr: NDArray[np.float64],
+) -> None:
+    """
+    Project state estimates ahead (in place).
+
+    References
+    ----------
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 3-5)
+    """
+    dvel_corr = R_nb @ dvel + dvel_g_corr
+    p_n[:] += dt * v_n + 0.5 * dt * dvel_corr
+    v_n[:] += dvel_corr
+    _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
+
+
 class AINS:
     """
     Aided inertial navigation system (AINS).
@@ -484,10 +508,16 @@ def update(
         _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)  # -> update phi
 
         # Project (a priori) state estimates ahead
-        dv_n_corr = R_nb @ dvel + self._dvel_g_corr  # corrected velocity increment
-        self._p_n[:] += self._dt * self._v_n + 0.5 * self._dt * dv_n_corr
-        self._v_n[:] += dv_n_corr
-        _update_quaternion_with_rotvec(self._q_nb, dtheta)  # -> update q_nb (in place)
+        _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._dt,
+            self._dvel_g_corr,
+        )
 
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index dbf1aa22..eba5c539 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -111,7 +111,7 @@ def _update_quaternion_with_rotvec(
 
     References
     ----------
-    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1)
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 1)
     """
 
     qw, qx, qy, qz = q

From b9aea7ed209f751e1cfb5881490fc662d9d03195 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:00:27 +0200
Subject: [PATCH 171/217] refactor AHRS and update tests

---
 src/smsfusion/_ins/_ahrs.py  | 201 ++++++++++-----
 tests/test_ins_/test_ahrs.py | 472 +++++++++++++++++++++--------------
 tests/test_ins_/test_ains.py |   4 +-
 3 files changed, 416 insertions(+), 261 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 41b359f7..a3036625 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -7,15 +7,17 @@
 from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 
-from ._aiding import _aiding_update_head, _aiding_update_vel
+from ._aiding import _aiding_update_gref, _aiding_update_head, _aiding_update_vel
 from ._common import (
     _dhda_head,
+    _gref_b_from_quat,
+    _nz2vg,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
 )
 
-P0 = (
+_P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
@@ -144,7 +146,9 @@ def _process_noise_covariance_matrix(
     return Q
 
 
-def _measurement_matrix_init(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
+def _measurement_matrix_init(
+    q_nb: NDArray[np.float64], nav_frame_factor: float
+) -> NDArray[np.float64]:
     """
     Measurement matrix.
 
@@ -152,16 +156,21 @@ def _measurement_matrix_init(q_nb: NDArray[np.float64]) -> NDArray[np.float64]:
     ----------
     q_nb : ndarray, shape (4,)
         Unit quaternion.
+    nav_frame_factor: float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
 
     Returns
     -------
-    ndarray, shape (4, 9)
+    ndarray, shape (7, 9)
         Linearized measurement matrix.
     """
-    dhdx = np.zeros((4, 9))
-    dhdx[0:3, 0:3] = np.eye(3)  # velocity
-    dhdx[3:4, 3:6] = _dhda_head(q_nb)  # heading
-    return dhdx
+    vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
+    H = np.zeros((7, 9))
+    H[0:3, 0:3] = np.eye(3)  # velocity
+    H[3:4, 3:6] = _dhda_head(q_nb)  # heading
+    H[4:7, 3:6] = _skew_symmetric(vg_b)  # gravity reference vector
+    return H
 
 
 def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
@@ -195,11 +204,9 @@ def _reset(
     v_n: NDArray[np.float64],
     q_nb: NDArray[np.float64],
     bg_b: NDArray[np.float64],
-) -> tuple[
-    NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]
-]:
+) -> None:
     """
-    Reset state.
+    Reset state (in place).
 
     Parameters
     ----------
@@ -214,29 +221,51 @@ def _reset(
         estimates. Will be reset to zero after applying the corrections.
     """
     v_n[:] += dx[0:3]
-    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[3:6])
+    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
     bg_b[:] += dx[6:9]
     dx[:] = 0.0
-    return dx, v_n, q_nb, bg_b
+
+
+@njit  # type: ignore[misc]
+def _project_state_ahead(
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    dvel_g_corr: NDArray[np.float64],
+) -> None:
+    """
+    Project state estimates ahead (in place).
+
+    References
+    ----------
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 3-5)
+    """
+    dvel_corr = R_nb @ dvel + dvel_g_corr
+    v_n[:] += dvel_corr
+    _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
 
 
 class AHRS:
     """
-    Attitude and Heading Reference System (AHRS) using a multiplicative extended
-    Kalman filter (MEKF).
+    Attitude and Heading Reference System (AHRS).
+
+    This class provides velocity, attitude and gyro bias estimation using a multiplicative
+    extended Kalman filter (MEKF).
 
     Parameters
     ----------
     fs : float
         Sampling rate in Hz.
-    vel : array_like, shape (3,), optional
+    v0 : array_like, shape (3,), optional
         Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
-    q : Attitude or array_like, shape (4,), optional
+    q0 : array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg : array_like, shape (3,), optional
+    bg0 : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P : array_like, shape (6, 6), optional
+    P0 : array_like, shape (9, 9), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
         a small diagonal matrix (1e-6 * np.eye(9)).
     acc_noise_density : float, optional
@@ -257,16 +286,15 @@ class AHRS:
         Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
         (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
         will be expressed relative to this frame.
-
     """
 
     def __init__(
         self,
         fs: float,
-        vel: ArrayLike = (0.0, 0.0, 0.0),
-        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = P0,
+        v0: ArrayLike = (0.0, 0.0, 0.0),
+        q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg0: ArrayLike = (0.0, 0.0, 0.0),
+        P0: ArrayLike = _P0,
         acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
@@ -276,8 +304,9 @@ def __init__(
     ) -> None:
         self._fs = fs
         self._dt = 1.0 / fs
-        self._g = g
         self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+        self._g = g
         self._g_n = _gravity_nav(self._g, self._nav_frame)
         self._dvel_g_corr = self._dt * self._g_n
 
@@ -288,10 +317,10 @@ def __init__(
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._v_n = np.asarray_chkfinite(vel).reshape(3).copy()
-        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
+        self._v_n = np.asarray_chkfinite(v0).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q0).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg0).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P0).reshape(9, 9).copy()
         self._dx = np.zeros(9)
 
         # Discrete state-space model
@@ -305,28 +334,29 @@ def __init__(
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._vrw, self._arw, self._gbs, self._gbc
         )
-        self._H = _measurement_matrix_init(self._q_nb)
+        self._H = _measurement_matrix_init(self._q_nb, self._nz2vg)
 
     def velocity(self) -> NDArray[np.float64]:
         """
-        Velocity expressed in the navigation frame.
+        Copy of the velocity estimate in m/s.
         """
         return self._v_n.copy()
 
     def quaternion(self) -> NDArray[np.float64]:
         """
-        Attitude expressed as a unit quaternion.
+        Copy of the attitude estimate expressed as a unit quaternion.
         """
         return self._q_nb.copy()
 
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
+        Copy of the attitude estimate expressed as Euler angles (roll, pitch, yaw).
 
         Parameters
         ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
+        degrees : bool, optional
+            Whether to return the Euler angles in degrees or radians. Defaults to
+            radians.
 
         Returns
         -------
@@ -343,7 +373,8 @@ def euler(self, degrees: bool = False) -> NDArray[np.float64]:
 
     def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
         """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
+        Copy of the gyroscope bias estimate in rad/s or deg/s depending on the
+        ``degrees`` flag.
 
         Parameters
         ----------
@@ -367,42 +398,54 @@ def update(
         dvel: ArrayLike,
         dtheta: ArrayLike,
         degrees: bool = False,
-        vel: ArrayLike | None = (0.0, 0.0, 0.0),
-        vel_var: ArrayLike = (100.0, 100.0, 100.0),
+        vel: ArrayLike | None = None,
+        vel_var: ArrayLike | None = None,
         head: float | None = None,
-        head_var: float = 0.001,
+        head_var: float | None = None,
         head_degrees: bool = False,
+        gref: bool = False,
+        gref_var: ArrayLike | None = None,
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
 
         Parameters
         ----------
-        dvel : array_like, shape (3,), optional
+        dvel : array_like, shape (3,)
             Velocity increment (sculling integral) in m/s.
-        dtheta : array_like, shape (3,), optional
-            Attitude increment (coning integral) in radians.
+        dtheta : array_like, shape (3,)
+            Attitude increment (coning integral) in radians or degrees depending
+            on the ``degrees`` flag.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
         vel : array-like, shape (3,), optional
-            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is
+            not used.
         vel_var : array-like, shape (3,), optional
-            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel`` is ``None``.
+            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
+            is ``None``.
         head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+            Heading measurement in radians or degrees depending on the ``head_degrees``
+            flag. I.e., the yaw angle of the 'body' frame relative to the assumed
+            'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used.
         head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Ignored if ``head`` is ``None``.
+            Variance of heading measurement noise in radians^2 or degrees^2 depending
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
         head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees
+            and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
+        gref : bool, optional
+            Specifies whether to use accelerometer measurements (dvel) and the known
+            direction of gravity as aiding. Defaults to ``False``.
+        gref_var : array_like, shape (3,), optional
+            Variance of gravity reference vector measurement noise (dimensionless).
+            Required for gravity reference vector aiding.
 
         Returns
         -------
-        AHRS
+        AINS
             A reference to the instance itself after the update.
         """
 
@@ -416,18 +459,27 @@ def update(
 
         # Update state-space model
         R_nb = _rot_matrix_from_quaternion(self._q_nb)
-        self._phi = _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)
+        _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb)  # -> update phi
 
         # Project (a priori) state estimates ahead
-        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr
-        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+        _project_state_ahead(  # -> update v_n, q_nb (in place)
+            self._v_n,
+            self._q_nb,
+            R_nb,
+            dvel,
+            dtheta,
+            self._dvel_g_corr,
+        )
 
         # Project (a priori) error covariance matrix estimate ahead
-        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
+        _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        # Update (a posteriori) state and covariance estimates with aiding measurements
+        # Update (a posteriori) estimates with velocity aiding
         if vel is not None:
-            self._dx, self._P = _aiding_update_vel(
+            if vel_var is None:
+                raise ValueError("'vel_var' is required for velocity aiding.")
+
+            _aiding_update_vel(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
                 self._H[0:3],
@@ -436,10 +488,14 @@ def update(
                 np.asarray(vel_var),
             )
 
+        # Update (a posteriori) estimates with heading aiding
         if head is not None:
-            self._H[3, 3:6] = _dhda_head(self._q_nb)  # Update measurement matrix
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            self._H[3, 6:9] = _dhda_head(self._q_nb)
 
-            self._dx, self._P = _aiding_update_head(
+            _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
                 self._H[3],
@@ -449,9 +505,24 @@ def update(
                 head_degrees,
             )
 
-        # Reset state
-        self._dx, self._v_n, self._q_nb, self._bg_b = _reset(
-            self._dx, self._v_n, self._q_nb, self._bg_b
-        )
+        # Update (a posteriori) estimates with gravity reference vector aiding
+        if gref is True:
+            if gref_var is None:
+                raise ValueError("'gref_var' is required for gravity reference aiding.")
+
+            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
+            self._H[4:7, 3:6] = _skew_symmetric(vg_b)
+
+            _aiding_update_gref(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[4:7],
+                vg_b,
+                dvel,
+                np.asarray(gref_var),
+            )
+
+        # Reset state -> update v_n, q_nb, bg_b and dx (in place)
+        _reset(self._dx, self._v_n, self._q_nb, self._bg_b)
 
         return self
diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index 20646284..d5f8bc1d 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -11,9 +11,12 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
+from smsfusion._ins._common import _gref_b_from_quat
+from smsfusion._transforms import _rot_matrix_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
-    benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A,
+    benchmark_full_pva_beat_202311A,
+    benchmark_full_pva_chirp_202311A,
 )
 
 
@@ -49,13 +52,11 @@ def test_state_transition_matrix_update():
     R_nb = np.eye(3)
     gbc = 0.01
 
-    phi_init = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
+    phi = _state_transition_matrix_init(dt, dvel, dtheta, R_nb, gbc)
 
     dtheta_update = np.ones(3) * 0.01
     dvel_update = np.ones(3) * 0.1
-    phi_out = _state_transition_matrix_update(
-        phi_init, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
-    )
+    _state_transition_matrix_update(phi, dvel_update, dtheta_update, R_nb)
 
     phi_expected = np.array(
         [
@@ -71,18 +72,19 @@ def test_state_transition_matrix_update():
         ]
     )
 
-    np.testing.assert_almost_equal(phi_out, phi_expected)
+    np.testing.assert_almost_equal(phi, phi_expected)
 
 
 def test_measurement_matrix_init():
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    nz2vg = 1.0
 
-    expect = np.zeros((4, 9))
+    expect = np.zeros((7, 9))
     expect[0:3, 0:3] = np.eye(3)
-    expect[3, 3:6] = np.array([0.0, 0.0, 1.0])
-    # kappa -> zero due to unit quat
+    expect[3:4, 3:6] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
+    expect[4:7, 3:6] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
 
-    np.testing.assert_array_equal(_measurement_matrix_init(q_nb), expect)
+    np.testing.assert_array_equal(_measurement_matrix_init(q_nb, nz2vg), expect)
 
 
 def test_process_noise_covariance_matrix():
@@ -110,204 +112,288 @@ def test_process_noise_covariance_matrix():
 
 
 def test_reset():
-    v_n = np.array([0.0, 0.1, 0.0])
+    v_n = np.array([0.0, 2.0, 0.0])
     q_nb = np.array([1.0, 0.0, 0.0, 0.0])
     bg_b = np.zeros(3)
-    dx = np.array([0.1, 0.0, 0.0, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+    dx = np.array([0.4, 0.5, 0.6, 0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
 
-    dx, v_n, q_nb, bg_b = _reset(dx, v_n, q_nb, bg_b)
+    _reset(dx, v_n, q_nb, bg_b)
 
     np.testing.assert_allclose(dx, np.zeros_like(dx))
-    np.testing.assert_allclose(v_n, np.array([0.1, 0.1, 0.0]))
+    np.testing.assert_allclose(v_n, np.array([0.4, 2.5, 0.6]))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
     np.testing.assert_allclose(
         q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
     )
 
 
-def test_ahrs_init():
-    mekf = AHRS(10.0)
-    np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
-    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
-    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-    np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs.P0))
-    assert mekf._g == 9.80665
-    assert mekf._nav_frame == "ned"
-
-
-@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-def test_ahrs_nav_frame(nav_frame, scale):
-    mekf = AHRS(10.0, nav_frame=nav_frame)
-
-    assert mekf._nav_frame == nav_frame.lower()
-    np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
-
-
-def test_ahrs_methods():
-    vel_init = np.array([0.0, 0.1, -0.2])
-    euler_init = np.array([10.0, 20.0, 30.0])
-    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
-    bg_init = np.array([0.01, -0.01, 0.02])
-
-    mekf = AHRS(10.0, vel=vel_init, q=quaternion_init, bg=bg_init)
-
-    np.testing.assert_allclose(mekf.velocity(), vel_init)
-    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
-    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
-    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
-    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
-    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, degrees",
-    [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
-    ],
-)
-def test_ahrs_no_head_aiding_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-    if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-
-    # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = AHRS(
-        fs_imu,
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-    )
-
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
-
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
-
-        mekf.update(
-            dvel,
-            dtheta,
-            degrees=degrees,
+class Test_AHRS:
+
+    def test_init(self):
+
+        v0 = np.random.random(3)
+        q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
+        bg0 = np.random.random(3)
+        P0 = 0.1 * np.eye(9)
+        vrw = 0.0001
+        arw = 0.0002
+        gbs = 0.0003
+        gbc = 123.0
+
+        mekf = AHRS(
+            51.2,
+            v0=v0,
+            q0=q0,
+            bg0=bg0,
+            P0=P0,
+            acc_noise_density=vrw,
+            gyro_noise_density=arw,
+            gyro_bias_stability=gbs,
+            gyro_bias_corr_time=gbc,
+            g=9.81,
+            nav_frame="enu",
         )
 
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
-
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
-    )
-
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, degrees",
-    [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
-    ],
-)
-def test_ahrs_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-    head_std = np.radians(1.0)
-    head_noise = euler_ref[:, -1] + np.random.normal(0.0, head_std, len(euler_ref))
-
-    if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-        head_noise = np.degrees(head_noise)
-        head_std = np.degrees(head_std)
-
-    # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = AHRS(
-        fs_imu,
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+        assert mekf._fs == pytest.approx(51.2)
+        assert mekf._dt == pytest.approx(1.0 / 51.2)
+        assert mekf._g == 9.81
+        assert mekf._nav_frame == "enu"
+        assert mekf._vrw == pytest.approx(vrw)
+        assert mekf._arw == pytest.approx(arw)
+        assert mekf._gbs == pytest.approx(gbs)
+        assert mekf._gbc == pytest.approx(gbc)
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, -9.81]))
+        np.testing.assert_allclose(mekf.velocity(), v0)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.P, P0)
+        np.testing.assert_allclose(mekf._dx, np.zeros(9))
+
+        # Check that the initial values are copied
+        assert mekf._v_n is not v0  # copy
+        assert mekf._q_nb is not q0  # copy
+        assert mekf._bg_b is not bg0  # copy
+        assert mekf._P is not P0  # copy
+
+    def test_init_default(self):
+        mekf = AHRS(10.0)
+        assert mekf._fs == pytest.approx(10.0)
+        assert mekf._dt == pytest.approx(0.1)
+        assert mekf._g == 9.80665
+        assert mekf._nav_frame == "ned"
+        assert mekf._vrw == pytest.approx(0.0007)
+        assert mekf._arw == pytest.approx(0.00005)
+        assert mekf._gbs == pytest.approx(0.00005)
+        assert mekf._gbc == pytest.approx(50.0)
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, 9.80665]))
+        np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
+        np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+        np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs._P0))
+        np.testing.assert_allclose(mekf._dx, np.zeros(9))
+
+    @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+    def test_nav_frame(self, nav_frame, scale):
+        mekf = AHRS(10.0, nav_frame=nav_frame)
+        assert mekf._nav_frame == nav_frame.lower()
+        np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
+
+    def test_velocity(self):
+        v0 = np.array([1.0, 2.0, 3.0])
+        mekf = AHRS(10.0, v0=v0)
+        np.testing.assert_allclose(mekf.velocity(), v0)
+        assert mekf.velocity() is not mekf._v_n  # copy
+
+    def test_quaternion(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        q0 = sf.quaternion_from_euler(euler, degrees=True)
+        mekf = AHRS(10.0, q0=q0)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+        assert mekf.quaternion() is not mekf._q_nb  # copy
+
+    def test_euler(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        mekf = AHRS(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        np.testing.assert_allclose(mekf.euler(), np.radians(euler))
+        np.testing.assert_allclose(mekf.euler(degrees=True), euler)
+        np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
+
+    def test_bias_gyro(self):
+        bg0 = np.array([0.01, -0.02, 0.03])
+        mekf = AHRS(10.0, bg0=bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
+        assert mekf.bias_gyro(degrees=False) is not mekf._bg_b  # copy
+
+    def test_P(self):
+        P0 = 0.1 * np.eye(9)
+        mekf = AHRS(10.0, P0=P0)
+        np.testing.assert_allclose(mekf.P, P0)
+        assert mekf.P is not mekf._P  # copy
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
     )
-
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i, head_i) in enumerate(zip(acc_noise, gyro_noise, head_noise)):
-
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
-
-        mekf.update(
-            dvel,
-            dtheta,
-            degrees=degrees,
-            head=head_i,
-            head_degrees=degrees,
-            head_var=head_std**2,
+    def test_benchmark(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        pos_std = 0.1  # m
+        vel_std = 0.01  # m/s
+        head_std = np.radians(0.1)  # rad
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.03])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_imu = acc_ref + imu_noise[:, :3]
+        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
+        head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
+
+        if gyro_degrees:
+            gyro_imu = np.degrees(gyro_imu)
+
+        # MEKF
+        mekf = AHRS(
+            fs_imu,
+            v0=vel_ref[0],
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
         )
 
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
+        pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
+        for f_i, w_i, h_i, v_i in zip(acc_imu, gyro_imu, head_meas, vel_meas):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(
+                dvel_i,
+                dtheta_i,
+                degrees=gyro_degrees,
+                head=h_i,
+                head_var=head_std**2,
+                head_degrees=False,
+                vel=v_i,
+                vel_var=vel_std**2 * np.ones(3),
+            )
+            vel_est.append(mekf.velocity())
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        vel_est = np.array(vel_est)
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        vel_ref = vel_ref[:-1, :]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        vx_rmse, vy_rmse, vz_rmse = rmse(vel_ref[warmup:], vel_est[warmup:])
+        roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, bgz_rmse = rmse(
+            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
+        )
 
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
+        assert vx_rmse <= 0.1
+        assert vy_rmse <= 0.1
+        assert vz_rmse <= 0.1
+        assert np.degrees(roll_rmse) <= 0.5
+        assert np.degrees(pitch_rmse) <= 0.5
+        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01
+        assert np.degrees(bgz_rmse) <= 0.01
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
     )
+    def test_benchmark_no_aiding(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.0])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_imu = acc_ref + imu_noise[:, :3]
+        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+
+        if gyro_degrees:
+            gyro_imu = np.degrees(gyro_imu)
+
+        # MEKF
+        mekf = AHRS(
+            fs_imu,
+            v0=vel_ref[0],
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
+        )
 
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(yaw_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005
-    assert np.degrees(bias_gyro_z_rms) <= 0.005
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i in zip(acc_imu, gyro_imu):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(
+                dvel_i,
+                dtheta_i,
+                degrees=gyro_degrees,
+                vel=np.zeros(3),
+                vel_var=100.0 * np.ones(3),
+                gref=True,
+                gref_var=(0.001, 0.001, 0.001),
+            )
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
+
+        assert np.degrees(roll_rmse) <= 0.5
+        assert np.degrees(pitch_rmse) <= 0.5
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 38580e12..97a24f95 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -59,9 +59,7 @@ def test_state_transition_matrix_update():
 
     dtheta_update = np.ones(3) * 0.01
     dvel_update = np.ones(3) * 0.1
-    _state_transition_matrix_update(
-        phi, dvel=dvel_update, dtheta=dtheta_update, R_nb=R_nb
-    )
+    _state_transition_matrix_update(phi, dvel_update, dtheta_update, R_nb)
 
     phi_expected = np.array(
         [

From f861d4b3c1acfb6ec44b35ca9728ac9805e8885b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:02:41 +0200
Subject: [PATCH 172/217] delete obsolete code

---
 tests/test_ins_/test_ahrs.py | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index d5f8bc1d..f8f7aa66 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -12,7 +12,6 @@
     _state_transition_matrix_update,
 )
 from smsfusion._ins._common import _gref_b_from_quat
-from smsfusion._transforms import _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
@@ -244,10 +243,9 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
         # Reference signals (without noise)
-        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
         # IMU and aiding measurements (with noise)
-        pos_std = 0.1  # m
         vel_std = 0.01  # m/s
         head_std = np.radians(0.1)  # rad
         err_acc = sf.constants.ERR_ACC_MOTION2
@@ -273,7 +271,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
             gyro_bias_corr_time=err_gyro["tau_cb"],
         )
 
-        pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
+        vel_est, euler_est, bias_gyro_est = [], [], []
         for f_i, w_i, h_i, v_i in zip(acc_imu, gyro_imu, head_meas, vel_meas):
 
             dvel_i = f_i / fs_imu
@@ -336,7 +334,7 @@ def test_benchmark_no_aiding(self, benchmark_gen, gyro_degrees):
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
         # Reference signals (without noise)
-        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
         # IMU and aiding measurements (with noise)
         err_acc = sf.constants.ERR_ACC_MOTION2

From 7bd1d4347e8e210dc20c7f720f21442b20329d44 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:06:04 +0200
Subject: [PATCH 173/217] small fixes

---
 src/smsfusion/_ins/_ahrs.py  | 5 ++---
 src/smsfusion/_ins/_ains_.py | 2 +-
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index a3036625..fbc3a4da 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -72,7 +72,7 @@ def _state_transition_matrix_update(
     dvel: NDArray[np.float64],
     dtheta: NDArray[np.float64],
     R_nb: NDArray[np.float64],
-) -> NDArray[np.float64]:
+) -> None:
     """
     Update the state transition matrix in place.
 
@@ -112,7 +112,6 @@ def _state_transition_matrix_update(
     phi[0, 5] = -dvy * r00 + dvx * r01
     phi[1, 5] = -dvy * r10 + dvx * r11
     phi[2, 5] = -dvy * r20 + dvx * r21
-    return phi
 
 
 def _process_noise_covariance_matrix(
@@ -221,7 +220,7 @@ def _reset(
         estimates. Will be reset to zero after applying the corrections.
     """
     v_n[:] += dx[0:3]
-    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
+    _update_quaternion_with_gibbs2(q_nb, dx[3:6])  # -> update q_nb (in place)
     bg_b[:] += dx[6:9]
     dx[:] = 0.0
 
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 241dcf01..ac554529 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -239,7 +239,7 @@ def _reset(
     """
     p_n[:] += dx[0:3]
     v_n[:] += dx[3:6]
-    _update_quaternion_with_gibbs2(q_nb, dx[6:9])
+    _update_quaternion_with_gibbs2(q_nb, dx[6:9])  # -> update q_nb (in place)
     bg_b[:] += dx[9:12]
     dx[:] = 0.0
 

From 869e85b7f76da13a52e02c9333e7cd9d17eca321 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:16:24 +0200
Subject: [PATCH 174/217] rename bench gref aid tests

---
 tests/test_ins_/test_ahrs.py | 2 +-
 tests/test_ins_/test_ains.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins_/test_ahrs.py
index f8f7aa66..57702f8c 100644
--- a/tests/test_ins_/test_ahrs.py
+++ b/tests/test_ins_/test_ahrs.py
@@ -329,7 +329,7 @@ def rmse(ref, est):
             (benchmark_full_pva_chirp_202311A, True),
         ],
     )
-    def test_benchmark_no_aiding(self, benchmark_gen, gyro_degrees):
+    def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         fs_imu = 10.0
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 97a24f95..47845a4e 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -414,7 +414,7 @@ def rmse(ref, est):
             (benchmark_full_pva_chirp_202311A, True),
         ],
     )
-    def test_benchmark_no_aiding(self, benchmark_gen, gyro_degrees):
+    def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         fs_imu = 10.0
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 

From cf623dd2ada6b5fb8db4e2381a9120c0be9fccc1 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:34:22 +0200
Subject: [PATCH 175/217] small fix

---
 src/smsfusion/_ins/_ahrs.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index fbc3a4da..9a65e0ba 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -444,7 +444,7 @@ def update(
 
         Returns
         -------
-        AINS
+        AHRS
             A reference to the instance itself after the update.
         """
 

From 3162e74a197abb2b216e1f600103e20a82b2ee40 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:43:51 +0200
Subject: [PATCH 176/217] refactore VRU

---
 src/smsfusion/_ins/_vru.py      | 169 ++++++++++-----
 tests/test_ins_/old_test_vru.py | 196 ++++++++++++++++++
 tests/test_ins_/test_vru.py     | 355 +++++++++++++++++++++++---------
 3 files changed, 562 insertions(+), 158 deletions(-)
 create mode 100644 tests/test_ins_/old_test_vru.py

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index f7a245e9..6b5b4f6a 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -7,8 +7,9 @@
 from smsfusion._transforms import _euler_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 
-from ._aiding import _aiding_update_gref
+from ._aiding import _aiding_update_gref, _aiding_update_head
 from ._common import (
+    _dhda_head,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
@@ -16,7 +17,7 @@
     _update_quaternion_with_rotvec,
 )
 
-P0 = (
+_P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
     (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
@@ -60,7 +61,7 @@ def _state_transition_matrix_init(
 def _state_transition_matrix_update(
     phi: NDArray[np.float64],
     dtheta: NDArray[np.float64],
-) -> NDArray[np.float64]:
+) -> None:
     """
     Update the state transition matrix in place.
 
@@ -80,7 +81,6 @@ def _state_transition_matrix_update(
     phi[1, 2] = dtx
     phi[2, 0] = dty
     phi[2, 1] = -dtx
-    return phi
 
 
 @njit  # type: ignore[misc]
@@ -112,25 +112,38 @@ def _process_noise_covariance_matrix(
     return Q
 
 
-@njit  # type: ignore[misc]
-def _measurement_matrix_init() -> NDArray[np.float64]:
+def _measurement_matrix_init(
+    q_nb: NDArray[np.float64], nav_frame_factor: float
+) -> NDArray[np.float64]:
     """
     Measurement matrix.
 
+    Parameters
+    ----------
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    nav_frame_factor: float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
+
     Returns
     -------
-    ndarray, shape (3, 6)
-        Initial linearized measurement matrix.
+    ndarray, shape (4, 6)
+        Linearized measurement matrix.
     """
-    return np.zeros((3, 6))
+    vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
+    H = np.zeros((4, 6))
+    H[0:1, 0:3] = _dhda_head(q_nb)  # heading
+    H[1:4, 0:3] = _skew_symmetric(vg_b)  # gravity reference vector
+    return H
 
 
 @njit  # type: ignore[misc]
 def _reset(
     dx: NDArray[np.float64], q_nb: NDArray[np.float64], bg_b: NDArray[np.float64]
-) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Reset state.
+    Reset state (in place).
 
     Parameters
     ----------
@@ -142,29 +155,30 @@ def _reset(
     bg_b : ndarray, shape (3,)
         Gyroscope bias state estimate to be reset in place.
     """
-    q_nb = _update_quaternion_with_gibbs2(q_nb, dx[0:3])
+    _update_quaternion_with_gibbs2(q_nb, dx[0:3])  # -> update q_nb (in place)
     bg_b[:] += dx[3:6]
     dx[:] = 0.0
-    return dx, q_nb, bg_b
 
 
 class VRU:
     """
-    Vertical Reference Unit (VRU) using a multiplicative extended
-    Kalman filter (MEKF). Uses only gravitational vector as aiding.
+    Vertical Reference Unit (VRU).
+
+    This class provides attitude and gyro bias estimation using a multiplicative
+    extended Kalman filter (MEKF).
 
     Parameters
     ----------
     fs : float
         Sampling rate in Hz.
-    q : Attitude or array_like, shape (4,), optional
+    q0 : array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg : array_like, shape (3,), optional
+    bg0 : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P : array_like, shape (6, 6), optional
+    P0 : array_like, shape (6, 6), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
-        a small diagonal matrix (1e-6 * np.eye(9)).
+        a small diagonal matrix (1e-6 * np.eye(6)).
     gyro_noise_density : float, optional
         Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
         0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
@@ -177,15 +191,15 @@ class VRU:
         Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
         (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
         will be expressed relative to this frame.
-
     """
 
     def __init__(
         self,
         fs: float,
-        q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg: ArrayLike = (0.0, 0.0, 0.0),
-        P: ArrayLike = P0,
+        q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg0: ArrayLike = (0.0, 0.0, 0.0),
+        P0: ArrayLike = _P0,
+        acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
         gyro_bias_corr_time: float = 50.0,
@@ -197,41 +211,39 @@ def __init__(
         self._nz2vg = _nz2vg(self._nav_frame)
 
         # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
         self._arw = gyro_noise_density  # angular random walk
         self._gbs = gyro_bias_stability  # gyro bias stability
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
-        self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
-        self._P = np.asarray_chkfinite(P).reshape(6, 6).copy()
+        self._q_nb = np.asarray_chkfinite(q0).reshape(4).copy()
+        self._bg_b = np.asarray_chkfinite(bg0).reshape(3).copy()
+        self._P = np.asarray_chkfinite(P0).reshape(6, 6).copy()
         self._dx = np.zeros(6)
 
         # Discrete state-space model
-        self._phi = _state_transition_matrix_init(
-            self._dt,
-            np.zeros(3),
-            self._gbc,
-        )
+        self._phi = _state_transition_matrix_init(self._dt, np.zeros(3), self._gbc)
         self._Q = _process_noise_covariance_matrix(
             self._dt, self._arw, self._gbs, self._gbc
         )
-        self._H = _measurement_matrix_init()
+        self._H = _measurement_matrix_init(self._q_nb, self._nz2vg)
 
     def quaternion(self) -> NDArray[np.float64]:
         """
-        Attitude expressed as a unit quaternion.
+        Copy of the attitude estimate expressed as a unit quaternion.
         """
         return self._q_nb.copy()
 
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
-        Attitude expressed as Euler angles (roll, pitch, yaw).
+        Copy of the attitude estimate expressed as Euler angles (roll, pitch, yaw).
 
         Parameters
         ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
+        degrees : bool, optional
+            Whether to return the Euler angles in degrees or radians. Defaults to
+            radians.
 
         Returns
         -------
@@ -248,7 +260,8 @@ def euler(self, degrees: bool = False) -> NDArray[np.float64]:
 
     def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
         """
-        Gyroscope bias estimate (rad/s) expressed in the body frame.
+        Copy of the gyroscope bias estimate in rad/s or deg/s depending on the
+        ``degrees`` flag.
 
         Parameters
         ----------
@@ -272,31 +285,46 @@ def update(
         dvel: ArrayLike,
         dtheta: ArrayLike,
         degrees: bool = False,
-        gref: bool = True,
-        gref_var: ArrayLike = (0.001, 0.001, 0.001),
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = False,
+        gref: bool = False,
+        gref_var: ArrayLike | None = None,
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
 
         Parameters
         ----------
-        dvel : array_like, shape (3,), optional
+        dvel : array_like, shape (3,)
             Velocity increment (sculling integral) in m/s.
-        dtheta : array_like, shape (3,), optional
-            Attitude increment (coning integral) in radians.
+        dtheta : array_like, shape (3,)
+            Attitude increment (coning integral) in radians or degrees depending
+            on the ``degrees`` flag.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
+        head : float, optional
+            Heading measurement in radians or degrees depending on the ``head_degrees``
+            flag. I.e., the yaw angle of the 'body' frame relative to the assumed
+            'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used.
+        head_var : float, optional
+            Variance of heading measurement noise in radians^2 or degrees^2 depending
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees
+            and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
         gref : bool, optional
-            Specifies whether to use accelerometer measurements (dv) and the known
-            direction of gravity as aiding. Defaults to ``True``.
+            Specifies whether to use accelerometer measurements (dvel) and the known
+            direction of gravity as aiding. Defaults to ``False``.
         gref_var : array_like, shape (3,), optional
             Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding. Defaults to (0.001, 0.001, 0.001).
+            Required for gravity reference vector aiding.
 
         Returns
         -------
-        AHRS
+        VRU
             A reference to the instance itself after the update.
         """
 
@@ -308,23 +336,50 @@ def update(
 
         dtheta = dtheta - self._dt * self._bg_b
 
-        # Update state-space model and project (a priori) error covariance matrix estimate ahead
-        self._phi = _state_transition_matrix_update(self._phi, dtheta)
-        self._P = _project_covariance_ahead(self._P, self._phi, self._Q)
+        # Update state-space model
+        _state_transition_matrix_update(self._phi, dtheta)  # -> update phi
 
         # Project (a priori) state estimates ahead
-        self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta)
+        _update_quaternion_with_rotvec(self._q_nb, dtheta)  # -> update q_nb (in place)
+
+        # Project (a priori) error covariance matrix estimate ahead
+        _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
+
+        # Update (a posteriori) estimates with heading aiding
+        if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            self._H[0, 0:3] = _dhda_head(self._q_nb)
+
+            _aiding_update_head(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[0],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
 
-        # Update (a posteriori) state and covariance estimates with aiding measurements
+        # Update (a posteriori) estimates with gravity reference vector aiding
         if gref is True:
-            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
-            self._H[0:3, 0:3] = _skew_symmetric(vg_b)  # Update measurement matrix
+            if gref_var is None:
+                raise ValueError("'gref_var' is required for gravity reference aiding.")
 
-            self._dx, self._P = _aiding_update_gref(
-                self._dx, self._P, self._H[0:3], vg_b, dvel, np.asarray(gref_var)
+            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
+            self._H[1:4, 0:3] = _skew_symmetric(vg_b)
+
+            _aiding_update_gref(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[1:4],
+                vg_b,
+                dvel,
+                np.asarray(gref_var),
             )
 
-        # Reset state
-        self._dx, self._q_nb, self._bg_b = _reset(self._dx, self._q_nb, self._bg_b)
+        # Reset state -> update q_nb, bg_b and dx (in place)
+        _reset(self._dx, self._q_nb, self._bg_b)
 
         return self
diff --git a/tests/test_ins_/old_test_vru.py b/tests/test_ins_/old_test_vru.py
new file mode 100644
index 00000000..0e185960
--- /dev/null
+++ b/tests/test_ins_/old_test_vru.py
@@ -0,0 +1,196 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion._ins._vru import (
+    VRU,
+    _measurement_matrix_init,
+    _process_noise_covariance_matrix,
+    _reset,
+    _state_transition_matrix_init,
+    _state_transition_matrix_update,
+)
+from smsfusion.benchmark import (
+    benchmark_pure_attitude_beat_202311A,
+    benchmark_pure_attitude_chirp_202311A,
+)
+
+
+def test_state_transition_matrix_init():
+    dt = 0.1
+    dtheta = np.ones(3) * 0.02
+    gbc = 0.01
+
+    phi_out = _state_transition_matrix_init(dt, dtheta, gbc)
+    phi_expected = np.array(
+        [
+            [1.0, 0.02, -0.02, -dt, 0.0, 0.0],
+            [-0.02, 1.0, 0.02, 0.0, -dt, 0.0],
+            [0.02, -0.02, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_state_transition_matrix_update():
+    dt = 0.1
+    dtheta = np.ones(3) * 0.02
+    gbc = 0.01
+
+    phi_init = _state_transition_matrix_init(dt, dtheta, gbc)
+
+    dtheta_update = np.ones(3) * 0.01
+    phi_out = _state_transition_matrix_update(phi_init, dtheta=dtheta_update)
+
+    phi_expected = np.array(
+        [
+            [1.0, 0.01, -0.01, -dt, 0.0, 0.0],
+            [-0.01, 1.0, 0.01, 0.0, -dt, 0.0],
+            [0.01, -0.01, 1.0, 0.0, 0.0, -dt],
+            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
+        ]
+    )
+
+    np.testing.assert_almost_equal(phi_out, phi_expected)
+
+
+def test_measurement_matrix_init():
+    np.testing.assert_array_equal(_measurement_matrix_init(), np.zeros((3, 6)))
+
+
+def test_process_noise_covariance_matrix():
+    dt = 0.1
+    arw = 0.00005
+    gbs = 0.00005
+    gbc = 50.0
+    Q_out = _process_noise_covariance_matrix(dt, arw, gbs, gbc)
+    Q_expect = np.array(
+        [
+            [dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+            [0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
+            [0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
+            [0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
+            [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
+        ]
+    )
+
+    np.testing.assert_allclose(Q_out, Q_expect)
+
+
+def test_reset():
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    bg_b = np.zeros(3)
+    dx = np.array([0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
+
+    dx, q_nb, bg_b = _reset(dx, q_nb, bg_b)
+
+    np.testing.assert_allclose(dx, np.zeros_like(dx))
+    np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
+    np.testing.assert_allclose(
+        q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
+    )
+
+
+def test_vru_init():
+    mekf = VRU(10.0)
+
+    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+    np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru.P0))
+
+
+@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+def test_vru_nav_frame(nav_frame, scale):
+    mekf = VRU(10.0, nav_frame=nav_frame)
+
+    assert mekf._nz2vg == scale
+
+
+def test_vru_methods():
+    euler_init = np.array([10.0, 20.0, 30.0])
+    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
+    bg_init = np.array([0.01, -0.01, 0.02])
+
+    mekf = VRU(10.0, q=quaternion_init, bg=bg_init)
+
+    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
+    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
+    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
+    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
+    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
+
+
+@pytest.mark.parametrize(
+    "benchmark_gen, degrees",
+    [
+        (benchmark_pure_attitude_beat_202311A, False),
+        (benchmark_pure_attitude_chirp_202311A, True),
+    ],
+)
+def test_vru_benchmark(benchmark_gen, degrees):
+    fs_imu = 100.0
+    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    # Reference signals (without noise)
+    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    # IMU measurements (with noise)
+    bg = np.array([0.01, -0.02, 0.0])
+    noise_model = sf.noise.IMUNoise(
+        err_acc=sf.constants.ERR_ACC_MOTION2,
+        err_gyro=sf.constants.ERR_GYRO_MOTION2,
+        seed=0,
+    )
+    imu_noise = noise_model(fs_imu, len(t))
+    acc_noise = acc_ref + imu_noise[:, :3]
+    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
+
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+
+    # MEKF
+    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+    mekf = VRU(
+        fs_imu,
+        q=q0,
+        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
+        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
+        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
+    )
+
+    # Apply filter
+    euler_out, bias_gyro_out = [], []
+    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
+
+        dvel = f_i / fs_imu
+        dtheta = w_i / fs_imu
+
+        mekf.update(dvel, dtheta, degrees=degrees, gref=True)
+
+        euler_out.append(mekf.euler(degrees=False))
+        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
+
+    euler_out = np.array(euler_out)
+    bias_gyro_out = np.array(bias_gyro_out)
+
+    # Half-sample shift (compensates for the delay introduced by Euler integration)
+    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
+    euler_ref = euler_ref[:-1, :]
+
+    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
+    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
+        (bias_gyro_out - bg)[warmup:], axis=0
+    )
+
+    assert np.degrees(roll_rms) <= 0.1
+    assert np.degrees(pitch_rms) <= 0.1
+    assert np.degrees(bias_gyro_x_rms) <= 0.005
+    assert np.degrees(bias_gyro_y_rms) <= 0.005
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py
index 0e185960..eb2900ab 100644
--- a/tests/test_ins_/test_vru.py
+++ b/tests/test_ins_/test_vru.py
@@ -3,6 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
+from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._ins._vru import (
     VRU,
     _measurement_matrix_init,
@@ -11,9 +12,10 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
+from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
-    benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A,
+    benchmark_full_pva_beat_202311A,
+    benchmark_full_pva_chirp_202311A,
 )
 
 
@@ -42,10 +44,10 @@ def test_state_transition_matrix_update():
     dtheta = np.ones(3) * 0.02
     gbc = 0.01
 
-    phi_init = _state_transition_matrix_init(dt, dtheta, gbc)
+    phi = _state_transition_matrix_init(dt, dtheta, gbc)
 
     dtheta_update = np.ones(3) * 0.01
-    phi_out = _state_transition_matrix_update(phi_init, dtheta=dtheta_update)
+    _state_transition_matrix_update(phi, dtheta=dtheta_update)
 
     phi_expected = np.array(
         [
@@ -58,11 +60,18 @@ def test_state_transition_matrix_update():
         ]
     )
 
-    np.testing.assert_almost_equal(phi_out, phi_expected)
+    np.testing.assert_almost_equal(phi, phi_expected)
 
 
 def test_measurement_matrix_init():
-    np.testing.assert_array_equal(_measurement_matrix_init(), np.zeros((3, 6)))
+    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
+    nz2vg = 1.0
+
+    expect = np.zeros((4, 6))
+    expect[0:1, 0:3] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
+    expect[1:4, 0:3] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+
+    np.testing.assert_array_equal(_measurement_matrix_init(q_nb, nz2vg), expect)
 
 
 def test_process_noise_covariance_matrix():
@@ -90,7 +99,7 @@ def test_reset():
     bg_b = np.zeros(3)
     dx = np.array([0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
 
-    dx, q_nb, bg_b = _reset(dx, q_nb, bg_b)
+    _reset(dx, q_nb, bg_b)
 
     np.testing.assert_allclose(dx, np.zeros_like(dx))
     np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
@@ -99,98 +108,242 @@ def test_reset():
     )
 
 
-def test_vru_init():
-    mekf = VRU(10.0)
-
-    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
-    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-    np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru.P0))
-
-
-@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-def test_vru_nav_frame(nav_frame, scale):
-    mekf = VRU(10.0, nav_frame=nav_frame)
-
-    assert mekf._nz2vg == scale
-
-
-def test_vru_methods():
-    euler_init = np.array([10.0, 20.0, 30.0])
-    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
-    bg_init = np.array([0.01, -0.01, 0.02])
-
-    mekf = VRU(10.0, q=quaternion_init, bg=bg_init)
-
-    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
-    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
-    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
-    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
-    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, degrees",
-    [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
-    ],
-)
-def test_vru_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-    if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-
-    # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = VRU(
-        fs_imu,
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-    )
-
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
-
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
-
-        mekf.update(dvel, dtheta, degrees=degrees, gref=True)
-
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
-
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
+class Test_VRU:
+
+    def test_init(self):
+
+        q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
+        bg0 = np.random.random(3)
+        P0 = 0.1 * np.eye(6)
+        vrw = 0.0001
+        arw = 0.0002
+        gbs = 0.0003
+        gbc = 123.0
+
+        mekf = VRU(
+            51.2,
+            q0=q0,
+            bg0=bg0,
+            P0=P0,
+            gyro_noise_density=arw,
+            gyro_bias_stability=gbs,
+            gyro_bias_corr_time=gbc,
+            nav_frame="enu",
+        )
+
+        assert mekf._fs == pytest.approx(51.2)
+        assert mekf._dt == pytest.approx(1.0 / 51.2)
+        assert mekf._nav_frame == "enu"
+        assert mekf._arw == pytest.approx(arw)
+        assert mekf._gbs == pytest.approx(gbs)
+        assert mekf._gbc == pytest.approx(gbc)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.P, P0)
+        np.testing.assert_allclose(mekf._dx, np.zeros(6))
+
+        # Check that the initial values are copied
+        assert mekf._q_nb is not q0  # copy
+        assert mekf._bg_b is not bg0  # copy
+        assert mekf._P is not P0  # copy
+
+    def test_init_default(self):
+        mekf = VRU(10.0)
+        assert mekf._fs == pytest.approx(10.0)
+        assert mekf._dt == pytest.approx(0.1)
+        assert mekf._nav_frame == "ned"
+        assert mekf._arw == pytest.approx(0.00005)
+        assert mekf._gbs == pytest.approx(0.00005)
+        assert mekf._gbc == pytest.approx(50.0)
+        np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
+        np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru._P0))
+        np.testing.assert_allclose(mekf._dx, np.zeros(6))
+
+    @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
+    def test_nav_frame(self, nav_frame, scale):
+        mekf = VRU(10.0, nav_frame=nav_frame)
+        assert mekf._nav_frame == nav_frame.lower()
+
+    def test_quaternion(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        q0 = sf.quaternion_from_euler(euler, degrees=True)
+        mekf = VRU(10.0, q0=q0)
+        np.testing.assert_allclose(mekf.quaternion(), q0)
+        assert mekf.quaternion() is not mekf._q_nb  # copy
+
+    def test_euler(self):
+        euler = np.array([10.0, 20.0, 30.0])
+        mekf = VRU(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        np.testing.assert_allclose(mekf.euler(), np.radians(euler))
+        np.testing.assert_allclose(mekf.euler(degrees=True), euler)
+        np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
+
+    def test_bias_gyro(self):
+        bg0 = np.array([0.01, -0.02, 0.03])
+        mekf = VRU(10.0, bg0=bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
+        np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
+        assert mekf.bias_gyro(degrees=False) is not mekf._bg_b  # copy
+
+    def test_P(self):
+        P0 = 0.1 * np.eye(6)
+        mekf = VRU(10.0, P0=P0)
+        np.testing.assert_allclose(mekf.P, P0)
+        assert mekf.P is not mekf._P  # copy
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
     )
-
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005
+    def test_benchmark(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        head_std = np.radians(0.1)  # rad
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.03])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_imu = acc_ref + imu_noise[:, :3]
+        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
+
+        if gyro_degrees:
+            gyro_imu = np.degrees(gyro_imu)
+
+        # MEKF
+        mekf = VRU(
+            fs_imu,
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
+        )
+
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i, h_i in zip(acc_imu, gyro_imu, head_meas):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(
+                dvel_i,
+                dtheta_i,
+                degrees=gyro_degrees,
+                head=h_i,
+                head_var=head_std**2,
+                head_degrees=False,
+                gref=True,
+                gref_var=(0.0001, 0.0001, 0.0001),
+            )
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        vel_ref = vel_ref[:-1, :]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, bgz_rmse = rmse(
+            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
+        )
+
+        assert np.degrees(roll_rmse) <= 0.5
+        assert np.degrees(pitch_rmse) <= 0.5
+        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01
+        assert np.degrees(bgz_rmse) <= 0.01
+
+    # @pytest.mark.parametrize(
+    #     "benchmark_gen, gyro_degrees",
+    #     [
+    #         (benchmark_full_pva_beat_202311A, False),
+    #         (benchmark_full_pva_chirp_202311A, True),
+    #     ],
+    # )
+    # def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
+    #     fs_imu = 10.0
+    #     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+    #     # Reference signals (without noise)
+    #     t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+    #     # IMU and aiding measurements (with noise)
+    #     err_acc = sf.constants.ERR_ACC_MOTION2
+    #     err_gyro = sf.constants.ERR_GYRO_MOTION2
+    #     noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+    #     bg = np.array([0.01, -0.02, 0.0])  # rad/s
+    #     imu_noise = noise_model(fs_imu, len(t))
+    #     acc_imu = acc_ref + imu_noise[:, :3]
+    #     gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+
+    #     if gyro_degrees:
+    #         gyro_imu = np.degrees(gyro_imu)
+
+    #     # MEKF
+    #     mekf = AHRS(
+    #         fs_imu,
+    #         v0=vel_ref[0],
+    #         q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+    #         gyro_noise_density=err_gyro["N"],
+    #         gyro_bias_stability=err_gyro["B"],
+    #         gyro_bias_corr_time=err_gyro["tau_cb"],
+    #     )
+
+    #     euler_est, bias_gyro_est = [], []
+    #     for f_i, w_i in zip(acc_imu, gyro_imu):
+
+    #         dvel_i = f_i / fs_imu
+    #         dtheta_i = w_i / fs_imu
+
+    #         mekf.update(
+    #             dvel_i,
+    #             dtheta_i,
+    #             degrees=gyro_degrees,
+    #             vel=np.zeros(3),
+    #             vel_var=100.0 * np.ones(3),
+    #             gref=True,
+    #             gref_var=(0.001, 0.001, 0.001),
+    #         )
+    #         euler_est.append(mekf.euler(degrees=False))
+    #         bias_gyro_est.append(mekf.bias_gyro())
+
+    #     euler_est = np.array(euler_est)
+    #     bias_gyro_est = np.array(bias_gyro_est)
+
+    #     # Half-sample shift (compensates for the delay introduced by Euler integration)
+    #     euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+    #     bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+    #     euler_ref = euler_ref[:-1, :]
+    #     bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+    #     def rmse(ref, est):
+    #         return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+    #     roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+    #     bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
+
+    #     assert np.degrees(roll_rmse) <= 0.5
+    #     assert np.degrees(pitch_rmse) <= 0.5
+    #     assert np.degrees(bgx_rmse) <= 0.01
+    #     assert np.degrees(bgy_rmse) <= 0.01

From 587b8feb5bb6311ba128358055a2b33962076000 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:44:13 +0200
Subject: [PATCH 177/217] delete old vru tests

---
 tests/test_ins_/old_test_vru.py | 196 --------------------------------
 1 file changed, 196 deletions(-)
 delete mode 100644 tests/test_ins_/old_test_vru.py

diff --git a/tests/test_ins_/old_test_vru.py b/tests/test_ins_/old_test_vru.py
deleted file mode 100644
index 0e185960..00000000
--- a/tests/test_ins_/old_test_vru.py
+++ /dev/null
@@ -1,196 +0,0 @@
-import numpy as np
-import pytest
-from scipy.signal import resample_poly
-
-import smsfusion as sf
-from smsfusion._ins._vru import (
-    VRU,
-    _measurement_matrix_init,
-    _process_noise_covariance_matrix,
-    _reset,
-    _state_transition_matrix_init,
-    _state_transition_matrix_update,
-)
-from smsfusion.benchmark import (
-    benchmark_pure_attitude_beat_202311A,
-    benchmark_pure_attitude_chirp_202311A,
-)
-
-
-def test_state_transition_matrix_init():
-    dt = 0.1
-    dtheta = np.ones(3) * 0.02
-    gbc = 0.01
-
-    phi_out = _state_transition_matrix_init(dt, dtheta, gbc)
-    phi_expected = np.array(
-        [
-            [1.0, 0.02, -0.02, -dt, 0.0, 0.0],
-            [-0.02, 1.0, 0.02, 0.0, -dt, 0.0],
-            [0.02, -0.02, 1.0, 0.0, 0.0, -dt],
-            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-        ]
-    )
-
-    np.testing.assert_almost_equal(phi_out, phi_expected)
-
-
-def test_state_transition_matrix_update():
-    dt = 0.1
-    dtheta = np.ones(3) * 0.02
-    gbc = 0.01
-
-    phi_init = _state_transition_matrix_init(dt, dtheta, gbc)
-
-    dtheta_update = np.ones(3) * 0.01
-    phi_out = _state_transition_matrix_update(phi_init, dtheta=dtheta_update)
-
-    phi_expected = np.array(
-        [
-            [1.0, 0.01, -0.01, -dt, 0.0, 0.0],
-            [-0.01, 1.0, 0.01, 0.0, -dt, 0.0],
-            [0.01, -0.01, 1.0, 0.0, 0.0, -dt],
-            [0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc, 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, 1.0 - dt / gbc],
-        ]
-    )
-
-    np.testing.assert_almost_equal(phi_out, phi_expected)
-
-
-def test_measurement_matrix_init():
-    np.testing.assert_array_equal(_measurement_matrix_init(), np.zeros((3, 6)))
-
-
-def test_process_noise_covariance_matrix():
-    dt = 0.1
-    arw = 0.00005
-    gbs = 0.00005
-    gbc = 50.0
-    Q_out = _process_noise_covariance_matrix(dt, arw, gbs, gbc)
-    Q_expect = np.array(
-        [
-            [dt * arw**2, 0.0, 0.0, 0.0, 0.0, 0.0],
-            [0.0, dt * arw**2, 0.0, 0.0, 0.0, 0.0],
-            [0.0, 0.0, dt * arw**2, 0.0, 0.0, 0.0],
-            [0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0, 0.0],
-            [0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc), 0.0],
-            [0.0, 0.0, 0.0, 0.0, 0.0, dt * (2.0 * gbs**2 / gbc)],
-        ]
-    )
-
-    np.testing.assert_allclose(Q_out, Q_expect)
-
-
-def test_reset():
-    q_nb = np.array([1.0, 0.0, 0.0, 0.0])
-    bg_b = np.zeros(3)
-    dx = np.array([0.01, 0.0, 0.0, 0.1, -0.1, 0.2])
-
-    dx, q_nb, bg_b = _reset(dx, q_nb, bg_b)
-
-    np.testing.assert_allclose(dx, np.zeros_like(dx))
-    np.testing.assert_allclose(bg_b, np.array([0.1, -0.1, 0.2]))
-    np.testing.assert_allclose(
-        q_nb, np.array([np.cos(0.01 / 2), np.sin(0.01 / 2), 0.0, 0.0]), atol=1e-6
-    )
-
-
-def test_vru_init():
-    mekf = VRU(10.0)
-
-    np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
-    np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-    np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru.P0))
-
-
-@pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-def test_vru_nav_frame(nav_frame, scale):
-    mekf = VRU(10.0, nav_frame=nav_frame)
-
-    assert mekf._nz2vg == scale
-
-
-def test_vru_methods():
-    euler_init = np.array([10.0, 20.0, 30.0])
-    quaternion_init = sf.quaternion_from_euler(euler_init, degrees=True)
-    bg_init = np.array([0.01, -0.01, 0.02])
-
-    mekf = VRU(10.0, q=quaternion_init, bg=bg_init)
-
-    np.testing.assert_allclose(mekf.euler(), np.radians(euler_init))
-    np.testing.assert_allclose(mekf.euler(degrees=True), euler_init)
-    np.testing.assert_allclose(mekf.quaternion(), quaternion_init)
-    np.testing.assert_allclose(mekf.bias_gyro(), bg_init)
-    np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg_init))
-
-
-@pytest.mark.parametrize(
-    "benchmark_gen, degrees",
-    [
-        (benchmark_pure_attitude_beat_202311A, False),
-        (benchmark_pure_attitude_chirp_202311A, True),
-    ],
-)
-def test_vru_benchmark(benchmark_gen, degrees):
-    fs_imu = 100.0
-    warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    # Reference signals (without noise)
-    t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    # IMU measurements (with noise)
-    bg = np.array([0.01, -0.02, 0.0])
-    noise_model = sf.noise.IMUNoise(
-        err_acc=sf.constants.ERR_ACC_MOTION2,
-        err_gyro=sf.constants.ERR_GYRO_MOTION2,
-        seed=0,
-    )
-    imu_noise = noise_model(fs_imu, len(t))
-    acc_noise = acc_ref + imu_noise[:, :3]
-    gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-    if degrees:
-        gyro_noise = np.degrees(gyro_noise)
-
-    # MEKF
-    q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-    mekf = VRU(
-        fs_imu,
-        q=q0,
-        gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-        gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-        gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-    )
-
-    # Apply filter
-    euler_out, bias_gyro_out = [], []
-    for i, (f_i, w_i) in enumerate(zip(acc_noise, gyro_noise)):
-
-        dvel = f_i / fs_imu
-        dtheta = w_i / fs_imu
-
-        mekf.update(dvel, dtheta, degrees=degrees, gref=True)
-
-        euler_out.append(mekf.euler(degrees=False))
-        bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-    euler_out = np.array(euler_out)
-    bias_gyro_out = np.array(bias_gyro_out)
-
-    # Half-sample shift (compensates for the delay introduced by Euler integration)
-    euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-    euler_ref = euler_ref[:-1, :]
-
-    roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-    bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-        (bias_gyro_out - bg)[warmup:], axis=0
-    )
-
-    assert np.degrees(roll_rms) <= 0.1
-    assert np.degrees(pitch_rms) <= 0.1
-    assert np.degrees(bias_gyro_x_rms) <= 0.005
-    assert np.degrees(bias_gyro_y_rms) <= 0.005

From ea6166837c44b15d6e395f0c9c1f1b1e42c00d37 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:48:56 +0200
Subject: [PATCH 178/217] delete obsolete vrw param VRU

---
 src/smsfusion/_ins/_vru.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 6b5b4f6a..30a03adf 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -199,7 +199,6 @@ def __init__(
         q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg0: ArrayLike = (0.0, 0.0, 0.0),
         P0: ArrayLike = _P0,
-        acc_noise_density: float = 0.0007,
         gyro_noise_density: float = 0.00005,
         gyro_bias_stability: float = 0.00005,
         gyro_bias_corr_time: float = 50.0,
@@ -211,7 +210,6 @@ def __init__(
         self._nz2vg = _nz2vg(self._nav_frame)
 
         # IMU noise parameters
-        self._vrw = acc_noise_density  # velocity random walk
         self._arw = gyro_noise_density  # angular random walk
         self._gbs = gyro_bias_stability  # gyro bias stability
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time

From a2da7c13759c47f625f1dc9ca58703d1eb4d8d15 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Tue, 25 Aug 2026 14:49:37 +0200
Subject: [PATCH 179/217] docstring fix P shape

---
 src/smsfusion/_ins/_ains_.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index ac554529..1f73a5d9 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -288,9 +288,9 @@ class AINS:
         to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
     bg0 : array_like, shape (3,), optional
         Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P0 : array_like, shape (6, 6), optional
+    P0 : array_like, shape (12, 12), optional
         Initial (a priori) estimate of the error covariance matrix. Defaults to
-        a small diagonal matrix (1e-6 * np.eye(9)).
+        a small diagonal matrix (1e-6 * np.eye(12)).
     acc_noise_density : float, optional
         Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
         0.0007 (m/s)/√Hz (SMS Motion 2 noise level).

From 4e7dee3fa66141fa409e93658dcd4e53663dff8d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 08:28:38 +0200
Subject: [PATCH 180/217] njit gref aiding

---
 src/smsfusion/_ins/_aiding.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index 0b214ca7..e43f6880 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -76,6 +76,7 @@ def _aiding_update_head(
     return dx, P
 
 
+@njit  # type: ignore[misc]
 def _aiding_update_gref(
     dx: NDArray[np.float64],
     P: NDArray[np.float64],

From 77341a9e770a7ad1bb00b38aa819d1c884cef55c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 09:30:35 +0200
Subject: [PATCH 181/217] aiding in place functions dont return arrays

---
 src/smsfusion/_ins/_aiding.py | 33 ++++++++++++++++-----------------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index e43f6880..0042075a 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -22,9 +22,10 @@ def _aiding_update_pos(
     pos_var: NDArray[np.float64],
     R_nb: NDArray[np.float64],
     lever_arm: NDArray[np.float64],
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Update error state (dx) and error covariance (P) with position aiding measurement.
+    Update (in place) the error state (dx) and the error covariance (P) with position
+    aiding measurement.
     """
 
     if lever_arm.any():
@@ -32,8 +33,7 @@ def _aiding_update_pos(
     else:
         dz = pos_meas - pos_n
 
-    dx, P = _kalman_update_sequential(dx, P, dz, pos_var, H)
-    return dx, P
+    _kalman_update_sequential(dx, P, dz, pos_var, H)  # -> update dx and P (in place)
 
 
 @njit  # type: ignore[misc]
@@ -44,13 +44,13 @@ def _aiding_update_vel(
     vel_n: NDArray[np.float64],
     vel_meas: NDArray[np.float64],
     vel_var: NDArray[np.float64],
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Update error state (dx) and error covariance (P) with velocity aiding measurement.
+    Update (in place) the error state (dx) and the error covariance (P) with velocity
+    aiding measurement.
     """
     dz = vel_meas - vel_n
-    dx, P = _kalman_update_sequential(dx, P, dz, vel_var, H)
-    return dx, P
+    _kalman_update_sequential(dx, P, dz, vel_var, H)
 
 
 @njit  # type: ignore[misc]
@@ -62,9 +62,10 @@ def _aiding_update_head(
     head_meas: float,
     head_var: float,
     head_degrees: bool,
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Update error state (dx) and error covariance (P) with heading aiding measurement.
+    Update (in place) the error state (dx) and the error covariance (P) with heading
+    aiding measurement.
     """
 
     if head_degrees:
@@ -72,8 +73,7 @@ def _aiding_update_head(
         head_var = (np.pi / 180.0) ** 2 * head_var
 
     dz = _signed_smallest_angle(head_meas - _h_head(q_nb))
-    dx, P = _kalman_update_scalar(dx, P, dz, head_var, H)
-    return dx, P
+    _kalman_update_scalar(dx, P, dz, head_var, H)  # -> update dx and P (in place)
 
 
 @njit  # type: ignore[misc]
@@ -84,11 +84,10 @@ def _aiding_update_gref(
     vg_b: NDArray[np.float64],
     dvel: NDArray[np.float64],
     gref_var: NDArray[np.float64],
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Update error state (dx) and error covariance (P) with gravity reference vector
-    aiding measurement.
+    Update (in place) the error state (dx) and the error covariance (P) with gravity
+    reference vector aiding measurement.
     """
     dz = -_normalize(dvel) - vg_b
-    dx, P = _kalman_update_sequential(dx, P, dz, gref_var, H)
-    return dx, P
+    _kalman_update_sequential(dx, P, dz, gref_var, H)  # -> update dx and P (in place)

From 44e188fb57126c4b257a889222c682c0ab35399a Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 09:42:53 +0200
Subject: [PATCH 182/217] in place kalman functions dont return

---
 src/smsfusion/_ins/_common.py | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index eba5c539..69995987 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -245,9 +245,9 @@ def _kalman_update_scalar(
     z: float,
     r: float,
     h: NDArray[np.float64],
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Scalar Kalman filter measurement update.
+    Scalar Kalman filter measurement update (in place).
 
     Parameters
     ----------
@@ -271,7 +271,6 @@ def _kalman_update_scalar(
 
     # Updated (a posteriori) covariance estimate (Joseph form)
     P[:, :] = _covariance_update(P, k, h, r)
-    return x, P
 
 
 @njit  # type: ignore[misc]
@@ -281,9 +280,9 @@ def _kalman_update_sequential(
     z: NDArray[np.float64],
     var: NDArray[np.float64],
     H: NDArray[np.float64],
-) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+) -> None:
     """
-    Sequential (one-at-a-time) Kalman filter measurement update.
+    Sequential (one-at-a-time) Kalman filter measurement update (in place).
 
     Parameters
     ----------
@@ -300,16 +299,15 @@ def _kalman_update_sequential(
     """
     m = z.shape[0]
     for i in range(m):
-        x, P = _kalman_update_scalar(x, P, z[i], var[i], H[i])
-    return x, P
+        _kalman_update_scalar(x, P, z[i], var[i], H[i])
 
 
 @njit  # type: ignore[misc]
 def _project_covariance_ahead(
     P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64]
-) -> NDArray[np.float64]:
+) -> None:
     """
-    Project the error covariance matrix estimate ahead.
+    Project the error covariance matrix estimate ahead (in place).
 
     Parameters
     ----------
@@ -321,7 +319,6 @@ def _project_covariance_ahead(
         Process noise covariance matrix.
     """
     P[:, :] = phi @ P @ phi.T + Q
-    return P
 
 
 @njit  # type: ignore[misc]

From af13ba4d74e24cf05d7331463ecf3085a6b19625 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 09:49:03 +0200
Subject: [PATCH 183/217] inplace operations dont return values

---
 src/smsfusion/_ins/_common.py  | 12 +++++-------
 tests/test_ins_/test_common.py | 20 ++++++++------------
 2 files changed, 13 insertions(+), 19 deletions(-)

diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index 69995987..2219d904 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -99,8 +99,8 @@ def _update_quaternion_with_rotvec(
     q: NDArray[np.float64], dtheta: NDArray[np.float64]
 ) -> NDArray[np.float64]:
     """
-    Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized
-    as a rotation vector.
+    Update (in place) a unit quaternion, q, with a small attitude increment, dtheta,
+    parameterized as a rotation vector.
 
     Parameters
     ----------
@@ -135,16 +135,15 @@ def _update_quaternion_with_rotvec(
     q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz
     q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz
     q[:] = _normalize(q)
-    return q
 
 
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
-) -> NDArray[np.float64]:
+) -> None:
     """
-    Update/correct a unit quaternion, q, with a small attitude error, da, parameterized
-    as a scaled (2x) Gibbs vector.
+    Update/correct (in place) a unit quaternion, q, with a small attitude error, da,
+    parameterized as a scaled (2x) Gibbs vector.
 
     As described in ref [1]_, this correction can be simplified by doing it in two
     steps: first a correction, followed by renormalization. The scaling factor becomes
@@ -171,7 +170,6 @@ def _update_quaternion_with_gibbs2(
     q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax)
     q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax)
     q[:] = _normalize(q)
-    return q
 
 
 @njit  # type: ignore[misc]
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py
index 04c0a777..f97ecb80 100644
--- a/tests/test_ins_/test_common.py
+++ b/tests/test_ins_/test_common.py
@@ -119,15 +119,13 @@ def test__signed_smallest_angle(angle, degrees, angle_expect):
     ],
 )
 def test__update_quaternion_with_rotvec(quaternion, dtheta, quaternion_update_expected):
-    quaternion_update = _common._update_quaternion_with_rotvec(quaternion, dtheta)
+    _common._update_quaternion_with_rotvec(quaternion, dtheta)
 
     assert np.isclose(
-        np.linalg.norm(quaternion_update), 1.0
-    ), f"Output quaternion is not unit norm: {quaternion_update}"
+        np.linalg.norm(quaternion), 1.0
+    ), f"Output quaternion is not unit norm: {quaternion}"
 
-    np.testing.assert_allclose(
-        quaternion_update, quaternion_update_expected, atol=1e-16
-    )
+    np.testing.assert_allclose(quaternion, quaternion_update_expected, atol=1e-16)
 
 
 @pytest.mark.parametrize(
@@ -184,16 +182,14 @@ def test__update_quaternion_with_rotvec(quaternion, dtheta, quaternion_update_ex
     ],
 )
 def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expected):
-    quaternion_update = _common._update_quaternion_with_gibbs2(quaternion.copy(), da)
+    _common._update_quaternion_with_gibbs2(quaternion, da)
 
     # Always assert unit norm
     assert np.isclose(
-        np.linalg.norm(quaternion_update), 1.0
-    ), f"Output quaternion is not unit norm: {quaternion_update}"
+        np.linalg.norm(quaternion), 1.0
+    ), f"Output quaternion is not unit norm: {quaternion}"
 
-    np.testing.assert_allclose(
-        quaternion_update, quaternion_update_expected, atol=1e-10
-    )
+    np.testing.assert_allclose(quaternion, quaternion_update_expected, atol=1e-10)
 
 
 @pytest.mark.parametrize(

From 7df1427a5abf30d7e85d108c6f1f2844eef8f35f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 09:55:35 +0200
Subject: [PATCH 184/217] comments fix

---
 src/smsfusion/_ins/_ahrs.py  |  8 +++++---
 src/smsfusion/_ins/_ains_.py | 10 ++++++----
 src/smsfusion/_ins/_vru.py   |  6 ++++--
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 9a65e0ba..dc960c7c 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -473,11 +473,11 @@ def update(
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        # Update (a posteriori) estimates with velocity aiding
         if vel is not None:
             if vel_var is None:
                 raise ValueError("'vel_var' is required for velocity aiding.")
 
+            # Update (a posteriori) estimates with velocity aiding
             _aiding_update_vel(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -487,13 +487,14 @@ def update(
                 np.asarray(vel_var),
             )
 
-        # Update (a posteriori) estimates with heading aiding
         if head is not None:
             if head_var is None:
                 raise ValueError("'head_var' is required for heading aiding.")
 
+            # Update measurement matrix (heading row)
             self._H[3, 6:9] = _dhda_head(self._q_nb)
 
+            # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -504,14 +505,15 @@ def update(
                 head_degrees,
             )
 
-        # Update (a posteriori) estimates with gravity reference vector aiding
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
+            # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
             self._H[4:7, 3:6] = _skew_symmetric(vg_b)
 
+            # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
index 1f73a5d9..6daea77d 100644
--- a/src/smsfusion/_ins/_ains_.py
+++ b/src/smsfusion/_ins/_ains_.py
@@ -522,11 +522,11 @@ def update(
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        # Update (a posteriori) estimates with position aiding
         if pos is not None:
             if pos_var is None:
                 raise ValueError("'pos_var' is required for position aiding.")
 
+            # Update (a posteriori) estimates with position aiding
             _aiding_update_pos(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -538,11 +538,11 @@ def update(
                 self._lever_arm,
             )
 
-        # Update (a posteriori) estimates with velocity aiding
         if vel is not None:
             if vel_var is None:
                 raise ValueError("'vel_var' is required for velocity aiding.")
 
+            # Update (a posteriori) estimates with velocity aiding
             _aiding_update_vel(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -552,13 +552,14 @@ def update(
                 np.asarray(vel_var),
             )
 
-        # Update (a posteriori) estimates with heading aiding
         if head is not None:
             if head_var is None:
                 raise ValueError("'head_var' is required for heading aiding.")
 
+            # Update measurement matrix (heading row)
             self._H[6, 6:9] = _dhda_head(self._q_nb)
 
+            # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -569,14 +570,15 @@ def update(
                 head_degrees,
             )
 
-        # Update (a posteriori) estimates with gravity reference vector aiding
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
+            # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
             self._H[7:10, 6:9] = _skew_symmetric(vg_b)
 
+            # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 30a03adf..48af24c2 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -343,13 +343,14 @@ def update(
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        # Update (a posteriori) estimates with heading aiding
         if head is not None:
             if head_var is None:
                 raise ValueError("'head_var' is required for heading aiding.")
 
+            # Update measurement matrix (heading row)
             self._H[0, 0:3] = _dhda_head(self._q_nb)
 
+            # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
@@ -360,14 +361,15 @@ def update(
                 head_degrees,
             )
 
-        # Update (a posteriori) estimates with gravity reference vector aiding
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
+            # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
             self._H[1:4, 0:3] = _skew_symmetric(vg_b)
 
+            # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,

From f803d69749d0e080a8fcefb23653fd1d7e497bb8 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:03:09 +0200
Subject: [PATCH 185/217] rename ains old module

---
 src/smsfusion/_ins/__init__.py  |    4 +-
 src/smsfusion/_ins/_ains.py     | 1789 ++++++++-----------------------
 src/smsfusion/_ins/_ains_.py    |  594 ----------
 src/smsfusion/_ins/_ains_old.py | 1501 ++++++++++++++++++++++++++
 tests/test_ins_/test_ains.py    |    4 +-
 5 files changed, 1946 insertions(+), 1946 deletions(-)
 delete mode 100644 src/smsfusion/_ins/_ains_.py
 create mode 100644 src/smsfusion/_ins/_ains_old.py

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index bf40489f..0a619081 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,5 +1,5 @@
 from ._ahrs import AHRS as AHRSv2
-from ._ains import AHRS, VRU, AidedINS, StrapdownINS
-from ._ains_ import AINS as AINSv2
+from ._ains_old import AHRS, VRU, AidedINS, StrapdownINS
+from ._ains import AINS as AINSv2
 from ._utils import FixedNED, euler_from_acc, gravity
 from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index f39423e4..6daea77d 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -1,1029 +1,440 @@
-from __future__ import annotations
-
-from typing import Any, Self
+from typing import Self
 
 import numpy as np
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from smsfusion._transforms import (
-    _angular_matrix_from_quaternion,
-    _euler_from_quaternion,
-    _quaternion_from_euler,
-    _rot_matrix_from_quaternion,
+from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
+from smsfusion._vectorops import _skew_symmetric
+
+from ._aiding import (
+    _aiding_update_gref,
+    _aiding_update_head,
+    _aiding_update_pos,
+    _aiding_update_vel,
+)
+from ._common import (
+    _dhda_head,
+    _gref_b_from_quat,
+    _nz2vg,
+    _project_covariance_ahead,
+    _update_quaternion_with_gibbs2,
+    _update_quaternion_with_rotvec,
+)
+
+_P0 = (
+    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
+    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
 )
-from smsfusion._vectorops import _normalize, _quaternion_product, _skew_symmetric
-from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0
 
 
-def _roll_pitch_from_acc(f, nav_frame):
+def _state_transition_matrix_init(
+    dt: float,
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    gbc: float,
+) -> NDArray[np.float64]:
     """
-    Estimate roll and pitch angles from specific force (i.e., accelerometer) measurement.
+    State transition matrix.
 
     Parameters
     ----------
-    f: array-like
-        Specific force (i.e., acceleration) measurement vector (fx, fy, fz).
-    nav_frame: {'NED', 'ENU'}
-        Navigation frame. Should be either 'NED' or 'ENU'.
+    dt : float
+        Time step in seconds.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
+    gbc : float
+        Gyro bias correlation time in seconds.
 
     Returns
     -------
-    roll: float
-        Estimated roll angle in radians.
-    pitch: float
-        Estimated pitch angle in radians.
+    ndarray, shape (12, 12)
+        State transition matrix.
     """
+    phi = np.eye(12)
+    phi[0:3, 3:6] += dt * np.eye(3)
+    phi[3:6, 6:9] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
+    phi[6:9, 6:9] -= _skew_symmetric(dtheta)  # NB! update each time step
+    phi[6:9, 9:12] -= dt * np.eye(3)
+    phi[9:12, 9:12] -= dt * np.eye(3) / gbc
+    return phi
 
-    fx, fy, fz = f
-
-    if nav_frame.lower() == "ned":
-        roll = np.arctan2(-fy, -fz)
-        pitch = np.arctan2(fx, np.sqrt(fy**2 + fz**2))
-    elif nav_frame.lower() == "enu":
-        roll = np.arctan2(fy, fz)
-        pitch = -np.arctan2(fx, np.sqrt(fy**2 + fz**2))
-    else:
-        raise ValueError("Invalid navigation frame. Should be 'NED' or 'ENU'.")
 
-    return roll, pitch
-
-
-class FixedNED:
+@njit  # type: ignore[misc]
+def _state_transition_matrix_update(
+    phi: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+) -> None:
     """
-    Convert position coordinates between a fixed NED frame (x, y, z) and ECEF frame
-    (lattitude, longitude, height).
-
-    The fixed NED frame is a tangential plane on the WGS-84 ellipsoid with its origin
-    fixed at the provided reference coordinates. It is assumed that the tangential
-    plane is close to the ellipsoid surface.
+    Update the state transition matrix in place.
 
     Parameters
     ----------
-    lat_ref: float
-        Reference latitude coordinate in decimal degrees.
-    lon_ref: float
-        Reference longitude coordinate in decimal degrees.
-    height_ref: ref
-        Reference height coordinate in decimal degrees.
+    phi : ndarray, shape (12, 12)
+        State transition matrix to be updated in place.
+    dvel : ndarray, shape (3,)
+        Velocity increment measurement (sculling integral).
+    dtheta : ndarray, shape (3,)
+        Attitude increment measurement (coning integral).
+    R_nb : ndarray, shape (3, 3)
+        Rotation matrix from body to navigation frame.
     """
-
-    def __init__(self, lat_ref: float, lon_ref: float, height_ref: float) -> None:
-        self._lat_ref = lat_ref
-        self._lon_ref = lon_ref
-        self._height_ref = height_ref
-
-        radius_eq = 6_378_137  # equatorial radius (WGS-84)
-        radius_polar = 6_356_752.314245  # polar radius (WGS-84)
-
-        radius_ratio_squared = (radius_polar / radius_eq) ** 2
-        denom = np.cos(
-            self._lat_ref * (np.pi / 180.0)
-        ) ** 2 + radius_ratio_squared * np.sin(self._lat_ref * (np.pi / 180.0))
-
-        self._Rn = radius_eq / np.sqrt(denom)  # radius prime vertical
-        self._Rm = self._Rn * radius_ratio_squared / denom  # radius meridian
-
-        self._Rm_h = self._Rm + height_ref
-        self._Rn_h_cos = (self._Rn + height_ref) * np.cos(
-            self._lat_ref * (np.pi / 180.0)
-        )
-
-    def to_llh(self, x: float, y: float, z: float) -> tuple[float, float, float]:
-        """
-        Compute longitude, latitude, and height coordinates (WGS-84) from local
-        Cartesian coordinates in the fixed NED frame.
-
-        Parameters
-        ----------
-        x: float
-            Local x-coordinate in meters in the fixed NED frame.
-        y: float
-            Local y-coordinate in meters in the fixed NED frame.
-        z: float
-            Local z-coordinate in meters in the fixed NED frame.
-
-        Returns
-        -------
-        lat: float
-            Latitude coordinate in decimal degrees.
-        lon: float
-            Longitude coordinate in decimal degrees.
-        height: float
-            Height coordinate in meters.
-        """
-        dlat = (180.0 / np.pi) * x / self._Rm_h
-        dlon = (180.0 / np.pi) * y / self._Rn_h_cos
-
-        lat = _signed_smallest_angle(self._lat_ref + dlat, degrees=True)
-        lon = _signed_smallest_angle(self._lon_ref + dlon, degrees=True)
-        h = self._height_ref - z
-        return lat, lon, h
-
-    def to_xyz(
-        self, lat: float, lon: float, height: float
-    ) -> tuple[float, float, float]:
-        """
-        Compute local Cartesian coordinates in the fixed NED frame from longitude,
-        latitude, and height coordinates (WGS-84).
-
-        Parameters
-        ----------
-        lat: float
-            Latitude coordinate in decimal degrees.
-        lon: float
-            Longitude coordinate in decimal degrees.
-        height: float
-            Height coordinate in meters.
-
-        Returns
-        -------
-        x: float
-            Local x-coordinate in meters in the fixed NED frame.
-        y: float
-            Local y-coordinate in meters in the fixed NED frame.
-        z: float
-            Local z-coordinate in meters in the fixed NED frame.
-        """
-        dlat = lat - self._lat_ref
-        dlon = lon - self._lon_ref
-
-        x = (np.pi / 180.0) * dlat * self._Rm_h
-        y = (np.pi / 180.0) * dlon * self._Rn_h_cos
-        z = self._height_ref - height
-        return x, y, z
-
-
-def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
+    dtx, dty, dtz = dtheta
+    dvx, dvy, dvz = dvel
+
+    r00, r01, r02 = R_nb[0]
+    r10, r11, r12 = R_nb[1]
+    r20, r21, r22 = R_nb[2]
+
+    # phi[6:9, 6:9] = np.eye(3) - dt * S(w_b)
+    phi[6, 7] = dtz
+    phi[6, 8] = -dty
+    phi[7, 6] = -dtz
+    phi[7, 8] = dtx
+    phi[8, 6] = dty
+    phi[8, 7] = -dtx
+
+    # phi[3:6, 6:9] = -dt * R_nb @ S(f_b)
+    phi[3, 6] = -dvz * r01 + dvy * r02
+    phi[4, 6] = -dvz * r11 + dvy * r12
+    phi[5, 6] = -dvz * r21 + dvy * r22
+    phi[3, 7] = dvz * r00 - dvx * r02
+    phi[4, 7] = dvz * r10 - dvx * r12
+    phi[5, 7] = dvz * r20 - dvx * r22
+    phi[3, 8] = -dvy * r00 + dvx * r01
+    phi[4, 8] = -dvy * r10 + dvx * r11
+    phi[5, 8] = -dvy * r20 + dvx * r21
+
+
+def _process_noise_covariance_matrix(
+    dt: float, vrw: float, arw: float, gbs: float, gbc: float
+) -> NDArray[np.float64]:
     """
-    Convert the given angle to the smallest angle between [-180., 180) degrees.
+    Process noise covariance matrix.
 
     Parameters
     ----------
-    angle : float
-        Value of angle.
-    degrees : bool, default True
-        Specify whether ``angle`` is given degrees or radians.
+    dt : float
+        Time step in seconds.
+    vrw : float
+        Velocity random walk (accelerometer noise density) in m/s/√Hz.
+    arw : float
+        Angular random walk (gyroscope noise density) in rad/√Hz.
+    gbs : float
+        Gyro bias stability (bias instability) in rad/s.
+    gbc : float
+        Gyro bias correlation time in seconds.
 
     Returns
     -------
-    float
-        The smallest angle between [-180., 180) degrees (or  [-pi, pi] radians).
+    ndarray, shape (12, 12)
+        Process noise covariance matrix.
     """
-    base = 180.0 if degrees else np.pi
-    return (angle + base) % (2.0 * base) - base
+    Q = np.zeros((12, 12))
+    Q[3:6, 3:6] = dt * vrw**2 * np.eye(3)
+    Q[6:9, 6:9] = dt * arw**2 * np.eye(3)
+    Q[9:12, 9:12] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
+    return Q
 
 
-def gravity(lat: float | None = None, degrees: bool = True) -> float:
+def _measurement_matrix_init(
+    q_nb: NDArray[np.float64], lever_arm: NDArray[np.float64], nav_frame_factor: float
+) -> NDArray[np.float64]:
     """
-    Calculates the gravitational acceleration based on the World Geodetic System
-    (1984) Ellipsoidal Gravity Formula (WGS-84).
-
-    The WGS-84 formula is given by::
-
-        g = g_e * (1 - k * sin(lat)^2) / sqrt(1 - e^2 * sin(lat)^2)
-
-    where, ::
-
-        g_e = 9.780325335903891718546
-        k = 0.00193185265245827352087
-        e^2 = 0.006694379990141316996137
-
-    and ``lat`` is the latitude.
-
-    If no latitude is provided, the 'standard gravity', ``g_0``, is returned instead.
-    The standard gravity is by definition of the ISO/IEC 8000 given as
-    ``g_0 = 9.80665``.
+    Measurement matrix.
 
     Parameters
     ----------
-    lat : float, optional
-        Latitude. If none provided, the 'standard gravity' is returned.
-    degrees : bool, optional
-        Specify whether the latitude, ``lat``, is in degrees or radians.
-        Applicapble only if ``lat`` is provided.
-    """
-    if lat is None:
-        g_0 = 9.80665  # standard gravity in m/s^2
-        return g_0
-
-    g_e = 9.780325335903891718546  # gravity at equator
-    k = 0.00193185265245827352087  # formula constant
-    e_2 = 0.006694379990141316996137  # spheroid's squared eccentricity
-
-    if degrees:
-        lat = (np.pi / 180.0) * lat
-
-    g = g_e * (1.0 + k * np.sin(lat) ** 2.0) / np.sqrt(1.0 - e_2 * np.sin(lat) ** 2.0)
-    return g  # type: ignore[no-any-return]  # numpy funcs declare Any as return when given scalar-like
-
+    q_nb : ndarray, shape (4,)
+        Unit quaternion.
+    lever_arm : ndarray, shape(3,)
+        Lever-arm vector describing the location of position aiding (in meters) relative
+        to the IMU expressed in the IMU/body reference frame. For instance, the location
+        of the GNSS antenna relative to the IMU.
+    nav_frame_factor: float
+        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
+        -1.0 for 'ENU'.
 
-class INSMixin:
+    Returns
+    -------
+    ndarray, shape (7, 12)
+        Linearized measurement matrix.
     """
-    Mixin class for inertial navigation systems (INS).
+    vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
+    H = np.zeros((10, 12))
+    H[0:3, 0:3] = np.eye(3)  # position
+    H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
+    H[3:6, 3:6] = np.eye(3)  # velocity
+    H[6:7, 6:9] = _dhda_head(q_nb)  # heading
+    H[7:10, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
+    return H
 
-    Requires that the inheriting class has an `_x` attribute which is a 1D numpy array
-    of length 16 containing the following elements in order:
 
-        * Position in x, y, z directions (3 elements).
-        * Velocity in x, y, z directions (3 elements).
-        * Attitude as unit quaternion (4 elements).
-        * Accelerometer bias in x, y, z directions (3 elements).
-        * Gyroscope bias in x, y, z directions (3 elements).
+def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
     """
-
-    _x: NDArray[np.float64]  # state array of length 16
-
-    @property
-    def _pos(self) -> NDArray[np.float64]:
-        return self._x[0:3]
-
-    @_pos.setter
-    def _pos(self, p: ArrayLike) -> None:
-        self._x[0:3] = p
-
-    @property
-    def _vel(self) -> NDArray[np.float64]:
-        return self._x[3:6]
-
-    @_vel.setter
-    def _vel(self, v: ArrayLike) -> None:
-        self._x[3:6] = v
-
-    @property
-    def _q_nm(self) -> NDArray[np.float64]:
-        return self._x[6:10]
-
-    @_q_nm.setter
-    def _q_nm(self, q_nm: ArrayLike) -> None:
-        self._x[6:10] = q_nm
-
-    @property
-    def _bias_acc(self) -> NDArray[np.float64]:
-        return self._x[10:13]
-
-    @_bias_acc.setter
-    def _bias_acc(self, b_acc: ArrayLike) -> None:
-        self._x[10:13] = b_acc
-
-    @property
-    def _bias_gyro(self) -> NDArray[np.float64]:
-        return self._x[13:16]
-
-    @_bias_gyro.setter
-    def _bias_gyro(self, b_gyro: ArrayLike) -> None:
-        self._x[13:16] = b_gyro
-
-    @property
-    def x(self) -> NDArray[np.float64]:
-        """
-        Get current state vector estimate.
-
-        Returns
-        -------
-        numpy.ndarray, shape (16,)
-            State vector, containing the following elements in order:
-
-            * Position in x, y, z directions (3 elements).
-            * Velocity in x, y, z directions (3 elements).
-            * Attitude as unit quaternion (4 elements).
-            * Accelerometer bias in x, y, z directions (3 elements).
-            * Gyroscope bias in x, y, z directions (3 elements).
-        """
-        return self._x.copy()
-
-    def position(self) -> NDArray[np.float64]:
-        """
-        Get current position estimate.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Position state vector, containing position in x-, y-, and z-direction
-            (in that order).
-        """
-        return self._pos.copy()
-
-    def velocity(self) -> NDArray[np.float64]:
-        """
-        Get current velocity estimate.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Velocity state vector, containing (linear) velocity in x-, y-, and z-direction
-            (in that order).
-        """
-        return self._vel.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Get current attitude estimate as Euler angles (see Notes).
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the Euler angles in degrees or radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles, specifically: alpha (roll), beta (pitch) and gamma (yaw)
-            in that order.
-
-        Notes
-        -----
-        The Euler angles describe how to transition from the 'navigation' frame
-        ('NED' or 'ENU) to the 'body' frame through three consecutive intrinsic
-        and passive rotations in the ZYX order:
-
-        #. A rotation by an angle gamma (often called yaw) about the z-axis.
-        #. A subsequent rotation by an angle beta (often called pitch) about the y-axis.
-        #. A final rotation by an angle alpha (often called roll) about the x-axis.
-
-        This sequence of rotations is used to describe the orientation of the 'body' frame
-        relative to the 'navigation' frame ('NED' or 'ENU) in 3D space.
-
-        Intrinsic rotations mean that the rotations are with respect to the changing
-        coordinate system; as one rotation is applied, the next is about the axis of
-        the newly rotated system.
-
-        Passive rotations mean that the frame itself is rotating, not the object
-        within the frame.
-        """
-        q = self.quaternion()
-        theta = _euler_from_quaternion(q)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta  # type: ignore[no-any-return]
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Get current attitude estimate as unit quaternion (from-body-to-navigation-frame).
-
-        Returns
-        -------
-        numpy.ndarray, shape (4,)
-            Attitude as unit quaternion. Given as ``[q1, q2, q3, q4]``, where
-            ``q1`` is the real part and ``q2``, ``q3`` and ``q4`` are the three
-            imaginary parts.
-        """
-        return self._q_nm.copy()
-
-    def bias_acc(self) -> NDArray[np.float64]:
-        """
-        Get current accelerometer bias estimate.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Accelerometer bias vector, containing biases in x-, y-, and z-direction
-            (in that order).
-        """
-        return self._bias_acc.copy()
-
-    def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Get current gyroscope bias estimate.
-
-        Parameters
-        ----------
-        degrees : bool, default False
-            Whether to return the bias in deg/s or rad/s.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Gyroscope bias vector, containing biases in x-, y-, and z-direction
-            (in that order).
-        """
-        b_gyro = self._bias_gyro.copy()
-        if degrees:
-            b_gyro = (180.0 / np.pi) * b_gyro
-        return b_gyro
-
-
-class StrapdownINS(INSMixin):
-    """
-    Strapdown inertial navigation system (INS).
-
-    This class provides an interface for estimating position, velocity and attitude
-    of a moving body by integrating the *strapdown navigation equations*.
+    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
 
     Parameters
     ----------
-    fs : float
-        Sampling rate in Hz.
-    x0 : array-like, shape (16,)
-        Initial state vector containing the following elements in order:
-
-        * Position in x, y, z directions (3 elements).
-        * Velocity in x, y, z directions (3 elements).
-        * Attitude as unit quaternion (4 elements).
-        * Accelerometer bias in x, y, z directions (3 elements).
-        * Gyroscope bias in x, y, z directions (3 elements).
-    g : float, default 9.80665
-        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
+    g : float
+        Gravitational acceleration in m/s^2.
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame in which the gravity vector is expressed.
 
-    Notes
-    -----
-    The quaternion provided as part of the initial state will be normalized to
-    ensure unity.
+    Returns
+    -------
+    ndarray, shape (3,)
+        Gravity vector expressed in the navigation frame.
     """
-
-    def __init__(
-        self, fs: float, x0: ArrayLike, g: float = 9.80665, nav_frame="NED"
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-
-        self._x0 = np.asarray_chkfinite(x0).reshape(16).copy()
-        self._x0[6:10] = _normalize(self._x0[6:10])
-        self._x = self._x0.copy()
-        self._g = g
-        self._nav_frame = nav_frame.lower()
-
-        if self._nav_frame == "ned":
-            self._g_n = np.array([0.0, 0.0, g])
-        elif self._nav_frame == "enu":
-            self._g_n = np.array([0.0, 0.0, -g])
-        else:
-            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
-
-    def reset(self, x_new: ArrayLike) -> None:
-        """
-        Reset current state with a new one.
-
-        Parameters
-        ----------
-        x_new : numpy.ndarray, shape (10,)
-            New state vector, containing the following elements in order:
-
-            * Position in x-, y-, and z-direction (3 elements).
-            * Velocity in x-, y-, and z-direction (3 elements).
-            * Attitude as unit quaternion (4 elements). Should be given as
-              [q1, q2, q3, q4], where q1 is the real part and q1, q2 and q3
-              are the three imaginary parts.
-
-        Notes
-        -----
-        The quaternion provided as part of the new state will be normalized to
-        ensure unity.
-        """
-        self._x = np.asarray_chkfinite(x_new).reshape(16).copy()
-        self._x[6:10] = _normalize(self._x[6:10])
-
-    def update(
-        self,
-        f_imu: ArrayLike,
-        w_imu: ArrayLike,
-        degrees: bool = False,
-    ) -> Self:
-        """
-        Update the INS states by integrating the *strapdown navigation equations*.
-
-        Assuming constant inputs (i.e., accelerations and angular velocities) over
-        the sampling period.
-
-        The states are updated according to::
-
-            p[k+1] = p[k] + h * v[k] + 0.5 * dt * a[k]
-
-            v[k+1] = v[k] + dt * a[k]
-
-            q[k+1] = q[k] + dt * T(q[k]) * w_ins[k]
-
-        with bias compensated IMU measurements::
-
-            f_ins[k] = f_imu[k] - b_acc[k]
-
-            w_ins[k] = w_imu[k] - b_gyro[k]
-
-        and::
-
-            a[k] = R(q[k]) * f_ins[k] + g
-
-            g = [0, 0, 9.81]^T
-
-        Parameters
-        ----------
-        f_imu : array-like, shape (3,)
-            Specific force measurements (i.e., accelerations + gravity), given
-            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
-            acceleration measurements in x-, y-, and z-direction, respectively.
-        w_imu : array-like, shape (3,)
-            Angular rate measurements, given as [w_x, w_y, w_z]^T where
-            w_x, w_y and w_z are angular rates about the x-, y-,
-            and z-axis, respectively.
-        degrees : bool, default False
-            Specify whether the angular rates are given in degrees or radians.
-
-        Returns
-        -------
-        StrapdownINS :
-            A reference to the instance itself after the update.
-        """
-        f_imu = np.asarray(f_imu, dtype=float)
-        w_imu = np.asarray(w_imu, dtype=float)
-
-        if degrees:
-            w_imu = (np.pi / 180.0) * w_imu
-
-        # Bias compensated IMU measurements
-        f_ins = f_imu - self._bias_acc
-        w_ins = w_imu - self._bias_gyro
-
-        q_nm = self._q_nm
-        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned
-        T = _angular_matrix_from_quaternion(q_nm)
-
-        # State propagation (assuming constant linear acceleration and angular velocity)
-        acc = R_nm @ f_ins + self._g_n
-        self._pos = self._pos + self._dt * self._vel
-        self._vel = self._vel + self._dt * acc
-        q_nm = q_nm + self._dt * T @ w_ins
-        self._q_nm = _normalize(q_nm)
-
-        return self
+    if nav_frame.lower() == "ned":
+        g_n = np.array([0.0, 0.0, g])
+    elif nav_frame.lower() == "enu":
+        g_n = np.array([0.0, 0.0, -g])
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+    return g_n
 
 
-def _h_head(q: NDArray[np.float64]) -> float:
+@njit  # type: ignore[misc]
+def _reset(
+    dx: NDArray[np.float64],
+    p_n: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    bg_b: NDArray[np.float64],
+) -> None:
     """
-    Compute yaw angle from unit quaternion.
-
-    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
-    unit quaternion here to avoid singularities.
+    Reset state (in place).
 
     Parameters
     ----------
-    q : numpy.ndarray, shape (4,)
-        Unit quaternion.
-
-    Returns
-    -------
-    float
-        Yaw angle in the NED reference frame.
-
-    References
-    ----------
-    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
-    2nd Edition, equation 14.251, John Wiley & Sons, 2021.
+    p_n : ndarray, shape (3,)
+        Position state estimate to be reset in place.
+    v_n : ndarray, shape (3,)
+        Velocity state estimate to be reset in place.
+    q_nb : ndarray, shape (4,)
+        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,)
+        Error state vector containing the corrections to be applied to the state
+        estimates. Will be reset to zero after applying the corrections.
     """
-    q_w, q_x, q_y, q_z = q
-    u_y = 2.0 * (q_x * q_y + q_z * q_w)
-    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
-    return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
+    p_n[:] += dx[0:3]
+    v_n[:] += dx[3:6]
+    _update_quaternion_with_gibbs2(q_nb, dx[6:9])  # -> update q_nb (in place)
+    bg_b[:] += dx[9:12]
+    dx[:] = 0.0
 
 
 @njit  # type: ignore[misc]
-def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
+def _project_state_ahead(
+    p_n: NDArray[np.float64],
+    v_n: NDArray[np.float64],
+    q_nb: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    dvel: NDArray[np.float64],
+    dtheta: NDArray[np.float64],
+    dt: float,
+    dvel_g_corr: NDArray[np.float64],
+) -> None:
     """
-    Compute yaw angle gradient wrt to the unit quaternion.
-
-    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
-    unit quaternion here to avoid singularities.
-
-    Parameters
-    ----------
-    q : numpy.ndarray, shape (3,)
-        Unit quaternion.
-
-    Returns
-    -------
-    numpy.ndarray, shape (3,)
-        Yaw angle gradient vector.
+    Project state estimates ahead (in place).
 
     References
     ----------
-    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
-    2nd Edition, equation 14.254, John Wiley & Sons, 2021.
+    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 3-5)
     """
-    q_w, q_x, q_y, q_z = q
-    u_y = 2.0 * (q_x * q_y + q_z * q_w)
-    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
-    u = u_y / u_x
+    dvel_corr = R_nb @ dvel + dvel_g_corr
+    p_n[:] += dt * v_n + 0.5 * dt * dvel_corr
+    v_n[:] += dvel_corr
+    _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
 
-    duda_scale = 1.0 / u_x**2
-    duda_x = -(q_w * q_y) * (1.0 - 2.0 * q_w**2) - (2.0 * q_w**2 * q_x * q_z)
-    duda_y = (q_w * q_x) * (1.0 - 2.0 * q_z**2) + (2.0 * q_w**2 * q_y * q_z)
-    duda_z = q_w**2 * (1.0 - 2.0 * q_y**2) + (2.0 * q_w * q_x * q_y * q_z)
-    duda = duda_scale * np.array([duda_x, duda_y, duda_z])
 
-    dhda = 1.0 / (1.0 + u**2) * duda
-
-    return dhda  # type: ignore[no-any-return]
-
-
-class AidedINS(INSMixin):
+class AINS:
     """
-    Aided inertial navigation system (AINS) using a multiplicative extended
-    Kalman filter (MEKF).
+    Aided inertial navigation system (AINS).
+
+    This class provides position, velocity, attitude and gyro bias estimation using
+    a multiplicative extended Kalman filter (MEKF).
 
     Parameters
     ----------
     fs : float
         Sampling rate in Hz.
-    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
-        Initial (a priori) 16-element INS state estimate:
-
-        * Position (x, y, z) - 3 elements
-        * Velocity (x, y, z) - 3 elements
-        * Attitude (unit quaternion) - 4 elements
-        * Accelerometer bias (x, y, z) - 3 elements
-        * Gyroscope bias (x, y, z) - 3 elements
-
-        Defaults to a zero vector, but with the attitude part as a unit quaternion
-        (i.e., no rotation).
-    P0_prior : array-like (shape (12, 12) or (15, 15)), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
-        Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a
-        small diagonal matrix will be used. If the accelerometer bias is excluded from the
-        error estimate (see ``ignore_bias_acc``), the covariance matrix should be of shape
-        (12, 12), otherwise (15, 15).
-    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
-        Dictionary containing accelerometer noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
-        * ``B``: Bias stability in m/s^2.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
-        Dictionary containing gyroscope noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
-        * ``B``: Bias stability in rad/s.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    g : float, default 9.80665
-        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
+    p0 : array_like, shape (3,), optional
+        Initial position estimate in m. Defaults to origin (0.0, 0.0, 0.0).
+    v0 : array_like, shape (3,), optional
+        Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
+    q0 : array_like, shape (4,), optional
+        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
+        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
+    bg0 : array_like, shape (3,), optional
+        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
+    P0 : array_like, shape (12, 12), optional
+        Initial (a priori) estimate of the error covariance matrix. Defaults to
+        a small diagonal matrix (1e-6 * np.eye(12)).
+    acc_noise_density : float, optional
+        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
+        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
+    gyro_noise_density : float, optional
+        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
+        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
+    gyro_bias_stability : float, optional
+        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
+        noise level).
+    gyro_bias_corr_time : float, optional
+        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
+    g : float, optional
+        The gravitational acceleration in m/s^2. Default is 'standard gravity' of
+        9.80665 m/s^2.
+    nav_frame : {'NED', 'ENU'}, optional
         Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
         (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame. Furthermore, the aiding heading angle is
-        also interpreted relative to this frame according to the right-hand rule.
-    lever_arm : array-like, shape (3,), default numpy.zeros(3)
+        will be expressed relative to this frame.
+    lever_arm : array-like, shape (3,), optional
         Lever-arm vector describing the location of position aiding (in meters) relative
-        to the IMU expressed in the IMU's measurement frame. For instance, the location
+        to the IMU expressed in the IMU/body reference frame. For instance, the location
         of the GNSS antenna relative to the IMU. By default it is assumed that the
         aiding position coincides with the IMU's origin.
-    ignore_bias_acc : bool, default True
-        Determines whether the accelerometer bias should be included in the error estimate.
-        If set to ``True``, the accelerometer bias provided in ``x0`` during initialization
-        will remain fixed and not updated. This option is useful in situations where the
-        accelerometer bias is unobservable, such as when there is insufficient aiding
-        information or minimal dynamic motion, making bias estimation unreliable. Note
-        that this will reduce the error-state dimension from 15 to 12, and hence also the
-        error covariance matrix, **P**, from dimension (15, 15) to (12, 12). When set to
-        ``False``, the P0_prior argument must have shape (15, 15).
-    cold_start : bool, default True
-        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
-        A cold state indicates that the provided initial conditions are uncertain,
-        and possibly far from the true state. Thus, to reduce the risk of divergence,
-        an initial vertical alignment (i.e., roll and pitch calibration) is performed
-        using accelerometer measurements and the known direction of gravity during
-        the first measurement update. The IMU should remain stationary with negligible
-        linear acceleration during a cold start; otherwise, divergence may occur.
-        A warm start, on the other hand, assumes accurate initial conditions, and
-        initializes the Kalman filter immediately without any initial roll and pitch
-        calibration.
     """
 
-    # Permutation matrix for reordering error-state bias terms, such that:
-    # [pos, vel, quat, b_gyro, b_acc]^T = T_dx @ [pos, vel, quat, b_acc, b_gyro]^T
-    _T_dx = np.zeros((15, 15))
-    _T_dx[:9, :9] = np.eye(9)
-    _T_dx[9:12, 12:15] = np.eye(3)
-    _T_dx[12:15, 9:12] = np.eye(3)
-
-    # Permutation matrix for reordering white noise bias terms, such that:
-    # [acc, gyro, b_gyro, b_acc]^T = T_wn @ [acc, gyro, b_acc, b_gyro]^T
-    _T_wn = np.zeros((12, 12))
-    _T_wn[:6, :6] = np.eye(6)
-    _T_wn[6:9, 9:12] = np.eye(3)
-    _T_wn[9:12, 6:9] = np.eye(3)
-
     def __init__(
         self,
         fs: float,
-        x0_prior: ArrayLike = X0,
-        P0_prior: ArrayLike = P0,
-        err_acc: dict[str, float] = ERR_ACC_MOTION2,
-        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
+        p0: ArrayLike = (0.0, 0.0, 0.0),
+        v0: ArrayLike = (0.0, 0.0, 0.0),
+        q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
+        bg0: ArrayLike = (0.0, 0.0, 0.0),
+        P0: ArrayLike = _P0,
+        acc_noise_density: float = 0.0007,
+        gyro_noise_density: float = 0.00005,
+        gyro_bias_stability: float = 0.00005,
+        gyro_bias_corr_time: float = 50.0,
         g: float = 9.80665,
         nav_frame: str = "NED",
-        lever_arm: ArrayLike = np.zeros(3),
-        ignore_bias_acc: bool = True,
-        cold_start: bool = True,
+        lever_arm: ArrayLike = (0.0, 0.0, 0.0),
     ) -> None:
         self._fs = fs
         self._dt = 1.0 / fs
-        self._err_acc = err_acc
-        self._err_gyro = err_gyro
+        self._nav_frame = nav_frame.lower()
+        self._nz2vg = _nz2vg(self._nav_frame)
+        self._g = g
+        self._g_n = _gravity_nav(self._g, self._nav_frame)
+        self._dvel_g_corr = self._dt * self._g_n
         self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy()
-        self._ignore_bias_acc = ignore_bias_acc
-        self._cold = cold_start
-        self._dq_prealloc = np.array([2.0, 0.0, 0.0, 0.0])  # Preallocation
-
-        # Strapdown algorithm / INS state
-        self._ins = StrapdownINS(self._fs, x0_prior, g=g, nav_frame=nav_frame)
-        self._vg_ref_n = _normalize(self._ins._g_n)  # gravity reference vector
-
-        # Total state estimate
-        self._x = self._ins.x
 
-        # Error state estimate (after reset)
-        self._dx_prealloc = np.zeros(15)  # always zero, but used in sequential update
-
-        # Initialize Kalman filter
-        self._P_prior = np.asarray_chkfinite(P0_prior).copy(order="C")
-        self._P = self._P_prior.copy(order="C")
-
-        # Verify error covariance matrix shape
-        if ignore_bias_acc and self._P_prior.shape != (12, 12):
-            raise ValueError(
-                f"P0_prior must be of shape (12, 12) when ignore_bias_acc is set to True. Was {self._P_prior.shape}."
-            )
-        if not ignore_bias_acc and self._P_prior.shape != (15, 15):
-            raise ValueError(
-                f"P0_prior must be of shape (15, 15) when ignore_bias_acc is set to False. Was {self._P_prior.shape}."
-            )
-
-        # Prepare system matrices
-        q0 = self._ins._q_nm
-        self._F = self._prep_F(err_acc, err_gyro, q0)
-        self._G = self._prep_G(q0)
-        self._H = self._prep_H()
-        self._W = self._prep_W(err_acc, err_gyro)
-        self._I = np.eye(15, order="C")
-
-        # Filter out the accelerometer bias terms from the system matrices (if ignored)
-        if self._ignore_bias_acc:
-            dx_dim = 12
-            wn_dim = 9
-            self._F = (self._T_dx @ self._F @ self._T_dx.T)[:dx_dim, :dx_dim]
-            self._G = (self._T_dx @ self._G @ self._T_wn)[:dx_dim, :wn_dim]
-            self._H = (self._H @ self._T_dx)[:, :dx_dim]
-            self._W = (self._T_wn @ self._W @ self._T_wn.T)[:wn_dim, :wn_dim]
-            self._I = self._I[:dx_dim, :dx_dim]
-            self._dx_prealloc = self._dx_prealloc[:dx_dim]
-
-        # Error-state estimate (before reset)
-        self._dx = np.empty_like(self._dx_prealloc)  # needed for smoothing only
-
-        # State transition matrix
-        self._phi = np.empty_like(self._F)  # needed for smoothing only
+        # IMU noise parameters
+        self._vrw = acc_noise_density  # velocity random walk
+        self._arw = gyro_noise_density  # angular random walk
+        self._gbs = gyro_bias_stability  # gyro bias stability
+        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
+
+        # State and covariance estimates
+        self._p_n = np.asarray_chkfinite(p0).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(v0).reshape(3).copy()
+        self._q_nb = np.asarray_chkfinite(q0).reshape(4).copy()
+        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)
+
+        # Discrete state-space model
+        self._phi = _state_transition_matrix_init(
+            self._dt,
+            np.zeros(3),
+            np.zeros(3),
+            _rot_matrix_from_quaternion(self._q_nb),
+            self._gbc,
+        )
+        self._Q = _process_noise_covariance_matrix(
+            self._dt, self._vrw, self._arw, self._gbs, self._gbc
+        )
+        self._H = _measurement_matrix_init(self._q_nb, self._lever_arm, self._nz2vg)
 
-    @property
-    def x_prior(self) -> NDArray[np.float64]:
+    def position(self) -> NDArray[np.float64]:
         """
-        Next a priori state vector estimate.
-
-        Returns
-        -------
-        numpy.ndarray, shape (16,)
-            A priori state vector estimate, containing the following elements in order:
-
-            * Position in x, y, z directions (3 elements).
-            * Velocity in x, y, z directions (3 elements).
-            * Attitude as unit quaternion (4 elements).
-            * Accelerometer bias in x, y, z directions (3 elements).
-            * Gyroscope bias in x, y, z directions (3 elements).
+        Copy of the position estimate in meters.
         """
-        return self._ins.x
+        return self._p_n.copy()
 
-    def dump(
-        self,
-    ) -> dict[str, np.float64 | list[np.float64] | dict[str, np.float64] | bool]:
+    def velocity(self) -> NDArray[np.float64]:
         """
-        Dump the configuration and current state of the AINS to a dictionary. The dumped
-        parameters can be used to restore the AINS to its current state.
-
-        Returns
-        -------
-        dict
-            A dictionary containing the configuration and current state of the AINS.
+        Copy of the velocity estimate in m/s.
         """
-        params = {
-            "fs": self._fs,
-            "x0_prior": self.x_prior.tolist(),
-            "P0_prior": self.P_prior.tolist(),
-            "err_acc": self._err_acc,
-            "err_gyro": self._err_gyro,
-            "g": self._ins._g,
-            "nav_frame": self._ins._nav_frame,
-            "lever_arm": self._lever_arm.tolist(),
-            "ignore_bias_acc": self._ignore_bias_acc,
-            "cold_start": self._cold,
-        }
-        return params
+        return self._v_n.copy()
 
-    @property
-    def P(self) -> NDArray[np.float64]:
+    def quaternion(self) -> NDArray[np.float64]:
         """
-        Error covariance matrix, **P**. I.e., the error covariance matrix associated with
-        the Kalman filter's updated (a posteriori) error-state estimate.
+        Copy of the attitude estimate expressed as a unit quaternion.
         """
-        P = self._P.copy()
-        return P
+        return self._q_nb.copy()
 
-    @property
-    def P_prior(self) -> NDArray[np.float64]:
-        """
-        Next (a priori) estimate of the error covariance matrix, **P**. I.e., the error
-        covariance matrix associated with the Kalman filter's projected (a priori)
-        error-state estimate.
-        """
-        P_prior = self._P_prior.copy()
-        return P_prior
-
-    @staticmethod
-    def _prep_F(
-        err_acc: dict[str, float],
-        err_gyro: dict[str, float],
-        q_nm: NDArray[np.float64],
-    ) -> NDArray[np.float64]:
-        """
-        Prepare linearized state matrix, F.
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
+        Copy of the attitude estimate expressed as Euler angles (roll, pitch, yaw).
 
-        beta_acc = 1.0 / err_acc["tau_cb"]
-        beta_gyro = 1.0 / err_gyro["tau_cb"]
+        Parameters
+        ----------
+        degrees : bool, optional
+            Whether to return the Euler angles in degrees or radians. Defaults to
+            radians.
 
-        # Temporary placeholder vectors (to be replaced each timestep)
-        f_ins = np.array([0.0, 0.0, 0.0])
-        w_ins = np.array([0.0, 0.0, 0.0])
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles (roll, pitch, yaw).
+        """
 
-        S = _skew_symmetric  # alias skew symmetric matrix
-        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned rotation matrix
+        theta = _euler_from_quaternion(self._q_nb)
 
-        # State transition matrix
-        F = np.zeros((15, 15))
-        F[0:3, 3:6] = np.eye(3)
-        F[3:6, 6:9] = -R_nm @ S(f_ins)  # NB! update each time step
-        F[3:6, 9:12] = -R_nm  # NB! update each time step
-        F[6:9, 6:9] = -S(w_ins)  # NB! update each time step
-        F[6:9, 12:15] = -np.eye(3)
-        F[9:12, 9:12] = -beta_acc * np.eye(3)
-        F[12:15, 12:15] = -beta_gyro * np.eye(3)
+        if degrees:
+            theta = (180.0 / np.pi) * theta
 
-        return F
+        return theta
 
-    def _update_F(
-        self,
-        R_nm: NDArray[np.float64],
-        f_ins: NDArray[np.float64],
-        w_ins: NDArray[np.float64],
-    ) -> None:
-        """Update linearized state transition matrix, F."""
-        S = _skew_symmetric  # alias skew symmetric matrix
-
-        # Update matrix
-        self._F[3:6, 6:9] = -R_nm @ S(f_ins)  # NB! update each time step
-        self._F[6:9, 6:9] = -S(w_ins)  # NB! update each time step
-        if not self._ignore_bias_acc:
-            self._F[3:6, 9:12] = -R_nm  # NB! update each time step
-
-    @staticmethod
-    def _prep_G(q_nm: NDArray[np.float64]) -> NDArray[np.float64]:
-        """Prepare (white noise) input matrix, G."""
-        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned rotation matrix
-
-        # Input (white noise) matrix
-        G = np.zeros((15, 12))
-        G[3:6, 0:3] = -R_nm  # NB! update each time step
-        G[6:9, 3:6] = -np.eye(3)
-        G[9:12, 6:9] = np.eye(3)
-        G[12:15, 9:12] = np.eye(3)
-        return G
-
-    def _update_G(self, R_nm: NDArray[np.float64]) -> None:
-        """Update (white noise) input matrix, G."""
-
-        # Update matrix
-        self._G[3:6, 0:3] = -R_nm
-
-    @staticmethod
-    def _prep_H() -> NDArray[np.float64]:
-        """Prepare linearized measurement matrix, H. Values are placeholders only"""
-        H = np.zeros((10, 15))
-        H[0:3, 0:3] = np.eye(3)  # position
-        H[3:6, 3:6] = np.eye(3)  # velocity
-        return H
-
-    def _update_H_pos(
-        self, R_nm: NDArray[np.float64], lever_arm: NDArray[np.float64]
-    ) -> NDArray[np.float64]:
-        """Update and return part of H matrix relevant for position aiding."""
-        S = _skew_symmetric
-        self._H[0:3, 6:9] = -R_nm @ S(lever_arm)
-        return self._H[0:3]
-
-    def _update_H_vel(self) -> NDArray[np.float64]:
-        """Update and return part of H matrix relevant for velocity aiding."""
-        return self._H[3:6]
-
-    def _update_H_g_ref(self, R_nm: NDArray[np.float64]) -> NDArray[np.float64]:
-        """Update and return part of H matrix relevant for g_ref aiding."""
-        S = _skew_symmetric
-        self._H[6:9, 6:9] = S(R_nm.T @ self._vg_ref_n)
-        return self._H[6:9]
-
-    def _update_H_head(self, q_nm: NDArray[np.float64]) -> NDArray[np.float64]:
-        """Update and return part of H matrix relevant for heading aiding."""
-        self._H[9:10, 6:9] = _dhda_head(q_nm)
-        return self._H[9:10]
-
-    @staticmethod
-    def _prep_W(
-        err_acc: dict[str, float], err_gyro: dict[str, float]
-    ) -> NDArray[np.float64]:
-        """Prepare white noise power spectral density matrix"""
-        N_acc = err_acc["N"]
-        sigma_acc = err_acc["B"]
-        beta_acc = 1.0 / err_acc["tau_cb"]
-        N_gyro = err_gyro["N"]
-        sigma_gyro = err_gyro["B"]
-        beta_gyro = 1.0 / err_gyro["tau_cb"]
-
-        # White noise power spectral density matrix
-        W = np.eye(12)
-        W[0:3, 0:3] *= N_acc**2
-        W[3:6, 3:6] *= N_gyro**2
-        W[6:9, 6:9] *= 2.0 * sigma_acc**2 * beta_acc
-        W[9:12, 9:12] *= 2.0 * sigma_gyro**2 * beta_gyro
-        return W
-
-    def _reset_ins(self, dx: NDArray[np.float64]) -> None:
-        """Combine states and reset INS"""
-        da = dx[6:9]
-        self._dq_prealloc[1:4] = da
-        dq = (1.0 / np.sqrt(4.0 + da.T @ da)) * self._dq_prealloc
-        self._ins._x[:3] = self._ins._x[:3] + dx[:3]
-        self._ins._x[3:6] = self._ins._x[3:6] + dx[3:6]
-        self._ins._x[6:10] = _quaternion_product(self._ins._x[6:10], dq)
-        self._ins._x[6:10] = _normalize(self._ins._x[6:10])
-        self._ins._x[-3:] = self._ins._x[-3:] + dx[-3:]
-        if not self._ignore_bias_acc:
-            self._ins._x[10:13] = self._ins._x[10:13] + dx[9:12]
-        self._dx_prealloc[:] = np.zeros(dx.size)
-
-    @staticmethod
-    @njit  # type: ignore[misc]
-    def _update_dx_P(
-        dx: NDArray[np.float64],
-        P: NDArray[np.float64],
-        dz: NDArray[np.float64],
-        var: NDArray[np.float64],
-        H: NDArray[np.float64],
-        I_: NDArray[np.float64],
-    ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
-        for i, (dz_i, var_i) in enumerate(zip(dz, var)):
-            H_i = np.ascontiguousarray(H[i, :])
-            K_i = P @ H_i.T / (H_i @ P @ H_i.T + var_i)
-            dx += K_i * (dz_i - H_i @ dx)
-            K_i = np.ascontiguousarray(K_i[:, np.newaxis])  # as 2D array
-            H_i = np.ascontiguousarray(H_i[np.newaxis, :])  # as 2D array
-            P = (I_ - K_i @ H_i) @ P @ (I_ - K_i @ H_i).T + var_i * K_i @ K_i.T
-        return dx, P
-
-    def _align_vertical(self, f_ins, head, head_degrees):
+    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
         """
-        Vertical alignment.
-
-        Estimate the attitude (roll and pitch) of the IMU sensor relative to the
-        navigation frame using accelerometer measurements and the known direction
-        of gravity. Assumes a static sensor; i.e., negligible linear acceleration.
+        Copy of the gyroscope bias estimate in rad/s or deg/s depending on the
+        ``degrees`` flag.
 
         Parameters
         ----------
-        f_ins : array-like, shape (3,)
-            Bias-compensated specific force measurements (fx, fy, fz).
-        head : float, optional
-            Heading of measurement frame relative to navigation frame.
-        head_degrees : bool, default False
-            Specifies whether the heading is given in degrees or radians.
+        degrees : bool, optional
+            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
         """
-        if head is None:
-            head = _h_head(self.quaternion())
-        else:
-            if head_degrees:
-                head = (np.pi / 180.0) * head
+        bg_b = self._bg_b.copy()
+        if degrees:
+            bg_b = (180.0 / np.pi) * bg_b
+        return bg_b
 
-        roll, pitch = _roll_pitch_from_acc(f_ins, self._ins._nav_frame)
-        self._ins._x[6:10] = _quaternion_from_euler(np.array([roll, pitch, head]))
-        self._x[:] = self._ins._x
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Copy of the error covariance matrix estimate.
+        """
+        return self._P.copy()
 
     def update(
         self,
-        f_imu: ArrayLike,
-        w_imu: ArrayLike,
+        dvel: ArrayLike,
+        dtheta: ArrayLike,
         degrees: bool = False,
         pos: ArrayLike | None = None,
         pos_var: ArrayLike | None = None,
@@ -1031,471 +442,153 @@ def update(
         vel_var: ArrayLike | None = None,
         head: float | None = None,
         head_var: float | None = None,
-        head_degrees: bool = True,
-        g_ref: bool = False,
-        g_var: ArrayLike | None = None,
+        head_degrees: bool = False,
+        gref: bool = False,
+        gref_var: ArrayLike | None = None,
     ) -> Self:
         """
-        Update/correct the AINS' state estimate with aiding measurements, and project
-        ahead using IMU measurements.
-
-        If no aiding measurements are provided, the AINS is simply propagated ahead
-        using dead reckoning with the IMU measurements.
+        Update state estimates with IMU and aiding measurements.
 
         Parameters
         ----------
-        f_imu : array-like, shape (3,)
-            Specific force measurements (i.e., accelerations + gravity), given
-            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
-            acceleration measurements in x-, y-, and z-direction, respectively.
-        w_imu : array-like, shape (3,)
-            Angular rate measurements, given as [w_x, w_y, w_z]^T where
-            w_x, w_y and w_z are angular rates about the x-, y-,
-            and z-axis, respectively.
-        degrees : bool, default False
-            Specifies whether the unit of ``w_imu`` are in degrees or radians.
+        dvel : array_like, shape (3,)
+            Velocity increment (sculling integral) in m/s.
+        dtheta : array_like, shape (3,)
+            Attitude increment (coning integral) in radians or degrees depending
+            on the ``degrees`` flag.
+        degrees : bool, optional
+            Specifies whether the unit of the attitude increment, ``dtheta``, is
+            degrees or radians. Defaults to radians.
         pos : array-like, shape (3,), optional
-            Position aiding measurement in m. If ``None``, position aiding is not used.
+            Position aiding measurement in meters. If ``None``, position aiding
+            is not used.
         pos_var : array-like, shape (3,), optional
-            Variance of position measurement noise in m^2. Required for ``pos``.
+            Variance of position measurement noise in m^2. Ignored if ``pos`` is
+            ``None``.
         vel : array-like, shape (3,), optional
-            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is
+            not used.
         vel_var : array-like, shape (3,), optional
-            Variance of velocity measurement noise in (m/s)^2. Required for ``vel``.
+            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
+            is ``None``.
         head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+            Heading measurement in radians or degrees depending on the ``head_degrees``
+            flag. I.e., the yaw angle of the 'body' frame relative to the assumed
+            'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used.
         head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
+            Variance of heading measurement noise in radians^2 or degrees^2 depending
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
         head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
-        g_ref : bool, optional, default False
-            Specifies whether the gravity reference vector is used as an aiding measurement.
-        g_var : array-like, shape (3,), optional
-            Variance of gravitational reference vector measurement noise. Required for
-            ``g_ref``.
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees
+            and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
+        gref : bool, optional
+            Specifies whether to use accelerometer measurements (dvel) and the known
+            direction of gravity as aiding. Defaults to ``False``.
+        gref_var : array_like, shape (3,), optional
+            Variance of gravity reference vector measurement noise (dimensionless).
+            Required for gravity reference vector aiding.
 
         Returns
         -------
-        AidedINS
+        AINS
             A reference to the instance itself after the update.
         """
 
-        f_imu = np.asarray(f_imu, dtype=float)
-        w_imu = np.asarray(w_imu, dtype=float)
+        dvel = np.asarray(dvel)
+        dtheta = np.asarray(dtheta)
 
         if degrees:
-            w_imu = (np.pi / 180.0) * w_imu
-
-        # Bias compensated IMU measurements
-        f_ins = f_imu - self._ins._bias_acc
-        w_ins = w_imu - self._ins._bias_gyro
-
-        # Initial vertical alignment (i.e., roll and pitch calibration)
-        if self._cold:
-            self._align_vertical(f_ins, head, head_degrees)
-            self._cold = False
-
-        # Current INS state estimates
-        pos_ins = self._ins._pos
-        vel_ins = self._ins._vel
-        q_ins_nm = self._ins._q_nm
-        R_ins_nm = _rot_matrix_from_quaternion(q_ins_nm)  # body-to-inertial rot matrix
-
-        # Aliases
-        dx = self._dx_prealloc  # zeros
-        dt = self._dt
-        F = self._F
-        G = self._G
-        W = self._W
-        P = self._P_prior
-        I_ = self._I
-
-        # Lever arm vector - IMU-to-aiding
-        lever_arm = self._lever_arm
-
-        # Update system matrices
-        self._update_F(R_ins_nm, f_ins, w_ins)
-        self._update_G(R_ins_nm)
-
-        # Update with available aiding measurements
+            dtheta = (np.pi / 180.0) * dtheta
+
+        dtheta = 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
+
+        # 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._dt,
+            self._dvel_g_corr,
+        )
+
+        # Project (a priori) error covariance matrix estimate ahead
+        _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
+
         if pos is not None:
             if pos_var is None:
-                raise ValueError("'pos_var' not provided.")
-
-            pos = np.asarray(pos, dtype=float, order="C")
-            pos_var = np.asarray(pos_var, dtype=float, order="C")
-            dz_pos = pos - pos_ins - R_ins_nm @ lever_arm
-            H_pos = self._update_H_pos(R_ins_nm, lever_arm)
-            dx, P = self._update_dx_P(dx, P, dz_pos, pos_var, H_pos, I_)
+                raise ValueError("'pos_var' is required for position aiding.")
+
+            # Update (a posteriori) estimates with position aiding
+            _aiding_update_pos(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[0:3],
+                self._p_n,
+                np.asarray(pos),
+                np.asarray(pos_var),
+                R_nb,
+                self._lever_arm,
+            )
 
         if vel is not None:
             if vel_var is None:
-                raise ValueError("'vel_var' not provided.")
-
-            vel = np.asarray(vel, dtype=float, order="C")
-            vel_var = np.asarray(vel_var, dtype=float, order="C")
-            dz_vel = vel - vel_ins
-            H_vel = self._update_H_vel()
-            dx, P = self._update_dx_P(dx, P, dz_vel, vel_var, H_vel, I_)
-
-        if g_ref:
-            if g_var is None:
-                raise ValueError("'g_var' not provided.")
-            vg_meas_m = -_normalize(f_ins)
-            g_var = np.asarray(g_var, dtype=float, order="C")
-            dz_g = vg_meas_m - R_ins_nm.T @ self._vg_ref_n
-            H_g = self._update_H_g_ref(R_ins_nm)
-            dx, P = self._update_dx_P(dx, P, dz_g, g_var, H_g, I_)
+                raise ValueError("'vel_var' is required for velocity aiding.")
+
+            # Update (a posteriori) estimates with velocity aiding
+            _aiding_update_vel(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[3:6],
+                self._v_n,
+                np.asarray(vel),
+                np.asarray(vel_var),
+            )
 
         if head is not None:
             if head_var is None:
-                raise ValueError("'head_var' not provided.")
-
-            if head_degrees:
-                head = (np.pi / 180.0) * head
-                head_var = (np.pi / 180.0) ** 2 * head_var
-
-            head_var_ = np.asarray([head_var], dtype=float, order="C")
-            dz_head = np.asarray(
-                [_signed_smallest_angle(head - _h_head(q_ins_nm), degrees=False)],
-                dtype=float,
-                order="C",
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            # Update measurement matrix (heading row)
+            self._H[6, 6:9] = _dhda_head(self._q_nb)
+
+            # Update (a posteriori) estimates with heading aiding
+            _aiding_update_head(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[6],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
             )
 
-            H_head = self._update_H_head(q_ins_nm)
-            dx, P = self._update_dx_P(dx, P, dz_head, head_var_, H_head, I_)
-
-        self._dx[:] = dx.ravel().copy()
-
-        # Reset INS state
-        if dx.any():
-            self._reset_ins(dx.ravel())
-
-        # Discretize system
-        self._phi[:] = I_ + dt * F  # state transition matrix
-        Q = dt * G @ W @ G.T  # process noise covariance matrix
-
-        # Update current state
-        self._x[:] = self._ins._x
-        self._P[:] = P
+        if gref is True:
+            if gref_var is None:
+                raise ValueError("'gref_var' is required for gravity reference aiding.")
+
+            # Update measurement matrix (gravity reference vector rows)
+            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
+            self._H[7:10, 6:9] = _skew_symmetric(vg_b)
+
+            # Update (a posteriori) estimates with gravity reference vector aiding
+            _aiding_update_gref(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[7:10],
+                vg_b,
+                dvel,
+                np.asarray(gref_var),
+            )
 
-        # Project ahead
-        self._ins.update(f_imu, w_imu, degrees=False)
-        self._P_prior[:] = self._phi @ P @ self._phi.T + Q
+        # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
+        _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
         return self
-
-
-class VRU(AidedINS):
-    """
-    Vertical Reference Unit (VRU) based on a multiplicative extended Kalman filter
-    (MEKF).
-
-    VRU is intended for applicatoins with negligble sustained linear accelerations.
-    For applications with sustained linear accelerations, accurate position and/or
-    velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for
-    those cases.
-
-    This class inherits from :class:`smsfusion.AidedINS` but applies sensible
-    defaults for vertical reference applications and simplifies the interface by
-    hiding non-essential configuration options.
-
-    Velocity aiding is set to zero with a default standard deviation of 10 m/s.
-    Position aiding also assumes zero values but with high uncertainty (default
-    standard deviation of 1000 m), making it effectively non-constraining. Heading
-    aiding is completely disabled.
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
-        Initial (a priori) 16-element INS state estimate:
-
-        * Position (x, y, z) - 3 elements
-        * Velocity (x, y, z) - 3 elements
-        * Attitude (unit quaternion) - 4 elements
-        * Accelerometer bias (x, y, z) - 3 elements
-        * Gyroscope bias (x, y, z) - 3 elements
-
-        Defaults to a zero vector, but with the attitude part as a unit quaternion
-        (i.e., no rotation).
-    P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
-        Initial (a priori) estimate of the error covariance matrix, **P**.
-    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
-        Dictionary containing accelerometer noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
-        * ``B``: Bias stability in m/s^2.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
-        Dictionary containing gyroscope noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
-        * ``B``: Bias stability in rad/s.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    g : float, default 9.80665
-        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame. Furthermore, the aiding heading angle is
-        also interpreted relative to this frame according to the right-hand rule.
-    cold_start : bool, default True
-        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
-        A cold state indicates that the provided initial conditions are uncertain,
-        and possibly far from the true state. Thus, to reduce the risk of divergence,
-        an initial vertical alignment (i.e., roll and pitch calibration) is performed
-        using accelerometer measurements and the known direction of gravity during
-        the first measurement update. The IMU should remain stationary with negligible
-        linear acceleration during a cold start; otherwise, divergence may occur.
-        A warm start, on the other hand, assumes accurate initial conditions, and
-        initializes the Kalman filter immediately without any initial roll and pitch
-        calibration.
-    **kwargs :
-        Ignored. For compatibility with parent class.
-    """
-
-    def __init__(
-        self,
-        fs: float,
-        x0_prior: ArrayLike = X0,
-        P0_prior: ArrayLike = P0,
-        err_acc: dict[str, float] = ERR_ACC_MOTION2,
-        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
-        g: float = 9.80665,
-        nav_frame: str = "NED",
-        cold_start: bool = True,
-        **kwargs: dict[str, Any],
-    ) -> None:
-        super().__init__(
-            fs=fs,
-            x0_prior=x0_prior,
-            P0_prior=P0_prior,
-            err_acc=err_acc,
-            err_gyro=err_gyro,
-            g=g,
-            nav_frame=nav_frame,
-            lever_arm=np.zeros(3),
-            ignore_bias_acc=True,
-            cold_start=cold_start,
-        )
-
-    def update(
-        self,
-        f_imu: ArrayLike,
-        w_imu: ArrayLike,
-        degrees: bool = False,
-        pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]),
-        vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]),
-    ) -> Self:
-        """
-        Update/correct the VRU's state estimate with pseudo aiding measurements
-        (i.e., zero velocity and zero position with corresponding variances), and
-        project ahead using IMU measurements.
-
-        Parameters
-        ----------
-        f_imu : array-like, shape (3,)
-            Specific force measurements (i.e., accelerations + gravity), given
-            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
-            acceleration measurements in x-, y-, and z-direction, respectively.
-        w_imu : array-like, shape (3,)
-            Angular rate measurements, given as [w_x, w_y, w_z]^T where
-            w_x, w_y and w_z are angular rates about the x-, y-,
-            and z-axis, respectively.
-        degrees : bool, default False
-            Specifies whether the unit of ``w_imu`` are in degrees or radians.
-        pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
-            Variance of position measurement noise in m^2. Defaults to
-            standard deviation of 1000 m, while assuming zero position.
-        vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
-            Variance of velocity measurement noise in (m/s)^2. Defaults to
-            standard deviation of 10 m/s, while assuming zero velocity.
-
-        Returns
-        -------
-        VRU
-            A reference to the instance itself after the update.
-        """
-        return super().update(
-            f_imu,
-            w_imu,
-            degrees=degrees,
-            pos=np.array([0.0, 0.0, 0.0]),
-            pos_var=pos_var,
-            vel=np.array([0.0, 0.0, 0.0]),
-            vel_var=vel_var,
-            head=None,
-            head_var=None,
-        )
-
-
-class AHRS(AidedINS):
-    """
-    Attitude and Heading Reference System (AHRS) based on a multiplicative extended
-    Kalman filter (MEKF).
-
-    AHRS is intended for applicatoins with negligble sustained linear accelerations.
-    For applications with sustained linear accelerations, accurate position and/or
-    velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for
-    those cases.
-
-    This class inherits from :class:`smsfusion.AidedINS` but applies sensible
-    defaults for attitude heading reference applications and simplifies the
-    interface by hiding non-essential configuration options.
-
-    Velocity aiding is set to zero with a default standard deviation of 10 m/s.
-    Position aiding also assumes zero values but with high uncertainty (default
-    standard deviation of 1000 m), making it effectively non-constraining.
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
-        Initial (a priori) 16-element INS state estimate:
-
-        * Position (x, y, z) - 3 elements
-        * Velocity (x, y, z) - 3 elements
-        * Attitude (unit quaternion) - 4 elements
-        * Accelerometer bias (x, y, z) - 3 elements
-        * Gyroscope bias (x, y, z) - 3 elements
-
-        Defaults to a zero vector, but with the attitude part as a unit quaternion
-        (i.e., no rotation).
-    P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
-        Initial (a priori) estimate of the error covariance matrix, **P**.
-    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
-        Dictionary containing accelerometer noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
-        * ``B``: Bias stability in m/s^2.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
-        Dictionary containing gyroscope noise parameters with keys:
-
-        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
-        * ``B``: Bias stability in rad/s.
-        * ``tau_cb``: Bias correlation time in seconds.
-
-        Defaults to error characteristics of SMS Motion gen. 2.
-    g : float, default 9.80665
-        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
-    nav_frame : {'NED', 'ENU'}, default 'NED'
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame. Furthermore, the aiding heading angle is
-        also interpreted relative to this frame according to the right-hand rule.
-    cold_start : bool, default True
-        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
-        A cold state indicates that the provided initial conditions are uncertain,
-        and possibly far from the true state. Thus, to reduce the risk of divergence,
-        an initial vertical alignment (i.e., roll and pitch calibration) is performed
-        using accelerometer measurements and the known direction of gravity during
-        the first measurement update. The IMU should remain stationary with negligible
-        linear acceleration during a cold start; otherwise, divergence may occur.
-        A warm start, on the other hand, assumes accurate initial conditions, and
-        initializes the Kalman filter immediately without any initial roll and pitch
-        calibration.
-    **kwargs :
-        Ignored. For compatibility with parent class.
-    """
-
-    def __init__(
-        self,
-        fs: float,
-        x0_prior: ArrayLike = X0,
-        P0_prior: ArrayLike = P0,
-        err_acc: dict[str, float] = ERR_ACC_MOTION2,
-        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
-        g: float = 9.80665,
-        nav_frame: str = "NED",
-        cold_start: bool = True,
-        **kwargs: dict[str, Any],
-    ) -> None:
-        super().__init__(
-            fs=fs,
-            x0_prior=x0_prior,
-            P0_prior=P0_prior,
-            err_acc=err_acc,
-            err_gyro=err_gyro,
-            g=g,
-            nav_frame=nav_frame,
-            lever_arm=np.zeros(3),
-            ignore_bias_acc=True,
-            cold_start=cold_start,
-        )
-
-    def update(
-        self,
-        f_imu: ArrayLike,
-        w_imu: ArrayLike,
-        degrees: bool = False,
-        head: float | None = None,
-        head_var: float | None = None,
-        head_degrees: bool = True,
-        pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]),
-        vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]),
-    ) -> Self:
-        """
-        Update/correct the AHRS' state estimate with pseudo aiding measurements
-        (i.e., zero velocity and zero position with corresponding variances), and
-        project ahead using IMU measurements.
-
-        Parameters
-        ----------
-        f_imu : array-like, shape (3,)
-            Specific force measurements (i.e., accelerations + gravity), given
-            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
-            acceleration measurements in x-, y-, and z-direction, respectively.
-        w_imu : array-like, shape (3,)
-            Angular rate measurements, given as [w_x, w_y, w_z]^T where
-            w_x, w_y and w_z are angular rates about the x-, y-,
-            and z-axis, respectively.
-        degrees : bool, default False
-            Specifies whether the unit of ``w_imu`` are in degrees or radians.
-        head : float, optional
-            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
-            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
-        head_var : float, optional
-            Variance of heading measurement noise. Units must be compatible with ``head``.
-             See ``head_degrees`` for units. Required for ``head``.
-        head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
-            or radians and radians^2. Default is in radians and radians^2.
-        pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
-            Variance of position measurement noise in m^2. Defaults to
-            standard deviation of 1000 m, while assuming zero position.
-        vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
-            Variance of velocity measurement noise in (m/s)^2. Defaults to
-            standard deviation of 10 m/s, while assuming zero velocity.
-
-        Returns
-        -------
-        AHRS
-            A reference to the instance itself after the update.
-        """
-        return super().update(
-            f_imu,
-            w_imu,
-            degrees=degrees,
-            pos=np.array([0.0, 0.0, 0.0]),
-            pos_var=pos_var,
-            vel=np.array([0.0, 0.0, 0.0]),
-            vel_var=vel_var,
-            head=head,
-            head_var=head_var,
-            head_degrees=head_degrees,
-        )
diff --git a/src/smsfusion/_ins/_ains_.py b/src/smsfusion/_ins/_ains_.py
deleted file mode 100644
index 6daea77d..00000000
--- a/src/smsfusion/_ins/_ains_.py
+++ /dev/null
@@ -1,594 +0,0 @@
-from typing import Self
-
-import numpy as np
-from numba import njit
-from numpy.typing import ArrayLike, NDArray
-
-from smsfusion._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
-from smsfusion._vectorops import _skew_symmetric
-
-from ._aiding import (
-    _aiding_update_gref,
-    _aiding_update_head,
-    _aiding_update_pos,
-    _aiding_update_vel,
-)
-from ._common import (
-    _dhda_head,
-    _gref_b_from_quat,
-    _nz2vg,
-    _project_covariance_ahead,
-    _update_quaternion_with_gibbs2,
-    _update_quaternion_with_rotvec,
-)
-
-_P0 = (
-    (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6, 0.0),
-    (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0e-6),
-)
-
-
-def _state_transition_matrix_init(
-    dt: float,
-    dvel: NDArray[np.float64],
-    dtheta: NDArray[np.float64],
-    R_nb: NDArray[np.float64],
-    gbc: float,
-) -> NDArray[np.float64]:
-    """
-    State transition matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    dtheta : ndarray, shape (3,)
-        Attitude increment measurement (coning integral).
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (12, 12)
-        State transition matrix.
-    """
-    phi = np.eye(12)
-    phi[0:3, 3:6] += dt * np.eye(3)
-    phi[3:6, 6:9] -= R_nb @ _skew_symmetric(dvel)  # NB! update each time step
-    phi[6:9, 6:9] -= _skew_symmetric(dtheta)  # NB! update each time step
-    phi[6:9, 9:12] -= dt * np.eye(3)
-    phi[9:12, 9:12] -= dt * np.eye(3) / gbc
-    return phi
-
-
-@njit  # type: ignore[misc]
-def _state_transition_matrix_update(
-    phi: NDArray[np.float64],
-    dvel: NDArray[np.float64],
-    dtheta: NDArray[np.float64],
-    R_nb: NDArray[np.float64],
-) -> None:
-    """
-    Update the state transition matrix in place.
-
-    Parameters
-    ----------
-    phi : ndarray, shape (12, 12)
-        State transition matrix to be updated in place.
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    dtheta : ndarray, shape (3,)
-        Attitude increment measurement (coning integral).
-    R_nb : ndarray, shape (3, 3)
-        Rotation matrix from body to navigation frame.
-    """
-    dtx, dty, dtz = dtheta
-    dvx, dvy, dvz = dvel
-
-    r00, r01, r02 = R_nb[0]
-    r10, r11, r12 = R_nb[1]
-    r20, r21, r22 = R_nb[2]
-
-    # phi[6:9, 6:9] = np.eye(3) - dt * S(w_b)
-    phi[6, 7] = dtz
-    phi[6, 8] = -dty
-    phi[7, 6] = -dtz
-    phi[7, 8] = dtx
-    phi[8, 6] = dty
-    phi[8, 7] = -dtx
-
-    # phi[3:6, 6:9] = -dt * R_nb @ S(f_b)
-    phi[3, 6] = -dvz * r01 + dvy * r02
-    phi[4, 6] = -dvz * r11 + dvy * r12
-    phi[5, 6] = -dvz * r21 + dvy * r22
-    phi[3, 7] = dvz * r00 - dvx * r02
-    phi[4, 7] = dvz * r10 - dvx * r12
-    phi[5, 7] = dvz * r20 - dvx * r22
-    phi[3, 8] = -dvy * r00 + dvx * r01
-    phi[4, 8] = -dvy * r10 + dvx * r11
-    phi[5, 8] = -dvy * r20 + dvx * r21
-
-
-def _process_noise_covariance_matrix(
-    dt: float, vrw: float, arw: float, gbs: float, gbc: float
-) -> NDArray[np.float64]:
-    """
-    Process noise covariance matrix.
-
-    Parameters
-    ----------
-    dt : float
-        Time step in seconds.
-    vrw : float
-        Velocity random walk (accelerometer noise density) in m/s/√Hz.
-    arw : float
-        Angular random walk (gyroscope noise density) in rad/√Hz.
-    gbs : float
-        Gyro bias stability (bias instability) in rad/s.
-    gbc : float
-        Gyro bias correlation time in seconds.
-
-    Returns
-    -------
-    ndarray, shape (12, 12)
-        Process noise covariance matrix.
-    """
-    Q = np.zeros((12, 12))
-    Q[3:6, 3:6] = dt * vrw**2 * np.eye(3)
-    Q[6:9, 6:9] = dt * arw**2 * np.eye(3)
-    Q[9:12, 9:12] = dt * (2.0 * gbs**2 / gbc) * np.eye(3)
-    return Q
-
-
-def _measurement_matrix_init(
-    q_nb: NDArray[np.float64], lever_arm: NDArray[np.float64], nav_frame_factor: float
-) -> NDArray[np.float64]:
-    """
-    Measurement matrix.
-
-    Parameters
-    ----------
-    q_nb : ndarray, shape (4,)
-        Unit quaternion.
-    lever_arm : ndarray, shape(3,)
-        Lever-arm vector describing the location of position aiding (in meters) relative
-        to the IMU expressed in the IMU/body reference frame. For instance, the location
-        of the GNSS antenna relative to the IMU.
-    nav_frame_factor: float
-        Gravity direction along the navigation frame's z-axis. +1.0 for 'NED' and
-        -1.0 for 'ENU'.
-
-    Returns
-    -------
-    ndarray, shape (7, 12)
-        Linearized measurement matrix.
-    """
-    vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
-    H = np.zeros((10, 12))
-    H[0:3, 0:3] = np.eye(3)  # position
-    H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
-    H[3:6, 3:6] = np.eye(3)  # velocity
-    H[6:7, 6:9] = _dhda_head(q_nb)  # heading
-    H[7:10, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
-    return H
-
-
-def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
-    """
-    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
-
-    Parameters
-    ----------
-    g : float
-        Gravitational acceleration in m/s^2.
-    nav_frame : {'NED', 'ENU'}
-        Navigation frame in which the gravity vector is expressed.
-
-    Returns
-    -------
-    ndarray, shape (3,)
-        Gravity vector expressed in the navigation frame.
-    """
-    if nav_frame.lower() == "ned":
-        g_n = np.array([0.0, 0.0, g])
-    elif nav_frame.lower() == "enu":
-        g_n = np.array([0.0, 0.0, -g])
-    else:
-        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
-    return g_n
-
-
-@njit  # type: ignore[misc]
-def _reset(
-    dx: NDArray[np.float64],
-    p_n: NDArray[np.float64],
-    v_n: NDArray[np.float64],
-    q_nb: NDArray[np.float64],
-    bg_b: NDArray[np.float64],
-) -> None:
-    """
-    Reset state (in place).
-
-    Parameters
-    ----------
-    p_n : ndarray, shape (3,)
-        Position state estimate to be reset in place.
-    v_n : ndarray, shape (3,)
-        Velocity state estimate to be reset in place.
-    q_nb : ndarray, shape (4,)
-        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,)
-        Error state vector containing the corrections to be applied to the state
-        estimates. Will be reset to zero after applying the corrections.
-    """
-    p_n[:] += dx[0:3]
-    v_n[:] += dx[3:6]
-    _update_quaternion_with_gibbs2(q_nb, dx[6:9])  # -> update q_nb (in place)
-    bg_b[:] += dx[9:12]
-    dx[:] = 0.0
-
-
-@njit  # type: ignore[misc]
-def _project_state_ahead(
-    p_n: NDArray[np.float64],
-    v_n: NDArray[np.float64],
-    q_nb: NDArray[np.float64],
-    R_nb: NDArray[np.float64],
-    dvel: NDArray[np.float64],
-    dtheta: NDArray[np.float64],
-    dt: float,
-    dvel_g_corr: NDArray[np.float64],
-) -> None:
-    """
-    Project state estimates ahead (in place).
-
-    References
-    ----------
-    .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 3-5)
-    """
-    dvel_corr = R_nb @ dvel + dvel_g_corr
-    p_n[:] += dt * v_n + 0.5 * dt * dvel_corr
-    v_n[:] += dvel_corr
-    _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
-
-
-class AINS:
-    """
-    Aided inertial navigation system (AINS).
-
-    This class provides position, velocity, attitude and gyro bias estimation using
-    a multiplicative extended Kalman filter (MEKF).
-
-    Parameters
-    ----------
-    fs : float
-        Sampling rate in Hz.
-    p0 : array_like, shape (3,), optional
-        Initial position estimate in m. Defaults to origin (0.0, 0.0, 0.0).
-    v0 : array_like, shape (3,), optional
-        Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
-    q0 : array_like, shape (4,), optional
-        Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
-        to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation).
-    bg0 : array_like, shape (3,), optional
-        Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias.
-    P0 : array_like, shape (12, 12), optional
-        Initial (a priori) estimate of the error covariance matrix. Defaults to
-        a small diagonal matrix (1e-6 * np.eye(12)).
-    acc_noise_density : float, optional
-        Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to
-        0.0007 (m/s)/√Hz (SMS Motion 2 noise level).
-    gyro_noise_density : float, optional
-        Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to
-        0.00005 (rad/s)/√Hz (SMS Motion 2 noise level).
-    gyro_bias_stability : float, optional
-        Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2
-        noise level).
-    gyro_bias_corr_time : float, optional
-        Gyroscope bias correlation time in seconds. Defaults to 50.0 s.
-    g : float, optional
-        The gravitational acceleration in m/s^2. Default is 'standard gravity' of
-        9.80665 m/s^2.
-    nav_frame : {'NED', 'ENU'}, optional
-        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
-        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
-        will be expressed relative to this frame.
-    lever_arm : array-like, shape (3,), optional
-        Lever-arm vector describing the location of position aiding (in meters) relative
-        to the IMU expressed in the IMU/body reference frame. For instance, the location
-        of the GNSS antenna relative to the IMU. By default it is assumed that the
-        aiding position coincides with the IMU's origin.
-    """
-
-    def __init__(
-        self,
-        fs: float,
-        p0: ArrayLike = (0.0, 0.0, 0.0),
-        v0: ArrayLike = (0.0, 0.0, 0.0),
-        q0: ArrayLike = (1.0, 0.0, 0.0, 0.0),
-        bg0: ArrayLike = (0.0, 0.0, 0.0),
-        P0: ArrayLike = _P0,
-        acc_noise_density: float = 0.0007,
-        gyro_noise_density: float = 0.00005,
-        gyro_bias_stability: float = 0.00005,
-        gyro_bias_corr_time: float = 50.0,
-        g: float = 9.80665,
-        nav_frame: str = "NED",
-        lever_arm: ArrayLike = (0.0, 0.0, 0.0),
-    ) -> None:
-        self._fs = fs
-        self._dt = 1.0 / fs
-        self._nav_frame = nav_frame.lower()
-        self._nz2vg = _nz2vg(self._nav_frame)
-        self._g = g
-        self._g_n = _gravity_nav(self._g, self._nav_frame)
-        self._dvel_g_corr = self._dt * self._g_n
-        self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy()
-
-        # IMU noise parameters
-        self._vrw = acc_noise_density  # velocity random walk
-        self._arw = gyro_noise_density  # angular random walk
-        self._gbs = gyro_bias_stability  # gyro bias stability
-        self._gbc = gyro_bias_corr_time  # gyro bias correlation time
-
-        # State and covariance estimates
-        self._p_n = np.asarray_chkfinite(p0).reshape(3).copy()
-        self._v_n = np.asarray_chkfinite(v0).reshape(3).copy()
-        self._q_nb = np.asarray_chkfinite(q0).reshape(4).copy()
-        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)
-
-        # Discrete state-space model
-        self._phi = _state_transition_matrix_init(
-            self._dt,
-            np.zeros(3),
-            np.zeros(3),
-            _rot_matrix_from_quaternion(self._q_nb),
-            self._gbc,
-        )
-        self._Q = _process_noise_covariance_matrix(
-            self._dt, self._vrw, self._arw, self._gbs, self._gbc
-        )
-        self._H = _measurement_matrix_init(self._q_nb, self._lever_arm, self._nz2vg)
-
-    def position(self) -> NDArray[np.float64]:
-        """
-        Copy of the position estimate in meters.
-        """
-        return self._p_n.copy()
-
-    def velocity(self) -> NDArray[np.float64]:
-        """
-        Copy of the velocity estimate in m/s.
-        """
-        return self._v_n.copy()
-
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Copy of the attitude estimate expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
-    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
-        """
-        Copy of the attitude estimate expressed as Euler angles (roll, pitch, yaw).
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the Euler angles in degrees or radians. Defaults to
-            radians.
-
-        Returns
-        -------
-        numpy.ndarray, shape (3,)
-            Euler angles (roll, pitch, yaw).
-        """
-
-        theta = _euler_from_quaternion(self._q_nb)
-
-        if degrees:
-            theta = (180.0 / np.pi) * theta
-
-        return theta
-
-    def bias_gyro(self, degrees=False) -> NDArray[np.float64]:
-        """
-        Copy of the gyroscope bias estimate in rad/s or deg/s depending on the
-        ``degrees`` flag.
-
-        Parameters
-        ----------
-        degrees : bool, optional
-            Whether to return the bias in deg/s or rad/s. Defaults to rad/s.
-        """
-        bg_b = self._bg_b.copy()
-        if degrees:
-            bg_b = (180.0 / np.pi) * bg_b
-        return bg_b
-
-    @property
-    def P(self) -> NDArray[np.float64]:
-        """
-        Copy of the error covariance matrix estimate.
-        """
-        return self._P.copy()
-
-    def update(
-        self,
-        dvel: ArrayLike,
-        dtheta: ArrayLike,
-        degrees: bool = False,
-        pos: ArrayLike | None = None,
-        pos_var: ArrayLike | None = None,
-        vel: ArrayLike | None = None,
-        vel_var: ArrayLike | None = None,
-        head: float | None = None,
-        head_var: float | None = None,
-        head_degrees: bool = False,
-        gref: bool = False,
-        gref_var: ArrayLike | None = None,
-    ) -> Self:
-        """
-        Update state estimates with IMU and aiding measurements.
-
-        Parameters
-        ----------
-        dvel : array_like, shape (3,)
-            Velocity increment (sculling integral) in m/s.
-        dtheta : array_like, shape (3,)
-            Attitude increment (coning integral) in radians or degrees depending
-            on the ``degrees`` flag.
-        degrees : bool, optional
-            Specifies whether the unit of the attitude increment, ``dtheta``, is
-            degrees or radians. Defaults to radians.
-        pos : array-like, shape (3,), optional
-            Position aiding measurement in meters. If ``None``, position aiding
-            is not used.
-        pos_var : array-like, shape (3,), optional
-            Variance of position measurement noise in m^2. Ignored if ``pos`` is
-            ``None``.
-        vel : array-like, shape (3,), optional
-            Velocity aiding measurement in m/s. If ``None``, velocity aiding is
-            not used.
-        vel_var : array-like, shape (3,), optional
-            Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
-            is ``None``.
-        head : float, optional
-            Heading measurement in radians or degrees depending on the ``head_degrees``
-            flag. I.e., the yaw angle of the 'body' frame relative to the assumed
-            'navigation' frame ('NED' or 'ENU') specified during initialization.
-            If ``None``, compass aiding is not used.
-        head_var : float, optional
-            Variance of heading measurement noise in radians^2 or degrees^2 depending
-            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
-        head_degrees : bool, default False
-            Specifies whether the unit of ``head`` and ``head_var`` are in degrees
-            and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
-        gref : bool, optional
-            Specifies whether to use accelerometer measurements (dvel) and the known
-            direction of gravity as aiding. Defaults to ``False``.
-        gref_var : array_like, shape (3,), optional
-            Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding.
-
-        Returns
-        -------
-        AINS
-            A reference to the instance itself after the update.
-        """
-
-        dvel = np.asarray(dvel)
-        dtheta = np.asarray(dtheta)
-
-        if degrees:
-            dtheta = (np.pi / 180.0) * dtheta
-
-        dtheta = 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
-
-        # 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._dt,
-            self._dvel_g_corr,
-        )
-
-        # Project (a priori) error covariance matrix estimate ahead
-        _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
-
-        if pos is not None:
-            if pos_var is None:
-                raise ValueError("'pos_var' is required for position aiding.")
-
-            # Update (a posteriori) estimates with position aiding
-            _aiding_update_pos(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[0:3],
-                self._p_n,
-                np.asarray(pos),
-                np.asarray(pos_var),
-                R_nb,
-                self._lever_arm,
-            )
-
-        if vel is not None:
-            if vel_var is None:
-                raise ValueError("'vel_var' is required for velocity aiding.")
-
-            # Update (a posteriori) estimates with velocity aiding
-            _aiding_update_vel(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[3:6],
-                self._v_n,
-                np.asarray(vel),
-                np.asarray(vel_var),
-            )
-
-        if head is not None:
-            if head_var is None:
-                raise ValueError("'head_var' is required for heading aiding.")
-
-            # Update measurement matrix (heading row)
-            self._H[6, 6:9] = _dhda_head(self._q_nb)
-
-            # Update (a posteriori) estimates with heading aiding
-            _aiding_update_head(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[6],
-                self._q_nb,
-                head,
-                head_var,
-                head_degrees,
-            )
-
-        if gref is True:
-            if gref_var is None:
-                raise ValueError("'gref_var' is required for gravity reference aiding.")
-
-            # Update measurement matrix (gravity reference vector rows)
-            vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
-            self._H[7:10, 6:9] = _skew_symmetric(vg_b)
-
-            # Update (a posteriori) estimates with gravity reference vector aiding
-            _aiding_update_gref(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[7:10],
-                vg_b,
-                dvel,
-                np.asarray(gref_var),
-            )
-
-        # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
-        _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
-
-        return self
diff --git a/src/smsfusion/_ins/_ains_old.py b/src/smsfusion/_ins/_ains_old.py
new file mode 100644
index 00000000..f39423e4
--- /dev/null
+++ b/src/smsfusion/_ins/_ains_old.py
@@ -0,0 +1,1501 @@
+from __future__ import annotations
+
+from typing import Any, Self
+
+import numpy as np
+from numba import njit
+from numpy.typing import ArrayLike, NDArray
+
+from smsfusion._transforms import (
+    _angular_matrix_from_quaternion,
+    _euler_from_quaternion,
+    _quaternion_from_euler,
+    _rot_matrix_from_quaternion,
+)
+from smsfusion._vectorops import _normalize, _quaternion_product, _skew_symmetric
+from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0
+
+
+def _roll_pitch_from_acc(f, nav_frame):
+    """
+    Estimate roll and pitch angles from specific force (i.e., accelerometer) measurement.
+
+    Parameters
+    ----------
+    f: array-like
+        Specific force (i.e., acceleration) measurement vector (fx, fy, fz).
+    nav_frame: {'NED', 'ENU'}
+        Navigation frame. Should be either 'NED' or 'ENU'.
+
+    Returns
+    -------
+    roll: float
+        Estimated roll angle in radians.
+    pitch: float
+        Estimated pitch angle in radians.
+    """
+
+    fx, fy, fz = f
+
+    if nav_frame.lower() == "ned":
+        roll = np.arctan2(-fy, -fz)
+        pitch = np.arctan2(fx, np.sqrt(fy**2 + fz**2))
+    elif nav_frame.lower() == "enu":
+        roll = np.arctan2(fy, fz)
+        pitch = -np.arctan2(fx, np.sqrt(fy**2 + fz**2))
+    else:
+        raise ValueError("Invalid navigation frame. Should be 'NED' or 'ENU'.")
+
+    return roll, pitch
+
+
+class FixedNED:
+    """
+    Convert position coordinates between a fixed NED frame (x, y, z) and ECEF frame
+    (lattitude, longitude, height).
+
+    The fixed NED frame is a tangential plane on the WGS-84 ellipsoid with its origin
+    fixed at the provided reference coordinates. It is assumed that the tangential
+    plane is close to the ellipsoid surface.
+
+    Parameters
+    ----------
+    lat_ref: float
+        Reference latitude coordinate in decimal degrees.
+    lon_ref: float
+        Reference longitude coordinate in decimal degrees.
+    height_ref: ref
+        Reference height coordinate in decimal degrees.
+    """
+
+    def __init__(self, lat_ref: float, lon_ref: float, height_ref: float) -> None:
+        self._lat_ref = lat_ref
+        self._lon_ref = lon_ref
+        self._height_ref = height_ref
+
+        radius_eq = 6_378_137  # equatorial radius (WGS-84)
+        radius_polar = 6_356_752.314245  # polar radius (WGS-84)
+
+        radius_ratio_squared = (radius_polar / radius_eq) ** 2
+        denom = np.cos(
+            self._lat_ref * (np.pi / 180.0)
+        ) ** 2 + radius_ratio_squared * np.sin(self._lat_ref * (np.pi / 180.0))
+
+        self._Rn = radius_eq / np.sqrt(denom)  # radius prime vertical
+        self._Rm = self._Rn * radius_ratio_squared / denom  # radius meridian
+
+        self._Rm_h = self._Rm + height_ref
+        self._Rn_h_cos = (self._Rn + height_ref) * np.cos(
+            self._lat_ref * (np.pi / 180.0)
+        )
+
+    def to_llh(self, x: float, y: float, z: float) -> tuple[float, float, float]:
+        """
+        Compute longitude, latitude, and height coordinates (WGS-84) from local
+        Cartesian coordinates in the fixed NED frame.
+
+        Parameters
+        ----------
+        x: float
+            Local x-coordinate in meters in the fixed NED frame.
+        y: float
+            Local y-coordinate in meters in the fixed NED frame.
+        z: float
+            Local z-coordinate in meters in the fixed NED frame.
+
+        Returns
+        -------
+        lat: float
+            Latitude coordinate in decimal degrees.
+        lon: float
+            Longitude coordinate in decimal degrees.
+        height: float
+            Height coordinate in meters.
+        """
+        dlat = (180.0 / np.pi) * x / self._Rm_h
+        dlon = (180.0 / np.pi) * y / self._Rn_h_cos
+
+        lat = _signed_smallest_angle(self._lat_ref + dlat, degrees=True)
+        lon = _signed_smallest_angle(self._lon_ref + dlon, degrees=True)
+        h = self._height_ref - z
+        return lat, lon, h
+
+    def to_xyz(
+        self, lat: float, lon: float, height: float
+    ) -> tuple[float, float, float]:
+        """
+        Compute local Cartesian coordinates in the fixed NED frame from longitude,
+        latitude, and height coordinates (WGS-84).
+
+        Parameters
+        ----------
+        lat: float
+            Latitude coordinate in decimal degrees.
+        lon: float
+            Longitude coordinate in decimal degrees.
+        height: float
+            Height coordinate in meters.
+
+        Returns
+        -------
+        x: float
+            Local x-coordinate in meters in the fixed NED frame.
+        y: float
+            Local y-coordinate in meters in the fixed NED frame.
+        z: float
+            Local z-coordinate in meters in the fixed NED frame.
+        """
+        dlat = lat - self._lat_ref
+        dlon = lon - self._lon_ref
+
+        x = (np.pi / 180.0) * dlat * self._Rm_h
+        y = (np.pi / 180.0) * dlon * self._Rn_h_cos
+        z = self._height_ref - height
+        return x, y, z
+
+
+def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
+    """
+    Convert the given angle to the smallest angle between [-180., 180) degrees.
+
+    Parameters
+    ----------
+    angle : float
+        Value of angle.
+    degrees : bool, default True
+        Specify whether ``angle`` is given degrees or radians.
+
+    Returns
+    -------
+    float
+        The smallest angle between [-180., 180) degrees (or  [-pi, pi] radians).
+    """
+    base = 180.0 if degrees else np.pi
+    return (angle + base) % (2.0 * base) - base
+
+
+def gravity(lat: float | None = None, degrees: bool = True) -> float:
+    """
+    Calculates the gravitational acceleration based on the World Geodetic System
+    (1984) Ellipsoidal Gravity Formula (WGS-84).
+
+    The WGS-84 formula is given by::
+
+        g = g_e * (1 - k * sin(lat)^2) / sqrt(1 - e^2 * sin(lat)^2)
+
+    where, ::
+
+        g_e = 9.780325335903891718546
+        k = 0.00193185265245827352087
+        e^2 = 0.006694379990141316996137
+
+    and ``lat`` is the latitude.
+
+    If no latitude is provided, the 'standard gravity', ``g_0``, is returned instead.
+    The standard gravity is by definition of the ISO/IEC 8000 given as
+    ``g_0 = 9.80665``.
+
+    Parameters
+    ----------
+    lat : float, optional
+        Latitude. If none provided, the 'standard gravity' is returned.
+    degrees : bool, optional
+        Specify whether the latitude, ``lat``, is in degrees or radians.
+        Applicapble only if ``lat`` is provided.
+    """
+    if lat is None:
+        g_0 = 9.80665  # standard gravity in m/s^2
+        return g_0
+
+    g_e = 9.780325335903891718546  # gravity at equator
+    k = 0.00193185265245827352087  # formula constant
+    e_2 = 0.006694379990141316996137  # spheroid's squared eccentricity
+
+    if degrees:
+        lat = (np.pi / 180.0) * lat
+
+    g = g_e * (1.0 + k * np.sin(lat) ** 2.0) / np.sqrt(1.0 - e_2 * np.sin(lat) ** 2.0)
+    return g  # type: ignore[no-any-return]  # numpy funcs declare Any as return when given scalar-like
+
+
+class INSMixin:
+    """
+    Mixin class for inertial navigation systems (INS).
+
+    Requires that the inheriting class has an `_x` attribute which is a 1D numpy array
+    of length 16 containing the following elements in order:
+
+        * Position in x, y, z directions (3 elements).
+        * Velocity in x, y, z directions (3 elements).
+        * Attitude as unit quaternion (4 elements).
+        * Accelerometer bias in x, y, z directions (3 elements).
+        * Gyroscope bias in x, y, z directions (3 elements).
+    """
+
+    _x: NDArray[np.float64]  # state array of length 16
+
+    @property
+    def _pos(self) -> NDArray[np.float64]:
+        return self._x[0:3]
+
+    @_pos.setter
+    def _pos(self, p: ArrayLike) -> None:
+        self._x[0:3] = p
+
+    @property
+    def _vel(self) -> NDArray[np.float64]:
+        return self._x[3:6]
+
+    @_vel.setter
+    def _vel(self, v: ArrayLike) -> None:
+        self._x[3:6] = v
+
+    @property
+    def _q_nm(self) -> NDArray[np.float64]:
+        return self._x[6:10]
+
+    @_q_nm.setter
+    def _q_nm(self, q_nm: ArrayLike) -> None:
+        self._x[6:10] = q_nm
+
+    @property
+    def _bias_acc(self) -> NDArray[np.float64]:
+        return self._x[10:13]
+
+    @_bias_acc.setter
+    def _bias_acc(self, b_acc: ArrayLike) -> None:
+        self._x[10:13] = b_acc
+
+    @property
+    def _bias_gyro(self) -> NDArray[np.float64]:
+        return self._x[13:16]
+
+    @_bias_gyro.setter
+    def _bias_gyro(self, b_gyro: ArrayLike) -> None:
+        self._x[13:16] = b_gyro
+
+    @property
+    def x(self) -> NDArray[np.float64]:
+        """
+        Get current state vector estimate.
+
+        Returns
+        -------
+        numpy.ndarray, shape (16,)
+            State vector, containing the following elements in order:
+
+            * Position in x, y, z directions (3 elements).
+            * Velocity in x, y, z directions (3 elements).
+            * Attitude as unit quaternion (4 elements).
+            * Accelerometer bias in x, y, z directions (3 elements).
+            * Gyroscope bias in x, y, z directions (3 elements).
+        """
+        return self._x.copy()
+
+    def position(self) -> NDArray[np.float64]:
+        """
+        Get current position estimate.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Position state vector, containing position in x-, y-, and z-direction
+            (in that order).
+        """
+        return self._pos.copy()
+
+    def velocity(self) -> NDArray[np.float64]:
+        """
+        Get current velocity estimate.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Velocity state vector, containing (linear) velocity in x-, y-, and z-direction
+            (in that order).
+        """
+        return self._vel.copy()
+
+    def euler(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Get current attitude estimate as Euler angles (see Notes).
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the Euler angles in degrees or radians.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Euler angles, specifically: alpha (roll), beta (pitch) and gamma (yaw)
+            in that order.
+
+        Notes
+        -----
+        The Euler angles describe how to transition from the 'navigation' frame
+        ('NED' or 'ENU) to the 'body' frame through three consecutive intrinsic
+        and passive rotations in the ZYX order:
+
+        #. A rotation by an angle gamma (often called yaw) about the z-axis.
+        #. A subsequent rotation by an angle beta (often called pitch) about the y-axis.
+        #. A final rotation by an angle alpha (often called roll) about the x-axis.
+
+        This sequence of rotations is used to describe the orientation of the 'body' frame
+        relative to the 'navigation' frame ('NED' or 'ENU) in 3D space.
+
+        Intrinsic rotations mean that the rotations are with respect to the changing
+        coordinate system; as one rotation is applied, the next is about the axis of
+        the newly rotated system.
+
+        Passive rotations mean that the frame itself is rotating, not the object
+        within the frame.
+        """
+        q = self.quaternion()
+        theta = _euler_from_quaternion(q)
+
+        if degrees:
+            theta = (180.0 / np.pi) * theta
+
+        return theta  # type: ignore[no-any-return]
+
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Get current attitude estimate as unit quaternion (from-body-to-navigation-frame).
+
+        Returns
+        -------
+        numpy.ndarray, shape (4,)
+            Attitude as unit quaternion. Given as ``[q1, q2, q3, q4]``, where
+            ``q1`` is the real part and ``q2``, ``q3`` and ``q4`` are the three
+            imaginary parts.
+        """
+        return self._q_nm.copy()
+
+    def bias_acc(self) -> NDArray[np.float64]:
+        """
+        Get current accelerometer bias estimate.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Accelerometer bias vector, containing biases in x-, y-, and z-direction
+            (in that order).
+        """
+        return self._bias_acc.copy()
+
+    def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]:
+        """
+        Get current gyroscope bias estimate.
+
+        Parameters
+        ----------
+        degrees : bool, default False
+            Whether to return the bias in deg/s or rad/s.
+
+        Returns
+        -------
+        numpy.ndarray, shape (3,)
+            Gyroscope bias vector, containing biases in x-, y-, and z-direction
+            (in that order).
+        """
+        b_gyro = self._bias_gyro.copy()
+        if degrees:
+            b_gyro = (180.0 / np.pi) * b_gyro
+        return b_gyro
+
+
+class StrapdownINS(INSMixin):
+    """
+    Strapdown inertial navigation system (INS).
+
+    This class provides an interface for estimating position, velocity and attitude
+    of a moving body by integrating the *strapdown navigation equations*.
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    x0 : array-like, shape (16,)
+        Initial state vector containing the following elements in order:
+
+        * Position in x, y, z directions (3 elements).
+        * Velocity in x, y, z directions (3 elements).
+        * Attitude as unit quaternion (4 elements).
+        * Accelerometer bias in x, y, z directions (3 elements).
+        * Gyroscope bias in x, y, z directions (3 elements).
+    g : float, default 9.80665
+        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame.
+
+    Notes
+    -----
+    The quaternion provided as part of the initial state will be normalized to
+    ensure unity.
+    """
+
+    def __init__(
+        self, fs: float, x0: ArrayLike, g: float = 9.80665, nav_frame="NED"
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+
+        self._x0 = np.asarray_chkfinite(x0).reshape(16).copy()
+        self._x0[6:10] = _normalize(self._x0[6:10])
+        self._x = self._x0.copy()
+        self._g = g
+        self._nav_frame = nav_frame.lower()
+
+        if self._nav_frame == "ned":
+            self._g_n = np.array([0.0, 0.0, g])
+        elif self._nav_frame == "enu":
+            self._g_n = np.array([0.0, 0.0, -g])
+        else:
+            raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.")
+
+    def reset(self, x_new: ArrayLike) -> None:
+        """
+        Reset current state with a new one.
+
+        Parameters
+        ----------
+        x_new : numpy.ndarray, shape (10,)
+            New state vector, containing the following elements in order:
+
+            * Position in x-, y-, and z-direction (3 elements).
+            * Velocity in x-, y-, and z-direction (3 elements).
+            * Attitude as unit quaternion (4 elements). Should be given as
+              [q1, q2, q3, q4], where q1 is the real part and q1, q2 and q3
+              are the three imaginary parts.
+
+        Notes
+        -----
+        The quaternion provided as part of the new state will be normalized to
+        ensure unity.
+        """
+        self._x = np.asarray_chkfinite(x_new).reshape(16).copy()
+        self._x[6:10] = _normalize(self._x[6:10])
+
+    def update(
+        self,
+        f_imu: ArrayLike,
+        w_imu: ArrayLike,
+        degrees: bool = False,
+    ) -> Self:
+        """
+        Update the INS states by integrating the *strapdown navigation equations*.
+
+        Assuming constant inputs (i.e., accelerations and angular velocities) over
+        the sampling period.
+
+        The states are updated according to::
+
+            p[k+1] = p[k] + h * v[k] + 0.5 * dt * a[k]
+
+            v[k+1] = v[k] + dt * a[k]
+
+            q[k+1] = q[k] + dt * T(q[k]) * w_ins[k]
+
+        with bias compensated IMU measurements::
+
+            f_ins[k] = f_imu[k] - b_acc[k]
+
+            w_ins[k] = w_imu[k] - b_gyro[k]
+
+        and::
+
+            a[k] = R(q[k]) * f_ins[k] + g
+
+            g = [0, 0, 9.81]^T
+
+        Parameters
+        ----------
+        f_imu : array-like, shape (3,)
+            Specific force measurements (i.e., accelerations + gravity), given
+            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
+            acceleration measurements in x-, y-, and z-direction, respectively.
+        w_imu : array-like, shape (3,)
+            Angular rate measurements, given as [w_x, w_y, w_z]^T where
+            w_x, w_y and w_z are angular rates about the x-, y-,
+            and z-axis, respectively.
+        degrees : bool, default False
+            Specify whether the angular rates are given in degrees or radians.
+
+        Returns
+        -------
+        StrapdownINS :
+            A reference to the instance itself after the update.
+        """
+        f_imu = np.asarray(f_imu, dtype=float)
+        w_imu = np.asarray(w_imu, dtype=float)
+
+        if degrees:
+            w_imu = (np.pi / 180.0) * w_imu
+
+        # Bias compensated IMU measurements
+        f_ins = f_imu - self._bias_acc
+        w_ins = w_imu - self._bias_gyro
+
+        q_nm = self._q_nm
+        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned
+        T = _angular_matrix_from_quaternion(q_nm)
+
+        # State propagation (assuming constant linear acceleration and angular velocity)
+        acc = R_nm @ f_ins + self._g_n
+        self._pos = self._pos + self._dt * self._vel
+        self._vel = self._vel + self._dt * acc
+        q_nm = q_nm + self._dt * T @ w_ins
+        self._q_nm = _normalize(q_nm)
+
+        return self
+
+
+def _h_head(q: NDArray[np.float64]) -> float:
+    """
+    Compute yaw angle from unit quaternion.
+
+    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
+    unit quaternion here to avoid singularities.
+
+    Parameters
+    ----------
+    q : numpy.ndarray, shape (4,)
+        Unit quaternion.
+
+    Returns
+    -------
+    float
+        Yaw angle in the NED reference frame.
+
+    References
+    ----------
+    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
+    2nd Edition, equation 14.251, John Wiley & Sons, 2021.
+    """
+    q_w, q_x, q_y, q_z = q
+    u_y = 2.0 * (q_x * q_y + q_z * q_w)
+    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
+    return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
+
+
+@njit  # type: ignore[misc]
+def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
+    """
+    Compute yaw angle gradient wrt to the unit quaternion.
+
+    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
+    unit quaternion here to avoid singularities.
+
+    Parameters
+    ----------
+    q : numpy.ndarray, shape (3,)
+        Unit quaternion.
+
+    Returns
+    -------
+    numpy.ndarray, shape (3,)
+        Yaw angle gradient vector.
+
+    References
+    ----------
+    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
+    2nd Edition, equation 14.254, John Wiley & Sons, 2021.
+    """
+    q_w, q_x, q_y, q_z = q
+    u_y = 2.0 * (q_x * q_y + q_z * q_w)
+    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
+    u = u_y / u_x
+
+    duda_scale = 1.0 / u_x**2
+    duda_x = -(q_w * q_y) * (1.0 - 2.0 * q_w**2) - (2.0 * q_w**2 * q_x * q_z)
+    duda_y = (q_w * q_x) * (1.0 - 2.0 * q_z**2) + (2.0 * q_w**2 * q_y * q_z)
+    duda_z = q_w**2 * (1.0 - 2.0 * q_y**2) + (2.0 * q_w * q_x * q_y * q_z)
+    duda = duda_scale * np.array([duda_x, duda_y, duda_z])
+
+    dhda = 1.0 / (1.0 + u**2) * duda
+
+    return dhda  # type: ignore[no-any-return]
+
+
+class AidedINS(INSMixin):
+    """
+    Aided inertial navigation system (AINS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
+        Initial (a priori) 16-element INS state estimate:
+
+        * Position (x, y, z) - 3 elements
+        * Velocity (x, y, z) - 3 elements
+        * Attitude (unit quaternion) - 4 elements
+        * Accelerometer bias (x, y, z) - 3 elements
+        * Gyroscope bias (x, y, z) - 3 elements
+
+        Defaults to a zero vector, but with the attitude part as a unit quaternion
+        (i.e., no rotation).
+    P0_prior : array-like (shape (12, 12) or (15, 15)), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
+        Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a
+        small diagonal matrix will be used. If the accelerometer bias is excluded from the
+        error estimate (see ``ignore_bias_acc``), the covariance matrix should be of shape
+        (12, 12), otherwise (15, 15).
+    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
+        Dictionary containing accelerometer noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
+        * ``B``: Bias stability in m/s^2.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
+        Dictionary containing gyroscope noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
+        * ``B``: Bias stability in rad/s.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    g : float, default 9.80665
+        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame. Furthermore, the aiding heading angle is
+        also interpreted relative to this frame according to the right-hand rule.
+    lever_arm : array-like, shape (3,), default numpy.zeros(3)
+        Lever-arm vector describing the location of position aiding (in meters) relative
+        to the IMU expressed in the IMU's measurement frame. For instance, the location
+        of the GNSS antenna relative to the IMU. By default it is assumed that the
+        aiding position coincides with the IMU's origin.
+    ignore_bias_acc : bool, default True
+        Determines whether the accelerometer bias should be included in the error estimate.
+        If set to ``True``, the accelerometer bias provided in ``x0`` during initialization
+        will remain fixed and not updated. This option is useful in situations where the
+        accelerometer bias is unobservable, such as when there is insufficient aiding
+        information or minimal dynamic motion, making bias estimation unreliable. Note
+        that this will reduce the error-state dimension from 15 to 12, and hence also the
+        error covariance matrix, **P**, from dimension (15, 15) to (12, 12). When set to
+        ``False``, the P0_prior argument must have shape (15, 15).
+    cold_start : bool, default True
+        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
+        A cold state indicates that the provided initial conditions are uncertain,
+        and possibly far from the true state. Thus, to reduce the risk of divergence,
+        an initial vertical alignment (i.e., roll and pitch calibration) is performed
+        using accelerometer measurements and the known direction of gravity during
+        the first measurement update. The IMU should remain stationary with negligible
+        linear acceleration during a cold start; otherwise, divergence may occur.
+        A warm start, on the other hand, assumes accurate initial conditions, and
+        initializes the Kalman filter immediately without any initial roll and pitch
+        calibration.
+    """
+
+    # Permutation matrix for reordering error-state bias terms, such that:
+    # [pos, vel, quat, b_gyro, b_acc]^T = T_dx @ [pos, vel, quat, b_acc, b_gyro]^T
+    _T_dx = np.zeros((15, 15))
+    _T_dx[:9, :9] = np.eye(9)
+    _T_dx[9:12, 12:15] = np.eye(3)
+    _T_dx[12:15, 9:12] = np.eye(3)
+
+    # Permutation matrix for reordering white noise bias terms, such that:
+    # [acc, gyro, b_gyro, b_acc]^T = T_wn @ [acc, gyro, b_acc, b_gyro]^T
+    _T_wn = np.zeros((12, 12))
+    _T_wn[:6, :6] = np.eye(6)
+    _T_wn[6:9, 9:12] = np.eye(3)
+    _T_wn[9:12, 6:9] = np.eye(3)
+
+    def __init__(
+        self,
+        fs: float,
+        x0_prior: ArrayLike = X0,
+        P0_prior: ArrayLike = P0,
+        err_acc: dict[str, float] = ERR_ACC_MOTION2,
+        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+        lever_arm: ArrayLike = np.zeros(3),
+        ignore_bias_acc: bool = True,
+        cold_start: bool = True,
+    ) -> None:
+        self._fs = fs
+        self._dt = 1.0 / fs
+        self._err_acc = err_acc
+        self._err_gyro = err_gyro
+        self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy()
+        self._ignore_bias_acc = ignore_bias_acc
+        self._cold = cold_start
+        self._dq_prealloc = np.array([2.0, 0.0, 0.0, 0.0])  # Preallocation
+
+        # Strapdown algorithm / INS state
+        self._ins = StrapdownINS(self._fs, x0_prior, g=g, nav_frame=nav_frame)
+        self._vg_ref_n = _normalize(self._ins._g_n)  # gravity reference vector
+
+        # Total state estimate
+        self._x = self._ins.x
+
+        # Error state estimate (after reset)
+        self._dx_prealloc = np.zeros(15)  # always zero, but used in sequential update
+
+        # Initialize Kalman filter
+        self._P_prior = np.asarray_chkfinite(P0_prior).copy(order="C")
+        self._P = self._P_prior.copy(order="C")
+
+        # Verify error covariance matrix shape
+        if ignore_bias_acc and self._P_prior.shape != (12, 12):
+            raise ValueError(
+                f"P0_prior must be of shape (12, 12) when ignore_bias_acc is set to True. Was {self._P_prior.shape}."
+            )
+        if not ignore_bias_acc and self._P_prior.shape != (15, 15):
+            raise ValueError(
+                f"P0_prior must be of shape (15, 15) when ignore_bias_acc is set to False. Was {self._P_prior.shape}."
+            )
+
+        # Prepare system matrices
+        q0 = self._ins._q_nm
+        self._F = self._prep_F(err_acc, err_gyro, q0)
+        self._G = self._prep_G(q0)
+        self._H = self._prep_H()
+        self._W = self._prep_W(err_acc, err_gyro)
+        self._I = np.eye(15, order="C")
+
+        # Filter out the accelerometer bias terms from the system matrices (if ignored)
+        if self._ignore_bias_acc:
+            dx_dim = 12
+            wn_dim = 9
+            self._F = (self._T_dx @ self._F @ self._T_dx.T)[:dx_dim, :dx_dim]
+            self._G = (self._T_dx @ self._G @ self._T_wn)[:dx_dim, :wn_dim]
+            self._H = (self._H @ self._T_dx)[:, :dx_dim]
+            self._W = (self._T_wn @ self._W @ self._T_wn.T)[:wn_dim, :wn_dim]
+            self._I = self._I[:dx_dim, :dx_dim]
+            self._dx_prealloc = self._dx_prealloc[:dx_dim]
+
+        # Error-state estimate (before reset)
+        self._dx = np.empty_like(self._dx_prealloc)  # needed for smoothing only
+
+        # State transition matrix
+        self._phi = np.empty_like(self._F)  # needed for smoothing only
+
+    @property
+    def x_prior(self) -> NDArray[np.float64]:
+        """
+        Next a priori state vector estimate.
+
+        Returns
+        -------
+        numpy.ndarray, shape (16,)
+            A priori state vector estimate, containing the following elements in order:
+
+            * Position in x, y, z directions (3 elements).
+            * Velocity in x, y, z directions (3 elements).
+            * Attitude as unit quaternion (4 elements).
+            * Accelerometer bias in x, y, z directions (3 elements).
+            * Gyroscope bias in x, y, z directions (3 elements).
+        """
+        return self._ins.x
+
+    def dump(
+        self,
+    ) -> dict[str, np.float64 | list[np.float64] | dict[str, np.float64] | bool]:
+        """
+        Dump the configuration and current state of the AINS to a dictionary. The dumped
+        parameters can be used to restore the AINS to its current state.
+
+        Returns
+        -------
+        dict
+            A dictionary containing the configuration and current state of the AINS.
+        """
+        params = {
+            "fs": self._fs,
+            "x0_prior": self.x_prior.tolist(),
+            "P0_prior": self.P_prior.tolist(),
+            "err_acc": self._err_acc,
+            "err_gyro": self._err_gyro,
+            "g": self._ins._g,
+            "nav_frame": self._ins._nav_frame,
+            "lever_arm": self._lever_arm.tolist(),
+            "ignore_bias_acc": self._ignore_bias_acc,
+            "cold_start": self._cold,
+        }
+        return params
+
+    @property
+    def P(self) -> NDArray[np.float64]:
+        """
+        Error covariance matrix, **P**. I.e., the error covariance matrix associated with
+        the Kalman filter's updated (a posteriori) error-state estimate.
+        """
+        P = self._P.copy()
+        return P
+
+    @property
+    def P_prior(self) -> NDArray[np.float64]:
+        """
+        Next (a priori) estimate of the error covariance matrix, **P**. I.e., the error
+        covariance matrix associated with the Kalman filter's projected (a priori)
+        error-state estimate.
+        """
+        P_prior = self._P_prior.copy()
+        return P_prior
+
+    @staticmethod
+    def _prep_F(
+        err_acc: dict[str, float],
+        err_gyro: dict[str, float],
+        q_nm: NDArray[np.float64],
+    ) -> NDArray[np.float64]:
+        """
+        Prepare linearized state matrix, F.
+        """
+
+        beta_acc = 1.0 / err_acc["tau_cb"]
+        beta_gyro = 1.0 / err_gyro["tau_cb"]
+
+        # Temporary placeholder vectors (to be replaced each timestep)
+        f_ins = np.array([0.0, 0.0, 0.0])
+        w_ins = np.array([0.0, 0.0, 0.0])
+
+        S = _skew_symmetric  # alias skew symmetric matrix
+        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned rotation matrix
+
+        # State transition matrix
+        F = np.zeros((15, 15))
+        F[0:3, 3:6] = np.eye(3)
+        F[3:6, 6:9] = -R_nm @ S(f_ins)  # NB! update each time step
+        F[3:6, 9:12] = -R_nm  # NB! update each time step
+        F[6:9, 6:9] = -S(w_ins)  # NB! update each time step
+        F[6:9, 12:15] = -np.eye(3)
+        F[9:12, 9:12] = -beta_acc * np.eye(3)
+        F[12:15, 12:15] = -beta_gyro * np.eye(3)
+
+        return F
+
+    def _update_F(
+        self,
+        R_nm: NDArray[np.float64],
+        f_ins: NDArray[np.float64],
+        w_ins: NDArray[np.float64],
+    ) -> None:
+        """Update linearized state transition matrix, F."""
+        S = _skew_symmetric  # alias skew symmetric matrix
+
+        # Update matrix
+        self._F[3:6, 6:9] = -R_nm @ S(f_ins)  # NB! update each time step
+        self._F[6:9, 6:9] = -S(w_ins)  # NB! update each time step
+        if not self._ignore_bias_acc:
+            self._F[3:6, 9:12] = -R_nm  # NB! update each time step
+
+    @staticmethod
+    def _prep_G(q_nm: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Prepare (white noise) input matrix, G."""
+        R_nm = _rot_matrix_from_quaternion(q_nm)  # body-to-ned rotation matrix
+
+        # Input (white noise) matrix
+        G = np.zeros((15, 12))
+        G[3:6, 0:3] = -R_nm  # NB! update each time step
+        G[6:9, 3:6] = -np.eye(3)
+        G[9:12, 6:9] = np.eye(3)
+        G[12:15, 9:12] = np.eye(3)
+        return G
+
+    def _update_G(self, R_nm: NDArray[np.float64]) -> None:
+        """Update (white noise) input matrix, G."""
+
+        # Update matrix
+        self._G[3:6, 0:3] = -R_nm
+
+    @staticmethod
+    def _prep_H() -> NDArray[np.float64]:
+        """Prepare linearized measurement matrix, H. Values are placeholders only"""
+        H = np.zeros((10, 15))
+        H[0:3, 0:3] = np.eye(3)  # position
+        H[3:6, 3:6] = np.eye(3)  # velocity
+        return H
+
+    def _update_H_pos(
+        self, R_nm: NDArray[np.float64], lever_arm: NDArray[np.float64]
+    ) -> NDArray[np.float64]:
+        """Update and return part of H matrix relevant for position aiding."""
+        S = _skew_symmetric
+        self._H[0:3, 6:9] = -R_nm @ S(lever_arm)
+        return self._H[0:3]
+
+    def _update_H_vel(self) -> NDArray[np.float64]:
+        """Update and return part of H matrix relevant for velocity aiding."""
+        return self._H[3:6]
+
+    def _update_H_g_ref(self, R_nm: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Update and return part of H matrix relevant for g_ref aiding."""
+        S = _skew_symmetric
+        self._H[6:9, 6:9] = S(R_nm.T @ self._vg_ref_n)
+        return self._H[6:9]
+
+    def _update_H_head(self, q_nm: NDArray[np.float64]) -> NDArray[np.float64]:
+        """Update and return part of H matrix relevant for heading aiding."""
+        self._H[9:10, 6:9] = _dhda_head(q_nm)
+        return self._H[9:10]
+
+    @staticmethod
+    def _prep_W(
+        err_acc: dict[str, float], err_gyro: dict[str, float]
+    ) -> NDArray[np.float64]:
+        """Prepare white noise power spectral density matrix"""
+        N_acc = err_acc["N"]
+        sigma_acc = err_acc["B"]
+        beta_acc = 1.0 / err_acc["tau_cb"]
+        N_gyro = err_gyro["N"]
+        sigma_gyro = err_gyro["B"]
+        beta_gyro = 1.0 / err_gyro["tau_cb"]
+
+        # White noise power spectral density matrix
+        W = np.eye(12)
+        W[0:3, 0:3] *= N_acc**2
+        W[3:6, 3:6] *= N_gyro**2
+        W[6:9, 6:9] *= 2.0 * sigma_acc**2 * beta_acc
+        W[9:12, 9:12] *= 2.0 * sigma_gyro**2 * beta_gyro
+        return W
+
+    def _reset_ins(self, dx: NDArray[np.float64]) -> None:
+        """Combine states and reset INS"""
+        da = dx[6:9]
+        self._dq_prealloc[1:4] = da
+        dq = (1.0 / np.sqrt(4.0 + da.T @ da)) * self._dq_prealloc
+        self._ins._x[:3] = self._ins._x[:3] + dx[:3]
+        self._ins._x[3:6] = self._ins._x[3:6] + dx[3:6]
+        self._ins._x[6:10] = _quaternion_product(self._ins._x[6:10], dq)
+        self._ins._x[6:10] = _normalize(self._ins._x[6:10])
+        self._ins._x[-3:] = self._ins._x[-3:] + dx[-3:]
+        if not self._ignore_bias_acc:
+            self._ins._x[10:13] = self._ins._x[10:13] + dx[9:12]
+        self._dx_prealloc[:] = np.zeros(dx.size)
+
+    @staticmethod
+    @njit  # type: ignore[misc]
+    def _update_dx_P(
+        dx: NDArray[np.float64],
+        P: NDArray[np.float64],
+        dz: NDArray[np.float64],
+        var: NDArray[np.float64],
+        H: NDArray[np.float64],
+        I_: NDArray[np.float64],
+    ) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+        for i, (dz_i, var_i) in enumerate(zip(dz, var)):
+            H_i = np.ascontiguousarray(H[i, :])
+            K_i = P @ H_i.T / (H_i @ P @ H_i.T + var_i)
+            dx += K_i * (dz_i - H_i @ dx)
+            K_i = np.ascontiguousarray(K_i[:, np.newaxis])  # as 2D array
+            H_i = np.ascontiguousarray(H_i[np.newaxis, :])  # as 2D array
+            P = (I_ - K_i @ H_i) @ P @ (I_ - K_i @ H_i).T + var_i * K_i @ K_i.T
+        return dx, P
+
+    def _align_vertical(self, f_ins, head, head_degrees):
+        """
+        Vertical alignment.
+
+        Estimate the attitude (roll and pitch) of the IMU sensor relative to the
+        navigation frame using accelerometer measurements and the known direction
+        of gravity. Assumes a static sensor; i.e., negligible linear acceleration.
+
+        Parameters
+        ----------
+        f_ins : array-like, shape (3,)
+            Bias-compensated specific force measurements (fx, fy, fz).
+        head : float, optional
+            Heading of measurement frame relative to navigation frame.
+        head_degrees : bool, default False
+            Specifies whether the heading is given in degrees or radians.
+        """
+        if head is None:
+            head = _h_head(self.quaternion())
+        else:
+            if head_degrees:
+                head = (np.pi / 180.0) * head
+
+        roll, pitch = _roll_pitch_from_acc(f_ins, self._ins._nav_frame)
+        self._ins._x[6:10] = _quaternion_from_euler(np.array([roll, pitch, head]))
+        self._x[:] = self._ins._x
+
+    def update(
+        self,
+        f_imu: ArrayLike,
+        w_imu: ArrayLike,
+        degrees: bool = False,
+        pos: ArrayLike | None = None,
+        pos_var: ArrayLike | None = None,
+        vel: ArrayLike | None = None,
+        vel_var: ArrayLike | None = None,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = True,
+        g_ref: bool = False,
+        g_var: ArrayLike | None = None,
+    ) -> Self:
+        """
+        Update/correct the AINS' state estimate with aiding measurements, and project
+        ahead using IMU measurements.
+
+        If no aiding measurements are provided, the AINS is simply propagated ahead
+        using dead reckoning with the IMU measurements.
+
+        Parameters
+        ----------
+        f_imu : array-like, shape (3,)
+            Specific force measurements (i.e., accelerations + gravity), given
+            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
+            acceleration measurements in x-, y-, and z-direction, respectively.
+        w_imu : array-like, shape (3,)
+            Angular rate measurements, given as [w_x, w_y, w_z]^T where
+            w_x, w_y and w_z are angular rates about the x-, y-,
+            and z-axis, respectively.
+        degrees : bool, default False
+            Specifies whether the unit of ``w_imu`` are in degrees or radians.
+        pos : array-like, shape (3,), optional
+            Position aiding measurement in m. If ``None``, position aiding is not used.
+        pos_var : array-like, shape (3,), optional
+            Variance of position measurement noise in m^2. Required for ``pos``.
+        vel : array-like, shape (3,), optional
+            Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used.
+        vel_var : array-like, shape (3,), optional
+            Variance of velocity measurement noise in (m/s)^2. Required for ``vel``.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+        g_ref : bool, optional, default False
+            Specifies whether the gravity reference vector is used as an aiding measurement.
+        g_var : array-like, shape (3,), optional
+            Variance of gravitational reference vector measurement noise. Required for
+            ``g_ref``.
+
+        Returns
+        -------
+        AidedINS
+            A reference to the instance itself after the update.
+        """
+
+        f_imu = np.asarray(f_imu, dtype=float)
+        w_imu = np.asarray(w_imu, dtype=float)
+
+        if degrees:
+            w_imu = (np.pi / 180.0) * w_imu
+
+        # Bias compensated IMU measurements
+        f_ins = f_imu - self._ins._bias_acc
+        w_ins = w_imu - self._ins._bias_gyro
+
+        # Initial vertical alignment (i.e., roll and pitch calibration)
+        if self._cold:
+            self._align_vertical(f_ins, head, head_degrees)
+            self._cold = False
+
+        # Current INS state estimates
+        pos_ins = self._ins._pos
+        vel_ins = self._ins._vel
+        q_ins_nm = self._ins._q_nm
+        R_ins_nm = _rot_matrix_from_quaternion(q_ins_nm)  # body-to-inertial rot matrix
+
+        # Aliases
+        dx = self._dx_prealloc  # zeros
+        dt = self._dt
+        F = self._F
+        G = self._G
+        W = self._W
+        P = self._P_prior
+        I_ = self._I
+
+        # Lever arm vector - IMU-to-aiding
+        lever_arm = self._lever_arm
+
+        # Update system matrices
+        self._update_F(R_ins_nm, f_ins, w_ins)
+        self._update_G(R_ins_nm)
+
+        # Update with available aiding measurements
+        if pos is not None:
+            if pos_var is None:
+                raise ValueError("'pos_var' not provided.")
+
+            pos = np.asarray(pos, dtype=float, order="C")
+            pos_var = np.asarray(pos_var, dtype=float, order="C")
+            dz_pos = pos - pos_ins - R_ins_nm @ lever_arm
+            H_pos = self._update_H_pos(R_ins_nm, lever_arm)
+            dx, P = self._update_dx_P(dx, P, dz_pos, pos_var, H_pos, I_)
+
+        if vel is not None:
+            if vel_var is None:
+                raise ValueError("'vel_var' not provided.")
+
+            vel = np.asarray(vel, dtype=float, order="C")
+            vel_var = np.asarray(vel_var, dtype=float, order="C")
+            dz_vel = vel - vel_ins
+            H_vel = self._update_H_vel()
+            dx, P = self._update_dx_P(dx, P, dz_vel, vel_var, H_vel, I_)
+
+        if g_ref:
+            if g_var is None:
+                raise ValueError("'g_var' not provided.")
+            vg_meas_m = -_normalize(f_ins)
+            g_var = np.asarray(g_var, dtype=float, order="C")
+            dz_g = vg_meas_m - R_ins_nm.T @ self._vg_ref_n
+            H_g = self._update_H_g_ref(R_ins_nm)
+            dx, P = self._update_dx_P(dx, P, dz_g, g_var, H_g, I_)
+
+        if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' not provided.")
+
+            if head_degrees:
+                head = (np.pi / 180.0) * head
+                head_var = (np.pi / 180.0) ** 2 * head_var
+
+            head_var_ = np.asarray([head_var], dtype=float, order="C")
+            dz_head = np.asarray(
+                [_signed_smallest_angle(head - _h_head(q_ins_nm), degrees=False)],
+                dtype=float,
+                order="C",
+            )
+
+            H_head = self._update_H_head(q_ins_nm)
+            dx, P = self._update_dx_P(dx, P, dz_head, head_var_, H_head, I_)
+
+        self._dx[:] = dx.ravel().copy()
+
+        # Reset INS state
+        if dx.any():
+            self._reset_ins(dx.ravel())
+
+        # Discretize system
+        self._phi[:] = I_ + dt * F  # state transition matrix
+        Q = dt * G @ W @ G.T  # process noise covariance matrix
+
+        # Update current state
+        self._x[:] = self._ins._x
+        self._P[:] = P
+
+        # Project ahead
+        self._ins.update(f_imu, w_imu, degrees=False)
+        self._P_prior[:] = self._phi @ P @ self._phi.T + Q
+
+        return self
+
+
+class VRU(AidedINS):
+    """
+    Vertical Reference Unit (VRU) based on a multiplicative extended Kalman filter
+    (MEKF).
+
+    VRU is intended for applicatoins with negligble sustained linear accelerations.
+    For applications with sustained linear accelerations, accurate position and/or
+    velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for
+    those cases.
+
+    This class inherits from :class:`smsfusion.AidedINS` but applies sensible
+    defaults for vertical reference applications and simplifies the interface by
+    hiding non-essential configuration options.
+
+    Velocity aiding is set to zero with a default standard deviation of 10 m/s.
+    Position aiding also assumes zero values but with high uncertainty (default
+    standard deviation of 1000 m), making it effectively non-constraining. Heading
+    aiding is completely disabled.
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
+        Initial (a priori) 16-element INS state estimate:
+
+        * Position (x, y, z) - 3 elements
+        * Velocity (x, y, z) - 3 elements
+        * Attitude (unit quaternion) - 4 elements
+        * Accelerometer bias (x, y, z) - 3 elements
+        * Gyroscope bias (x, y, z) - 3 elements
+
+        Defaults to a zero vector, but with the attitude part as a unit quaternion
+        (i.e., no rotation).
+    P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
+        Initial (a priori) estimate of the error covariance matrix, **P**.
+    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
+        Dictionary containing accelerometer noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
+        * ``B``: Bias stability in m/s^2.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
+        Dictionary containing gyroscope noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
+        * ``B``: Bias stability in rad/s.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    g : float, default 9.80665
+        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame. Furthermore, the aiding heading angle is
+        also interpreted relative to this frame according to the right-hand rule.
+    cold_start : bool, default True
+        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
+        A cold state indicates that the provided initial conditions are uncertain,
+        and possibly far from the true state. Thus, to reduce the risk of divergence,
+        an initial vertical alignment (i.e., roll and pitch calibration) is performed
+        using accelerometer measurements and the known direction of gravity during
+        the first measurement update. The IMU should remain stationary with negligible
+        linear acceleration during a cold start; otherwise, divergence may occur.
+        A warm start, on the other hand, assumes accurate initial conditions, and
+        initializes the Kalman filter immediately without any initial roll and pitch
+        calibration.
+    **kwargs :
+        Ignored. For compatibility with parent class.
+    """
+
+    def __init__(
+        self,
+        fs: float,
+        x0_prior: ArrayLike = X0,
+        P0_prior: ArrayLike = P0,
+        err_acc: dict[str, float] = ERR_ACC_MOTION2,
+        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+        cold_start: bool = True,
+        **kwargs: dict[str, Any],
+    ) -> None:
+        super().__init__(
+            fs=fs,
+            x0_prior=x0_prior,
+            P0_prior=P0_prior,
+            err_acc=err_acc,
+            err_gyro=err_gyro,
+            g=g,
+            nav_frame=nav_frame,
+            lever_arm=np.zeros(3),
+            ignore_bias_acc=True,
+            cold_start=cold_start,
+        )
+
+    def update(
+        self,
+        f_imu: ArrayLike,
+        w_imu: ArrayLike,
+        degrees: bool = False,
+        pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]),
+        vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]),
+    ) -> Self:
+        """
+        Update/correct the VRU's state estimate with pseudo aiding measurements
+        (i.e., zero velocity and zero position with corresponding variances), and
+        project ahead using IMU measurements.
+
+        Parameters
+        ----------
+        f_imu : array-like, shape (3,)
+            Specific force measurements (i.e., accelerations + gravity), given
+            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
+            acceleration measurements in x-, y-, and z-direction, respectively.
+        w_imu : array-like, shape (3,)
+            Angular rate measurements, given as [w_x, w_y, w_z]^T where
+            w_x, w_y and w_z are angular rates about the x-, y-,
+            and z-axis, respectively.
+        degrees : bool, default False
+            Specifies whether the unit of ``w_imu`` are in degrees or radians.
+        pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
+            Variance of position measurement noise in m^2. Defaults to
+            standard deviation of 1000 m, while assuming zero position.
+        vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
+            Variance of velocity measurement noise in (m/s)^2. Defaults to
+            standard deviation of 10 m/s, while assuming zero velocity.
+
+        Returns
+        -------
+        VRU
+            A reference to the instance itself after the update.
+        """
+        return super().update(
+            f_imu,
+            w_imu,
+            degrees=degrees,
+            pos=np.array([0.0, 0.0, 0.0]),
+            pos_var=pos_var,
+            vel=np.array([0.0, 0.0, 0.0]),
+            vel_var=vel_var,
+            head=None,
+            head_var=None,
+        )
+
+
+class AHRS(AidedINS):
+    """
+    Attitude and Heading Reference System (AHRS) based on a multiplicative extended
+    Kalman filter (MEKF).
+
+    AHRS is intended for applicatoins with negligble sustained linear accelerations.
+    For applications with sustained linear accelerations, accurate position and/or
+    velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for
+    those cases.
+
+    This class inherits from :class:`smsfusion.AidedINS` but applies sensible
+    defaults for attitude heading reference applications and simplifies the
+    interface by hiding non-essential configuration options.
+
+    Velocity aiding is set to zero with a default standard deviation of 10 m/s.
+    Position aiding also assumes zero values but with high uncertainty (default
+    standard deviation of 1000 m), making it effectively non-constraining.
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0`
+        Initial (a priori) 16-element INS state estimate:
+
+        * Position (x, y, z) - 3 elements
+        * Velocity (x, y, z) - 3 elements
+        * Attitude (unit quaternion) - 4 elements
+        * Accelerometer bias (x, y, z) - 3 elements
+        * Gyroscope bias (x, y, z) - 3 elements
+
+        Defaults to a zero vector, but with the attitude part as a unit quaternion
+        (i.e., no rotation).
+    P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`)
+        Initial (a priori) estimate of the error covariance matrix, **P**.
+    err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2`
+        Dictionary containing accelerometer noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz).
+        * ``B``: Bias stability in m/s^2.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2`
+        Dictionary containing gyroscope noise parameters with keys:
+
+        * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz).
+        * ``B``: Bias stability in rad/s.
+        * ``tau_cb``: Bias correlation time in seconds.
+
+        Defaults to error characteristics of SMS Motion gen. 2.
+    g : float, default 9.80665
+        The gravitational acceleration. Default is 'standard gravity' of 9.80665.
+    nav_frame : {'NED', 'ENU'}, default 'NED'
+        Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down)
+        (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom
+        will be expressed relative to this frame. Furthermore, the aiding heading angle is
+        also interpreted relative to this frame according to the right-hand rule.
+    cold_start : bool, default True
+        Whether to start the AINS filter in a 'cold' (default) or 'warm' state.
+        A cold state indicates that the provided initial conditions are uncertain,
+        and possibly far from the true state. Thus, to reduce the risk of divergence,
+        an initial vertical alignment (i.e., roll and pitch calibration) is performed
+        using accelerometer measurements and the known direction of gravity during
+        the first measurement update. The IMU should remain stationary with negligible
+        linear acceleration during a cold start; otherwise, divergence may occur.
+        A warm start, on the other hand, assumes accurate initial conditions, and
+        initializes the Kalman filter immediately without any initial roll and pitch
+        calibration.
+    **kwargs :
+        Ignored. For compatibility with parent class.
+    """
+
+    def __init__(
+        self,
+        fs: float,
+        x0_prior: ArrayLike = X0,
+        P0_prior: ArrayLike = P0,
+        err_acc: dict[str, float] = ERR_ACC_MOTION2,
+        err_gyro: dict[str, float] = ERR_GYRO_MOTION2,
+        g: float = 9.80665,
+        nav_frame: str = "NED",
+        cold_start: bool = True,
+        **kwargs: dict[str, Any],
+    ) -> None:
+        super().__init__(
+            fs=fs,
+            x0_prior=x0_prior,
+            P0_prior=P0_prior,
+            err_acc=err_acc,
+            err_gyro=err_gyro,
+            g=g,
+            nav_frame=nav_frame,
+            lever_arm=np.zeros(3),
+            ignore_bias_acc=True,
+            cold_start=cold_start,
+        )
+
+    def update(
+        self,
+        f_imu: ArrayLike,
+        w_imu: ArrayLike,
+        degrees: bool = False,
+        head: float | None = None,
+        head_var: float | None = None,
+        head_degrees: bool = True,
+        pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]),
+        vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]),
+    ) -> Self:
+        """
+        Update/correct the AHRS' state estimate with pseudo aiding measurements
+        (i.e., zero velocity and zero position with corresponding variances), and
+        project ahead using IMU measurements.
+
+        Parameters
+        ----------
+        f_imu : array-like, shape (3,)
+            Specific force measurements (i.e., accelerations + gravity), given
+            as [f_x, f_y, f_z]^T where f_x, f_y and f_z are
+            acceleration measurements in x-, y-, and z-direction, respectively.
+        w_imu : array-like, shape (3,)
+            Angular rate measurements, given as [w_x, w_y, w_z]^T where
+            w_x, w_y and w_z are angular rates about the x-, y-,
+            and z-axis, respectively.
+        degrees : bool, default False
+            Specifies whether the unit of ``w_imu`` are in degrees or radians.
+        head : float, optional
+            Heading measurement. I.e., the yaw angle of the 'body' frame relative to the
+            assumed 'navigation' frame ('NED' or 'ENU') specified during initialization.
+            If ``None``, compass aiding is not used. See ``head_degrees`` for units.
+        head_var : float, optional
+            Variance of heading measurement noise. Units must be compatible with ``head``.
+             See ``head_degrees`` for units. Required for ``head``.
+        head_degrees : bool, default False
+            Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
+            or radians and radians^2. Default is in radians and radians^2.
+        pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
+            Variance of position measurement noise in m^2. Defaults to
+            standard deviation of 1000 m, while assuming zero position.
+        vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
+            Variance of velocity measurement noise in (m/s)^2. Defaults to
+            standard deviation of 10 m/s, while assuming zero velocity.
+
+        Returns
+        -------
+        AHRS
+            A reference to the instance itself after the update.
+        """
+        return super().update(
+            f_imu,
+            w_imu,
+            degrees=degrees,
+            pos=np.array([0.0, 0.0, 0.0]),
+            pos_var=pos_var,
+            vel=np.array([0.0, 0.0, 0.0]),
+            vel_var=vel_var,
+            head=head,
+            head_var=head_var,
+            head_degrees=head_degrees,
+        )
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins_/test_ains.py
index 47845a4e..9ff23df5 100644
--- a/tests/test_ins_/test_ains.py
+++ b/tests/test_ins_/test_ains.py
@@ -3,7 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._ains_ import (
+from smsfusion._ins._ains import (
     AINS,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
@@ -252,7 +252,7 @@ def test_init_default(self):
         np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
         np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
         np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains_._P0))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(12))
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))

From da0f831574b733c477a100a0a3b2d7f281e68ade Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:04:37 +0200
Subject: [PATCH 186/217] rename old ains module to legacy

---
 src/smsfusion/_ins/__init__.py                       | 2 +-
 src/smsfusion/_ins/{_ains_old.py => _ains_legacy.py} | 0
 tests/{_test_ins.py => _test_ins_legacy.py}          | 0
 3 files changed, 1 insertion(+), 1 deletion(-)
 rename src/smsfusion/_ins/{_ains_old.py => _ains_legacy.py} (100%)
 rename tests/{_test_ins.py => _test_ins_legacy.py} (100%)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index 0a619081..7addc780 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,5 +1,5 @@
 from ._ahrs import AHRS as AHRSv2
-from ._ains_old import AHRS, VRU, AidedINS, StrapdownINS
+from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS
 from ._ains import AINS as AINSv2
 from ._utils import FixedNED, euler_from_acc, gravity
 from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins/_ains_old.py b/src/smsfusion/_ins/_ains_legacy.py
similarity index 100%
rename from src/smsfusion/_ins/_ains_old.py
rename to src/smsfusion/_ins/_ains_legacy.py
diff --git a/tests/_test_ins.py b/tests/_test_ins_legacy.py
similarity index 100%
rename from tests/_test_ins.py
rename to tests/_test_ins_legacy.py

From 5101e3fc8e082cb7699a36264b26bc06e4aa5ac3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:05:01 +0200
Subject: [PATCH 187/217] delete old obsolete tests of version 2

---
 tests/_test_ins_v2.py | 106 ------------------------------------------
 1 file changed, 106 deletions(-)
 delete mode 100644 tests/_test_ins_v2.py

diff --git a/tests/_test_ins_v2.py b/tests/_test_ins_v2.py
deleted file mode 100644
index ab21d85f..00000000
--- a/tests/_test_ins_v2.py
+++ /dev/null
@@ -1,106 +0,0 @@
-import numpy as np
-import pytest
-from scipy.signal import resample_poly
-
-import smsfusion as sf
-from smsfusion._ins._ahrs import AHRSv2
-from smsfusion.benchmark import (
-    benchmark_full_pva_beat_202311A,
-    benchmark_full_pva_chirp_202311A,
-)
-
-
-class Test_v2:
-    @pytest.mark.parametrize(
-        "benchmark_gen",
-        [benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A],
-    )
-    def test_benchmark(self, benchmark_gen):
-        fs_imu = 100.0
-        fs_aiding = 1.0
-        fs_ratio = np.ceil(fs_imu / fs_aiding)
-        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-        compass_noise_std = np.radians(0.5)
-        vel_noise_std = 0.1
-
-        # Reference signals (without noise)
-        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-        # IMU measurements (with noise)
-        bg = np.array([0.01, -0.02, 0.015])
-        noise_model = sf.noise.IMUNoise(
-            err_acc=sf.constants.ERR_ACC_MOTION2,
-            err_gyro=sf.constants.ERR_GYRO_MOTION2,
-            seed=0,
-        )
-        imu_noise = noise_model(fs_imu, len(t))
-        acc_noise = acc_ref + imu_noise[:, :3]
-        gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
-
-        # Aiding measurements (with noise)
-        rng = np.random.default_rng(seed=42)
-        head_meas = euler_ref[:, 2] + compass_noise_std * rng.standard_normal(
-            euler_ref.shape[0]
-        )
-        vel_meas = vel_ref + vel_noise_std * rng.standard_normal(vel_ref.shape)
-
-        # MEKF
-        v0 = vel_ref[0]
-        q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-        mekf = AHRSv2(
-            fs_imu,
-            v=v0,
-            q=q0,
-            acc_noise_density=sf.constants.ERR_ACC_MOTION2["N"],
-            gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"],
-            gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"],
-            gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"],
-        )
-
-        # Apply filter
-        vel_out, euler_out, bias_gyro_out = [], [], []
-        for i, (f_i, w_i, v_i, h_i) in enumerate(
-            zip(acc_noise, gyro_noise, vel_meas, head_meas)
-        ):
-            if not (i % fs_ratio):  # with aiding
-                mekf.update(
-                    f_i / fs_imu,
-                    w_i / fs_imu,
-                    degrees=False,
-                    vel=v_i,
-                    vel_var=vel_noise_std**2 * np.ones(3),
-                    head=h_i,
-                    head_var=compass_noise_std**2,
-                    head_degrees=False,
-                )
-            else:  # without aiding
-                mekf.update(f_i / fs_imu, w_i / fs_imu, degrees=False)
-            vel_out.append(mekf.velocity())
-            euler_out.append(mekf.euler(degrees=False))
-            bias_gyro_out.append(mekf.bias_gyro(degrees=False))
-
-        vel_out = np.array(vel_out)
-        euler_out = np.array(euler_out)
-        bias_gyro_out = np.array(bias_gyro_out)
-
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
-        vel_out = resample_poly(vel_out, 2, 1)[1:-1:2]
-        vel_ref = vel_ref[:-1, :]
-        euler_out = resample_poly(euler_out, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
-
-        vel_x_rms, vel_y_rms, vel_z_rms = np.std((vel_out - vel_ref)[warmup:], axis=0)
-        roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0)
-        bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std(
-            (bias_gyro_out - bg)[warmup:], axis=0
-        )
-
-        assert vel_x_rms <= 0.05
-        assert vel_y_rms <= 0.05
-        assert vel_z_rms <= 0.05
-        assert np.degrees(roll_rms) <= 0.1
-        assert np.degrees(pitch_rms) <= 0.1
-        assert np.degrees(yaw_rms) <= 0.2
-        assert np.degrees(bias_gyro_x_rms) <= 0.005
-        assert np.degrees(bias_gyro_y_rms) <= 0.005
-        assert np.degrees(bias_gyro_z_rms) <= 0.005

From 1332c478435c4044cf68cd8f84abcebfd3f955d6 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:23:24 +0200
Subject: [PATCH 188/217] clean up legacy module, delete duplicate code

---
 src/smsfusion/_ins/__init__.py                |   2 +-
 src/smsfusion/_ins/_ains_legacy.py            | 237 +-----------------
 ..._test_ins_legacy.py => test_ins_legacy.py} |   4 +-
 3 files changed, 5 insertions(+), 238 deletions(-)
 rename tests/{_test_ins_legacy.py => test_ins_legacy.py} (99%)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index 7addc780..ce58166e 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,5 +1,5 @@
 from ._ahrs import AHRS as AHRSv2
-from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS
 from ._ains import AINS as AINSv2
+from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS
 from ._utils import FixedNED, euler_from_acc, gravity
 from ._vru import VRU as VRUv2
diff --git a/src/smsfusion/_ins/_ains_legacy.py b/src/smsfusion/_ins/_ains_legacy.py
index f39423e4..8960647b 100644
--- a/src/smsfusion/_ins/_ains_legacy.py
+++ b/src/smsfusion/_ins/_ains_legacy.py
@@ -6,6 +6,7 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
+from smsfusion._ins._common import _dhda_head, _h_head, _signed_smallest_angle
 from smsfusion._transforms import (
     _angular_matrix_from_quaternion,
     _euler_from_quaternion,
@@ -49,175 +50,6 @@ def _roll_pitch_from_acc(f, nav_frame):
     return roll, pitch
 
 
-class FixedNED:
-    """
-    Convert position coordinates between a fixed NED frame (x, y, z) and ECEF frame
-    (lattitude, longitude, height).
-
-    The fixed NED frame is a tangential plane on the WGS-84 ellipsoid with its origin
-    fixed at the provided reference coordinates. It is assumed that the tangential
-    plane is close to the ellipsoid surface.
-
-    Parameters
-    ----------
-    lat_ref: float
-        Reference latitude coordinate in decimal degrees.
-    lon_ref: float
-        Reference longitude coordinate in decimal degrees.
-    height_ref: ref
-        Reference height coordinate in decimal degrees.
-    """
-
-    def __init__(self, lat_ref: float, lon_ref: float, height_ref: float) -> None:
-        self._lat_ref = lat_ref
-        self._lon_ref = lon_ref
-        self._height_ref = height_ref
-
-        radius_eq = 6_378_137  # equatorial radius (WGS-84)
-        radius_polar = 6_356_752.314245  # polar radius (WGS-84)
-
-        radius_ratio_squared = (radius_polar / radius_eq) ** 2
-        denom = np.cos(
-            self._lat_ref * (np.pi / 180.0)
-        ) ** 2 + radius_ratio_squared * np.sin(self._lat_ref * (np.pi / 180.0))
-
-        self._Rn = radius_eq / np.sqrt(denom)  # radius prime vertical
-        self._Rm = self._Rn * radius_ratio_squared / denom  # radius meridian
-
-        self._Rm_h = self._Rm + height_ref
-        self._Rn_h_cos = (self._Rn + height_ref) * np.cos(
-            self._lat_ref * (np.pi / 180.0)
-        )
-
-    def to_llh(self, x: float, y: float, z: float) -> tuple[float, float, float]:
-        """
-        Compute longitude, latitude, and height coordinates (WGS-84) from local
-        Cartesian coordinates in the fixed NED frame.
-
-        Parameters
-        ----------
-        x: float
-            Local x-coordinate in meters in the fixed NED frame.
-        y: float
-            Local y-coordinate in meters in the fixed NED frame.
-        z: float
-            Local z-coordinate in meters in the fixed NED frame.
-
-        Returns
-        -------
-        lat: float
-            Latitude coordinate in decimal degrees.
-        lon: float
-            Longitude coordinate in decimal degrees.
-        height: float
-            Height coordinate in meters.
-        """
-        dlat = (180.0 / np.pi) * x / self._Rm_h
-        dlon = (180.0 / np.pi) * y / self._Rn_h_cos
-
-        lat = _signed_smallest_angle(self._lat_ref + dlat, degrees=True)
-        lon = _signed_smallest_angle(self._lon_ref + dlon, degrees=True)
-        h = self._height_ref - z
-        return lat, lon, h
-
-    def to_xyz(
-        self, lat: float, lon: float, height: float
-    ) -> tuple[float, float, float]:
-        """
-        Compute local Cartesian coordinates in the fixed NED frame from longitude,
-        latitude, and height coordinates (WGS-84).
-
-        Parameters
-        ----------
-        lat: float
-            Latitude coordinate in decimal degrees.
-        lon: float
-            Longitude coordinate in decimal degrees.
-        height: float
-            Height coordinate in meters.
-
-        Returns
-        -------
-        x: float
-            Local x-coordinate in meters in the fixed NED frame.
-        y: float
-            Local y-coordinate in meters in the fixed NED frame.
-        z: float
-            Local z-coordinate in meters in the fixed NED frame.
-        """
-        dlat = lat - self._lat_ref
-        dlon = lon - self._lon_ref
-
-        x = (np.pi / 180.0) * dlat * self._Rm_h
-        y = (np.pi / 180.0) * dlon * self._Rn_h_cos
-        z = self._height_ref - height
-        return x, y, z
-
-
-def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
-    """
-    Convert the given angle to the smallest angle between [-180., 180) degrees.
-
-    Parameters
-    ----------
-    angle : float
-        Value of angle.
-    degrees : bool, default True
-        Specify whether ``angle`` is given degrees or radians.
-
-    Returns
-    -------
-    float
-        The smallest angle between [-180., 180) degrees (or  [-pi, pi] radians).
-    """
-    base = 180.0 if degrees else np.pi
-    return (angle + base) % (2.0 * base) - base
-
-
-def gravity(lat: float | None = None, degrees: bool = True) -> float:
-    """
-    Calculates the gravitational acceleration based on the World Geodetic System
-    (1984) Ellipsoidal Gravity Formula (WGS-84).
-
-    The WGS-84 formula is given by::
-
-        g = g_e * (1 - k * sin(lat)^2) / sqrt(1 - e^2 * sin(lat)^2)
-
-    where, ::
-
-        g_e = 9.780325335903891718546
-        k = 0.00193185265245827352087
-        e^2 = 0.006694379990141316996137
-
-    and ``lat`` is the latitude.
-
-    If no latitude is provided, the 'standard gravity', ``g_0``, is returned instead.
-    The standard gravity is by definition of the ISO/IEC 8000 given as
-    ``g_0 = 9.80665``.
-
-    Parameters
-    ----------
-    lat : float, optional
-        Latitude. If none provided, the 'standard gravity' is returned.
-    degrees : bool, optional
-        Specify whether the latitude, ``lat``, is in degrees or radians.
-        Applicapble only if ``lat`` is provided.
-    """
-    if lat is None:
-        g_0 = 9.80665  # standard gravity in m/s^2
-        return g_0
-
-    g_e = 9.780325335903891718546  # gravity at equator
-    k = 0.00193185265245827352087  # formula constant
-    e_2 = 0.006694379990141316996137  # spheroid's squared eccentricity
-
-    if degrees:
-        lat = (np.pi / 180.0) * lat
-
-    g = g_e * (1.0 + k * np.sin(lat) ** 2.0) / np.sqrt(1.0 - e_2 * np.sin(lat) ** 2.0)
-    return g  # type: ignore[no-any-return]  # numpy funcs declare Any as return when given scalar-like
-
-
 class INSMixin:
     """
     Mixin class for inertial navigation systems (INS).
@@ -553,73 +385,6 @@ def update(
         return self
 
 
-def _h_head(q: NDArray[np.float64]) -> float:
-    """
-    Compute yaw angle from unit quaternion.
-
-    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
-    unit quaternion here to avoid singularities.
-
-    Parameters
-    ----------
-    q : numpy.ndarray, shape (4,)
-        Unit quaternion.
-
-    Returns
-    -------
-    float
-        Yaw angle in the NED reference frame.
-
-    References
-    ----------
-    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
-    2nd Edition, equation 14.251, John Wiley & Sons, 2021.
-    """
-    q_w, q_x, q_y, q_z = q
-    u_y = 2.0 * (q_x * q_y + q_z * q_w)
-    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
-    return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
-
-
-@njit  # type: ignore[misc]
-def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
-    """
-    Compute yaw angle gradient wrt to the unit quaternion.
-
-    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
-    unit quaternion here to avoid singularities.
-
-    Parameters
-    ----------
-    q : numpy.ndarray, shape (3,)
-        Unit quaternion.
-
-    Returns
-    -------
-    numpy.ndarray, shape (3,)
-        Yaw angle gradient vector.
-
-    References
-    ----------
-    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
-    2nd Edition, equation 14.254, John Wiley & Sons, 2021.
-    """
-    q_w, q_x, q_y, q_z = q
-    u_y = 2.0 * (q_x * q_y + q_z * q_w)
-    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
-    u = u_y / u_x
-
-    duda_scale = 1.0 / u_x**2
-    duda_x = -(q_w * q_y) * (1.0 - 2.0 * q_w**2) - (2.0 * q_w**2 * q_x * q_z)
-    duda_y = (q_w * q_x) * (1.0 - 2.0 * q_z**2) + (2.0 * q_w**2 * q_y * q_z)
-    duda_z = q_w**2 * (1.0 - 2.0 * q_y**2) + (2.0 * q_w * q_x * q_y * q_z)
-    duda = duda_scale * np.array([duda_x, duda_y, duda_z])
-
-    dhda = 1.0 / (1.0 + u**2) * duda
-
-    return dhda  # type: ignore[no-any-return]
-
-
 class AidedINS(INSMixin):
     """
     Aided inertial navigation system (AINS) using a multiplicative extended
diff --git a/tests/_test_ins_legacy.py b/tests/test_ins_legacy.py
similarity index 99%
rename from tests/_test_ins_legacy.py
rename to tests/test_ins_legacy.py
index e34409af..f0aa54d9 100644
--- a/tests/_test_ins_legacy.py
+++ b/tests/test_ins_legacy.py
@@ -18,7 +18,7 @@
 from scipy.spatial.transform import Rotation
 
 import smsfusion as sf
-from smsfusion._ins import (  # INSMixin,; _dhda_head,; _h_head,; _roll_pitch_from_acc,; _signed_smallest_angle,
+from smsfusion._ins import (
     AHRS,
     VRU,
     AidedINS,
@@ -26,6 +26,8 @@
     StrapdownINS,
     gravity,
 )
+from smsfusion._ins._ains_legacy import INSMixin, _roll_pitch_from_acc
+from smsfusion._ins._common import _dhda_head, _h_head, _signed_smallest_angle
 from smsfusion._transforms import (
     _angular_matrix_from_quaternion,
     _rot_matrix_from_quaternion,

From f849757e3550c281c022e9048f46485e1049e5a4 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:29:48 +0200
Subject: [PATCH 189/217] remove trailing underscore from ins test folder name

---
 tests/{test_ins_ => test_ins}/__init__.py    | 0
 tests/{test_ins_ => test_ins}/test_ahrs.py   | 0
 tests/{test_ins_ => test_ins}/test_ains.py   | 0
 tests/{test_ins_ => test_ins}/test_common.py | 0
 tests/{test_ins_ => test_ins}/test_utils.py  | 0
 tests/{test_ins_ => test_ins}/test_vru.py    | 0
 6 files changed, 0 insertions(+), 0 deletions(-)
 rename tests/{test_ins_ => test_ins}/__init__.py (100%)
 rename tests/{test_ins_ => test_ins}/test_ahrs.py (100%)
 rename tests/{test_ins_ => test_ins}/test_ains.py (100%)
 rename tests/{test_ins_ => test_ins}/test_common.py (100%)
 rename tests/{test_ins_ => test_ins}/test_utils.py (100%)
 rename tests/{test_ins_ => test_ins}/test_vru.py (100%)

diff --git a/tests/test_ins_/__init__.py b/tests/test_ins/__init__.py
similarity index 100%
rename from tests/test_ins_/__init__.py
rename to tests/test_ins/__init__.py
diff --git a/tests/test_ins_/test_ahrs.py b/tests/test_ins/test_ahrs.py
similarity index 100%
rename from tests/test_ins_/test_ahrs.py
rename to tests/test_ins/test_ahrs.py
diff --git a/tests/test_ins_/test_ains.py b/tests/test_ins/test_ains.py
similarity index 100%
rename from tests/test_ins_/test_ains.py
rename to tests/test_ins/test_ains.py
diff --git a/tests/test_ins_/test_common.py b/tests/test_ins/test_common.py
similarity index 100%
rename from tests/test_ins_/test_common.py
rename to tests/test_ins/test_common.py
diff --git a/tests/test_ins_/test_utils.py b/tests/test_ins/test_utils.py
similarity index 100%
rename from tests/test_ins_/test_utils.py
rename to tests/test_ins/test_utils.py
diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins/test_vru.py
similarity index 100%
rename from tests/test_ins_/test_vru.py
rename to tests/test_ins/test_vru.py

From 171f59125e1b523d9f552521780cde37eadf7de7 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:32:22 +0200
Subject: [PATCH 190/217] comment fix

---
 src/smsfusion/_ins/_vru.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 48af24c2..3547796d 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -335,7 +335,7 @@ def update(
         dtheta = dtheta - self._dt * self._bg_b
 
         # Update state-space model
-        _state_transition_matrix_update(self._phi, dtheta)  # -> update phi
+        _state_transition_matrix_update(self._phi, dtheta)  # -> update phi (in place)
 
         # Project (a priori) state estimates ahead
         _update_quaternion_with_rotvec(self._q_nb, dtheta)  # -> update q_nb (in place)

From 62a586fda7075bbea502e7f48e9f20334cb9c578 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:51:58 +0200
Subject: [PATCH 191/217] test kalman update sequential

---
 tests/test_ins/test_common.py | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py
index f97ecb80..9d99e4be 100644
--- a/tests/test_ins/test_common.py
+++ b/tests/test_ins/test_common.py
@@ -217,3 +217,30 @@ def test__gref_b_from_quat(q_nb, nav_frame_factor):
 def test__nz2vg():
     assert _common._nz2vg("NED") == 1.0
     assert _common._nz2vg("ENU") == -1.0
+
+
+def test_kalman_update_sequential():
+
+    rng = np.random.default_rng(42)
+
+    m = 10  # number of measurements
+    n = 12  # state dimension
+
+    x = rng.random(n)
+    A = rng.random((n, n))
+    P = A @ A.T + np.eye(n)  # positive semi-definite
+    H = rng.random((m, n))
+    var = rng.random(m)
+    z = rng.random(m)
+
+    x_upd = x.copy()
+    P_upd = P.copy()
+    _common._kalman_update_sequential(x_upd, P_upd, z, var, H)
+
+    R = np.diag(var)
+    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
+    x_expect = x + K @ (z - H @ x)
+    P_expect = (np.eye(n) - K @ H) @ P @ (np.eye(n) - K @ H).T + K @ R @ K.T
+
+    np.testing.assert_allclose(x_upd, x_expect)
+    np.testing.assert_allclose(P_upd, P_expect)

From 4639aaae0525b8dfcd67e4b85da73e212957ba54 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:54:37 +0200
Subject: [PATCH 192/217] test kalman update scalar

---
 tests/test_ins/test_common.py | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py
index 9d99e4be..f7195473 100644
--- a/tests/test_ins/test_common.py
+++ b/tests/test_ins/test_common.py
@@ -244,3 +244,30 @@ def test_kalman_update_sequential():
 
     np.testing.assert_allclose(x_upd, x_expect)
     np.testing.assert_allclose(P_upd, P_expect)
+
+
+def test_kalman_update_scalar():
+
+    rng = np.random.default_rng(42)
+
+    n = 9  # state dimension
+
+    x = rng.random(n)
+    A = rng.random((n, n))
+    P = A @ A.T + np.eye(n)  # positive semi-definite
+    h = rng.random(n)
+    r = rng.random()
+    z = rng.random()
+
+    x_upd = x.copy()
+    P_upd = P.copy()
+    _common._kalman_update_scalar(x_upd, P_upd, z, r, h)
+
+    R = np.array([[r]])
+    H = h.reshape(1, n)
+    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
+    x_expect = x + K @ (z - H @ x)
+    P_expect = (np.eye(n) - K @ H) @ P @ (np.eye(n) - K @ H).T + K @ R @ K.T
+
+    np.testing.assert_allclose(x_upd, x_expect)
+    np.testing.assert_allclose(P_upd, P_expect)

From 8d5f59679f1cb8edb234a10dd830c957393ce201 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 10:57:34 +0200
Subject: [PATCH 193/217] test project covariance ahead

---
 tests/test_ins/test_common.py | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py
index f7195473..1a440209 100644
--- a/tests/test_ins/test_common.py
+++ b/tests/test_ins/test_common.py
@@ -271,3 +271,23 @@ def test_kalman_update_scalar():
 
     np.testing.assert_allclose(x_upd, x_expect)
     np.testing.assert_allclose(P_upd, P_expect)
+
+
+def test_project_covariance_ahead():
+
+    rng = np.random.default_rng(42)
+
+    n = 6  # state dimension
+
+    A = rng.random((n, n))
+    P = A @ A.T + np.eye(n)  # positive semi-definite
+    phi = rng.random((n, n))
+    A = rng.random((n, n))
+    Q = A @ A.T + np.eye(n)  # positive semi-definite
+
+    P_proj = P.copy()
+    _common._project_covariance_ahead(P_proj, phi, Q)
+
+    P_expect = phi @ P @ phi.T + Q
+
+    np.testing.assert_allclose(P_proj, P_expect)

From 64255a49297c68feae9dd15deb70539862e9b4cd Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 11:06:06 +0200
Subject: [PATCH 194/217] test rename meas params

---
 tests/test_ins/test_ahrs.py | 16 ++++++++--------
 tests/test_ins/test_ains.py | 16 ++++++++--------
 tests/test_ins/test_vru.py  |  8 ++++----
 3 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 57702f8c..34e07b26 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -253,13 +253,13 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
         bg = np.array([0.01, -0.02, 0.03])  # rad/s
         imu_noise = noise_model(fs_imu, len(t))
-        acc_imu = acc_ref + imu_noise[:, :3]
-        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
         vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
         head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
         if gyro_degrees:
-            gyro_imu = np.degrees(gyro_imu)
+            gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
         mekf = AHRS(
@@ -272,7 +272,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         )
 
         vel_est, euler_est, bias_gyro_est = [], [], []
-        for f_i, w_i, h_i, v_i in zip(acc_imu, gyro_imu, head_meas, vel_meas):
+        for f_i, w_i, h_i, v_i in zip(acc_meas, gyro_meas, head_meas, vel_meas):
 
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu
@@ -342,11 +342,11 @@ def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
         bg = np.array([0.01, -0.02, 0.0])  # rad/s
         imu_noise = noise_model(fs_imu, len(t))
-        acc_imu = acc_ref + imu_noise[:, :3]
-        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
 
         if gyro_degrees:
-            gyro_imu = np.degrees(gyro_imu)
+            gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
         mekf = AHRS(
@@ -359,7 +359,7 @@ def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         )
 
         euler_est, bias_gyro_est = [], []
-        for f_i, w_i in zip(acc_imu, gyro_imu):
+        for f_i, w_i in zip(acc_meas, gyro_meas):
 
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index 9ff23df5..c461bd66 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -324,14 +324,14 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
         bg = np.array([0.01, -0.02, 0.03])  # rad/s
         imu_noise = noise_model(fs_imu, len(t))
-        acc_imu = acc_ref + imu_noise[:, :3]
-        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
         pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape)
         vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape)
         head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
         if gyro_degrees:
-            gyro_imu = np.degrees(gyro_imu)
+            gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
         mekf = AINS(
@@ -346,7 +346,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
 
         pos_est, vel_est, euler_est, bias_gyro_est = [], [], [], []
         for f_i, w_i, h_i, p_i, v_i in zip(
-            acc_imu, gyro_imu, head_meas, pos_meas, vel_meas
+            acc_meas, gyro_meas, head_meas, pos_meas, vel_meas
         ):
 
             dvel_i = f_i / fs_imu
@@ -427,11 +427,11 @@ def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
         bg = np.array([0.01, -0.02, 0.0])  # rad/s
         imu_noise = noise_model(fs_imu, len(t))
-        acc_imu = acc_ref + imu_noise[:, :3]
-        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
 
         if gyro_degrees:
-            gyro_imu = np.degrees(gyro_imu)
+            gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
         mekf = AINS(
@@ -445,7 +445,7 @@ def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
         )
 
         euler_est, bias_gyro_est = [], []
-        for f_i, w_i in zip(acc_imu, gyro_imu):
+        for f_i, w_i in zip(acc_meas, gyro_meas):
 
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index eb2900ab..b57b79a6 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -214,12 +214,12 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
         bg = np.array([0.01, -0.02, 0.03])  # rad/s
         imu_noise = noise_model(fs_imu, len(t))
-        acc_imu = acc_ref + imu_noise[:, :3]
-        gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
         head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
         if gyro_degrees:
-            gyro_imu = np.degrees(gyro_imu)
+            gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
         mekf = VRU(
@@ -231,7 +231,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         )
 
         euler_est, bias_gyro_est = [], []
-        for f_i, w_i, h_i in zip(acc_imu, gyro_imu, head_meas):
+        for f_i, w_i, h_i in zip(acc_meas, gyro_meas, head_meas):
 
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu

From e220ca6589295b7a69761c8acea31a0fb6100136 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 11:07:05 +0200
Subject: [PATCH 195/217] delete commented out test vru

---
 tests/test_ins/test_vru.py | 74 --------------------------------------
 1 file changed, 74 deletions(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index b57b79a6..08acb936 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -273,77 +273,3 @@ def rmse(ref, est):
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
-
-    # @pytest.mark.parametrize(
-    #     "benchmark_gen, gyro_degrees",
-    #     [
-    #         (benchmark_full_pva_beat_202311A, False),
-    #         (benchmark_full_pva_chirp_202311A, True),
-    #     ],
-    # )
-    # def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
-    #     fs_imu = 10.0
-    #     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-    #     # Reference signals (without noise)
-    #     t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-    #     # IMU and aiding measurements (with noise)
-    #     err_acc = sf.constants.ERR_ACC_MOTION2
-    #     err_gyro = sf.constants.ERR_GYRO_MOTION2
-    #     noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
-    #     bg = np.array([0.01, -0.02, 0.0])  # rad/s
-    #     imu_noise = noise_model(fs_imu, len(t))
-    #     acc_imu = acc_ref + imu_noise[:, :3]
-    #     gyro_imu = gyro_ref + imu_noise[:, 3:] + bg
-
-    #     if gyro_degrees:
-    #         gyro_imu = np.degrees(gyro_imu)
-
-    #     # MEKF
-    #     mekf = AHRS(
-    #         fs_imu,
-    #         v0=vel_ref[0],
-    #         q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
-    #         gyro_noise_density=err_gyro["N"],
-    #         gyro_bias_stability=err_gyro["B"],
-    #         gyro_bias_corr_time=err_gyro["tau_cb"],
-    #     )
-
-    #     euler_est, bias_gyro_est = [], []
-    #     for f_i, w_i in zip(acc_imu, gyro_imu):
-
-    #         dvel_i = f_i / fs_imu
-    #         dtheta_i = w_i / fs_imu
-
-    #         mekf.update(
-    #             dvel_i,
-    #             dtheta_i,
-    #             degrees=gyro_degrees,
-    #             vel=np.zeros(3),
-    #             vel_var=100.0 * np.ones(3),
-    #             gref=True,
-    #             gref_var=(0.001, 0.001, 0.001),
-    #         )
-    #         euler_est.append(mekf.euler(degrees=False))
-    #         bias_gyro_est.append(mekf.bias_gyro())
-
-    #     euler_est = np.array(euler_est)
-    #     bias_gyro_est = np.array(bias_gyro_est)
-
-    #     # Half-sample shift (compensates for the delay introduced by Euler integration)
-    #     euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
-    #     bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-    #     euler_ref = euler_ref[:-1, :]
-    #     bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
-
-    #     def rmse(ref, est):
-    #         return np.sqrt(np.mean((ref - est) ** 2, axis=0))
-
-    #     roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
-    #     bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
-
-    #     assert np.degrees(roll_rmse) <= 0.5
-    #     assert np.degrees(pitch_rmse) <= 0.5
-    #     assert np.degrees(bgx_rmse) <= 0.01
-    #     assert np.degrees(bgy_rmse) <= 0.01

From 6a5e70e495278049fd60fa2082817199d9fe49a8 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 12:50:50 +0200
Subject: [PATCH 196/217] add default aiding to VRU

---
 src/smsfusion/_ins/_vru.py |  8 ++---
 tests/test_ins/test_vru.py | 70 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 74 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 3547796d..fd40486f 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -286,8 +286,8 @@ def update(
         head: float | None = None,
         head_var: float | None = None,
         head_degrees: bool = False,
-        gref: bool = False,
-        gref_var: ArrayLike | None = None,
+        gref: bool = True,
+        gref_var: ArrayLike | None = (0.0001, 0.0001, 0.0001),
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -315,10 +315,10 @@ def update(
             and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
         gref : bool, optional
             Specifies whether to use accelerometer measurements (dvel) and the known
-            direction of gravity as aiding. Defaults to ``False``.
+            direction of gravity as aiding. Defaults to ``True``.
         gref_var : array_like, shape (3,), optional
             Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding.
+            Required for gravity reference vector aiding. Defaults to (0.0001, 0.0001, 0.0001).
 
         Returns
         -------
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 08acb936..b92191d6 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -273,3 +273,73 @@ def rmse(ref, est):
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
+
+    @pytest.mark.parametrize(
+        "benchmark_gen, gyro_degrees",
+        [
+            (benchmark_full_pva_beat_202311A, False),
+            (benchmark_full_pva_chirp_202311A, True),
+        ],
+    )
+    def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        head_std = np.radians(0.1)  # rad
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.0])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
+        head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
+
+        if gyro_degrees:
+            gyro_meas = np.degrees(gyro_meas)
+
+        # MEKF
+        mekf = VRU(
+            fs_imu,
+            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
+            gyro_noise_density=err_gyro["N"],
+            gyro_bias_stability=err_gyro["B"],
+            gyro_bias_corr_time=err_gyro["tau_cb"],
+        )
+
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i, h_i in zip(acc_meas, gyro_meas, head_meas):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(dvel_i, dtheta_i, degrees=gyro_degrees)
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        vel_ref = vel_ref[:-1, :]
+        euler_ref = euler_ref[:-1, :]
+        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(
+            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
+        )
+
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(bgx_rmse) <= 0.02
+        assert np.degrees(bgy_rmse) <= 0.02

From cf8e98e7ea1a38ce12e40d0343172861fa2d3984 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 12:55:11 +0200
Subject: [PATCH 197/217] default aiding test fix

---
 tests/test_ins/test_vru.py | 22 ++++++----------------
 1 file changed, 6 insertions(+), 16 deletions(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index b92191d6..86500b22 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -286,10 +286,9 @@ def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
         # Reference signals (without noise)
-        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+        t, _, _, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
         # IMU and aiding measurements (with noise)
-        head_std = np.radians(0.1)  # rad
         err_acc = sf.constants.ERR_ACC_MOTION2
         err_gyro = sf.constants.ERR_GYRO_MOTION2
         noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
@@ -297,22 +296,16 @@ def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
         imu_noise = noise_model(fs_imu, len(t))
         acc_meas = acc_ref + imu_noise[:, :3]
         gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
-        head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref))
 
         if gyro_degrees:
             gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
-        mekf = VRU(
-            fs_imu,
-            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
-            gyro_noise_density=err_gyro["N"],
-            gyro_bias_stability=err_gyro["B"],
-            gyro_bias_corr_time=err_gyro["tau_cb"],
-        )
+        q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+        mekf = VRU(fs_imu, q0=q0)
 
         euler_est, bias_gyro_est = [], []
-        for f_i, w_i, h_i in zip(acc_meas, gyro_meas, head_meas):
+        for f_i, w_i in zip(acc_meas, gyro_meas):
 
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu
@@ -327,17 +320,14 @@ def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
         # Half-sample shift (compensates for the delay introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        vel_ref = vel_ref[:-1, :]
         euler_ref = euler_ref[:-1, :]
-        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+        bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
             return np.sqrt(np.mean((ref - est) ** 2, axis=0))
 
         roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
-        bgx_rmse, bgy_rmse, _ = rmse(
-            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
-        )
+        bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
 
         assert np.degrees(roll_rmse) <= 0.6
         assert np.degrees(pitch_rmse) <= 0.6

From 16a1a967cebb62aa63f0ffdbbd6eba539e522fc3 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 12:57:29 +0200
Subject: [PATCH 198/217] test fix

---
 tests/test_ins/test_vru.py | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 86500b22..53d5fb9e 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -275,13 +275,13 @@ def rmse(ref, est):
         assert np.degrees(bgz_rmse) <= 0.01
 
     @pytest.mark.parametrize(
-        "benchmark_gen, gyro_degrees",
+        "benchmark_gen",
         [
-            (benchmark_full_pva_beat_202311A, False),
-            (benchmark_full_pva_chirp_202311A, True),
+            benchmark_full_pva_beat_202311A,
+            benchmark_full_pva_chirp_202311A,
         ],
     )
-    def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
+    def test_benchmark_default_aiding(self, benchmark_gen):
         fs_imu = 10.0
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
@@ -297,9 +297,6 @@ def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
         acc_meas = acc_ref + imu_noise[:, :3]
         gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
 
-        if gyro_degrees:
-            gyro_meas = np.degrees(gyro_meas)
-
         # MEKF
         q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
         mekf = VRU(fs_imu, q0=q0)
@@ -310,7 +307,7 @@ def test_benchmark_default_aiding(self, benchmark_gen, gyro_degrees):
             dvel_i = f_i / fs_imu
             dtheta_i = w_i / fs_imu
 
-            mekf.update(dvel_i, dtheta_i, degrees=gyro_degrees)
+            mekf.update(dvel_i, dtheta_i, degrees=False)
             euler_est.append(mekf.euler(degrees=False))
             bias_gyro_est.append(mekf.bias_gyro())
 

From 771ec3dff0844b8bfec67a98a5cd1658e3cd3d5e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:02:42 +0200
Subject: [PATCH 199/217] add default aiding to AHRS

---
 src/smsfusion/_ins/_ahrs.py |  8 +++---
 tests/test_ins/test_ahrs.py | 57 +++++++++++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+), 4 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index dc960c7c..a101a385 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -397,8 +397,8 @@ def update(
         dvel: ArrayLike,
         dtheta: ArrayLike,
         degrees: bool = False,
-        vel: ArrayLike | None = None,
-        vel_var: ArrayLike | None = None,
+        vel: ArrayLike | None = (0.0, 0.0, 0.0),
+        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
         head: float | None = None,
         head_var: float | None = None,
         head_degrees: bool = False,
@@ -420,10 +420,10 @@ def update(
             degrees or radians. Defaults to radians.
         vel : array-like, shape (3,), optional
             Velocity aiding measurement in m/s. If ``None``, velocity aiding is
-            not used.
+            not used. Defaults to zero velocity (stationary).
         vel_var : array-like, shape (3,), optional
             Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
-            is ``None``.
+            is ``None``. Defaults to (100.0, 100.0, 100.0) (m/s)^2.
         head : float, optional
             Heading measurement in radians or degrees depending on the ``head_degrees``
             flag. I.e., the yaw angle of the 'body' frame relative to the assumed
diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 34e07b26..10ad942b 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -395,3 +395,60 @@ def rmse(ref, est):
         assert np.degrees(pitch_rmse) <= 0.5
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
+
+    @pytest.mark.parametrize(
+        "benchmark_gen",
+        [
+            benchmark_full_pva_beat_202311A,
+            benchmark_full_pva_chirp_202311A,
+        ],
+    )
+    def test_benchmark_default_aiding(self, benchmark_gen):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, _, _, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.0])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
+
+        # MEKF
+        q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+        mekf = AHRS(fs_imu, q0=q0)
+
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i in zip(acc_meas, gyro_meas):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(dvel_i, dtheta_i, degrees=False)
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+        bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
+
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(bgx_rmse) <= 0.02
+        assert np.degrees(bgy_rmse) <= 0.02

From 25c0abeddc8b7acf0f813a1c40d1c8cf851af0a8 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:07:14 +0200
Subject: [PATCH 200/217] add default values to ains

---
 src/smsfusion/_ins/_ahrs.py |  2 +-
 src/smsfusion/_ins/_ains.py | 16 +++++------
 tests/test_ins/test_ains.py | 57 +++++++++++++++++++++++++++++++++++++
 3 files changed, 66 insertions(+), 9 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index a101a385..b382287b 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -423,7 +423,7 @@ def update(
             not used. Defaults to zero velocity (stationary).
         vel_var : array-like, shape (3,), optional
             Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
-            is ``None``. Defaults to (100.0, 100.0, 100.0) (m/s)^2.
+            is ``None``. Defaults to (100.0, 100.0, 100.0).
         head : float, optional
             Heading measurement in radians or degrees depending on the ``head_degrees``
             flag. I.e., the yaw angle of the 'body' frame relative to the assumed
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index 6daea77d..83c81e7d 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -436,10 +436,10 @@ def update(
         dvel: ArrayLike,
         dtheta: ArrayLike,
         degrees: bool = False,
-        pos: ArrayLike | None = None,
-        pos_var: ArrayLike | None = None,
-        vel: ArrayLike | None = None,
-        vel_var: ArrayLike | None = None,
+        pos: ArrayLike | None = (0.0, 0.0, 0.0),
+        pos_var: ArrayLike | None = (1_000_000.0, 1_000_000.0, 1_000_000.0),
+        vel: ArrayLike | None = (0.0, 0.0, 0.0),
+        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
         head: float | None = None,
         head_var: float | None = None,
         head_degrees: bool = False,
@@ -461,16 +461,16 @@ def update(
             degrees or radians. Defaults to radians.
         pos : array-like, shape (3,), optional
             Position aiding measurement in meters. If ``None``, position aiding
-            is not used.
+            is not used. Defaults to (0.0, 0.0, 0.0) (i.e., origin).
         pos_var : array-like, shape (3,), optional
             Variance of position measurement noise in m^2. Ignored if ``pos`` is
-            ``None``.
+            ``None``. Defaults to (1_000_000.0, 1_000_000.0, 1_000_000.0).
         vel : array-like, shape (3,), optional
             Velocity aiding measurement in m/s. If ``None``, velocity aiding is
-            not used.
+            not used. Defaults to (0.0, 0.0, 0.0) (i.e., zero velocity).
         vel_var : array-like, shape (3,), optional
             Variance of velocity measurement noise in (m/s)^2. Ignored if ``vel``
-            is ``None``.
+            is ``None``. Defaults to (100.0, 100.0, 100.0).
         head : float, optional
             Heading measurement in radians or degrees depending on the ``head_degrees``
             flag. I.e., the yaw angle of the 'body' frame relative to the assumed
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index c461bd66..c4901697 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -483,3 +483,60 @@ def rmse(ref, est):
         assert np.degrees(pitch_rmse) <= 0.5
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
+
+    @pytest.mark.parametrize(
+        "benchmark_gen",
+        [
+            benchmark_full_pva_beat_202311A,
+            benchmark_full_pva_chirp_202311A,
+        ],
+    )
+    def test_benchmark_default_aiding(self, benchmark_gen):
+        fs_imu = 10.0
+        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
+
+        # Reference signals (without noise)
+        t, _, _, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+
+        # IMU and aiding measurements (with noise)
+        err_acc = sf.constants.ERR_ACC_MOTION2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2
+        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
+        bg = np.array([0.01, -0.02, 0.0])  # rad/s
+        imu_noise = noise_model(fs_imu, len(t))
+        acc_meas = acc_ref + imu_noise[:, :3]
+        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
+
+        # MEKF
+        q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
+        mekf = AINS(fs_imu, q0=q0)
+
+        euler_est, bias_gyro_est = [], []
+        for f_i, w_i in zip(acc_meas, gyro_meas):
+
+            dvel_i = f_i / fs_imu
+            dtheta_i = w_i / fs_imu
+
+            mekf.update(dvel_i, dtheta_i, degrees=False)
+            euler_est.append(mekf.euler(degrees=False))
+            bias_gyro_est.append(mekf.bias_gyro())
+
+        euler_est = np.array(euler_est)
+        bias_gyro_est = np.array(bias_gyro_est)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
+        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+        bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
+
+        def rmse(ref, est):
+            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
+
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
+
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(bgx_rmse) <= 0.02
+        assert np.degrees(bgy_rmse) <= 0.02

From 21e884fec9ebc160bbaf19be9c5998463968681e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:14:33 +0200
Subject: [PATCH 201/217] delete test aind bench gref

---
 tests/test_ins/test_ains.py | 79 +------------------------------------
 1 file changed, 2 insertions(+), 77 deletions(-)

diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index c4901697..774661d1 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -363,6 +363,8 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
                 pos_var=pos_std**2 * np.ones(3),
                 vel=v_i,
                 vel_var=vel_std**2 * np.ones(3),
+                gref=True,
+                gref_var=(0.1, 0.1, 0.1),  # high uncertainty
             )
             pos_est.append(mekf.position())
             vel_est.append(mekf.velocity())
@@ -407,83 +409,6 @@ def rmse(ref, est):
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
 
-    @pytest.mark.parametrize(
-        "benchmark_gen, gyro_degrees",
-        [
-            (benchmark_full_pva_beat_202311A, False),
-            (benchmark_full_pva_chirp_202311A, True),
-        ],
-    )
-    def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
-        fs_imu = 10.0
-        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-        # Reference signals (without noise)
-        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-        # IMU and aiding measurements (with noise)
-        err_acc = sf.constants.ERR_ACC_MOTION2
-        err_gyro = sf.constants.ERR_GYRO_MOTION2
-        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
-        bg = np.array([0.01, -0.02, 0.0])  # rad/s
-        imu_noise = noise_model(fs_imu, len(t))
-        acc_meas = acc_ref + imu_noise[:, :3]
-        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
-
-        if gyro_degrees:
-            gyro_meas = np.degrees(gyro_meas)
-
-        # MEKF
-        mekf = AINS(
-            fs_imu,
-            p0=pos_ref[0],
-            v0=vel_ref[0],
-            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
-            gyro_noise_density=err_gyro["N"],
-            gyro_bias_stability=err_gyro["B"],
-            gyro_bias_corr_time=err_gyro["tau_cb"],
-        )
-
-        euler_est, bias_gyro_est = [], []
-        for f_i, w_i in zip(acc_meas, gyro_meas):
-
-            dvel_i = f_i / fs_imu
-            dtheta_i = w_i / fs_imu
-
-            mekf.update(
-                dvel_i,
-                dtheta_i,
-                degrees=gyro_degrees,
-                pos=np.zeros(3),
-                pos_var=1_000_000.0 * np.ones(3),
-                vel=np.zeros(3),
-                vel_var=100.0 * np.ones(3),
-                gref=True,
-                gref_var=(0.001, 0.001, 0.001),
-            )
-            euler_est.append(mekf.euler(degrees=False))
-            bias_gyro_est.append(mekf.bias_gyro())
-
-        euler_est = np.array(euler_est)
-        bias_gyro_est = np.array(bias_gyro_est)
-
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
-        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
-        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
-        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
-
-        def rmse(ref, est):
-            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
-
-        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
-        bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
-
-        assert np.degrees(roll_rmse) <= 0.5
-        assert np.degrees(pitch_rmse) <= 0.5
-        assert np.degrees(bgx_rmse) <= 0.01
-        assert np.degrees(bgy_rmse) <= 0.01
-
     @pytest.mark.parametrize(
         "benchmark_gen",
         [

From 7c8b92dbeb84d80cba69baa838fae559c57aac32 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:16:49 +0200
Subject: [PATCH 202/217] delete test ahrs bench gref

---
 tests/test_ins/test_ahrs.py | 76 +------------------------------------
 tests/test_ins/test_ains.py |  2 +-
 2 files changed, 3 insertions(+), 75 deletions(-)

diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 10ad942b..6d7fd309 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -286,6 +286,8 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
                 head_degrees=False,
                 vel=v_i,
                 vel_var=vel_std**2 * np.ones(3),
+                gref=True,
+                gref_var=(0.1, 0.1, 0.1),
             )
             vel_est.append(mekf.velocity())
             euler_est.append(mekf.euler(degrees=False))
@@ -322,80 +324,6 @@ def rmse(ref, est):
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
 
-    @pytest.mark.parametrize(
-        "benchmark_gen, gyro_degrees",
-        [
-            (benchmark_full_pva_beat_202311A, False),
-            (benchmark_full_pva_chirp_202311A, True),
-        ],
-    )
-    def test_benchmark_with_gref_aiding(self, benchmark_gen, gyro_degrees):
-        fs_imu = 10.0
-        warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
-
-        # Reference signals (without noise)
-        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
-
-        # IMU and aiding measurements (with noise)
-        err_acc = sf.constants.ERR_ACC_MOTION2
-        err_gyro = sf.constants.ERR_GYRO_MOTION2
-        noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0)
-        bg = np.array([0.01, -0.02, 0.0])  # rad/s
-        imu_noise = noise_model(fs_imu, len(t))
-        acc_meas = acc_ref + imu_noise[:, :3]
-        gyro_meas = gyro_ref + imu_noise[:, 3:] + bg
-
-        if gyro_degrees:
-            gyro_meas = np.degrees(gyro_meas)
-
-        # MEKF
-        mekf = AHRS(
-            fs_imu,
-            v0=vel_ref[0],
-            q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
-            gyro_noise_density=err_gyro["N"],
-            gyro_bias_stability=err_gyro["B"],
-            gyro_bias_corr_time=err_gyro["tau_cb"],
-        )
-
-        euler_est, bias_gyro_est = [], []
-        for f_i, w_i in zip(acc_meas, gyro_meas):
-
-            dvel_i = f_i / fs_imu
-            dtheta_i = w_i / fs_imu
-
-            mekf.update(
-                dvel_i,
-                dtheta_i,
-                degrees=gyro_degrees,
-                vel=np.zeros(3),
-                vel_var=100.0 * np.ones(3),
-                gref=True,
-                gref_var=(0.001, 0.001, 0.001),
-            )
-            euler_est.append(mekf.euler(degrees=False))
-            bias_gyro_est.append(mekf.bias_gyro())
-
-        euler_est = np.array(euler_est)
-        bias_gyro_est = np.array(bias_gyro_est)
-
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
-        euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
-        bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
-        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
-
-        def rmse(ref, est):
-            return np.sqrt(np.mean((ref - est) ** 2, axis=0))
-
-        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
-        bgx_rmse, bgy_rmse, _ = rmse(bias_gyro_ref[warmup:], bias_gyro_est[warmup:])
-
-        assert np.degrees(roll_rmse) <= 0.5
-        assert np.degrees(pitch_rmse) <= 0.5
-        assert np.degrees(bgx_rmse) <= 0.01
-        assert np.degrees(bgy_rmse) <= 0.01
-
     @pytest.mark.parametrize(
         "benchmark_gen",
         [
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index 774661d1..cf9f9d5f 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -364,7 +364,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
                 vel=v_i,
                 vel_var=vel_std**2 * np.ones(3),
                 gref=True,
-                gref_var=(0.1, 0.1, 0.1),  # high uncertainty
+                gref_var=(0.1, 0.1, 0.1),
             )
             pos_est.append(mekf.position())
             vel_est.append(mekf.velocity())

From 4815e256206a0abc6b003d364a441592a2bcc87c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:28:46 +0200
Subject: [PATCH 203/217] comment fix update phi

---
 src/smsfusion/_ins/_ains.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index 83c81e7d..26955f5f 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -103,7 +103,7 @@ def _state_transition_matrix_update(
     r10, r11, r12 = R_nb[1]
     r20, r21, r22 = R_nb[2]
 
-    # phi[6:9, 6:9] = np.eye(3) - dt * S(w_b)
+    # Equivalent to: phi[6:9, 6:9] = np.eye(3) - S(dtheta)
     phi[6, 7] = dtz
     phi[6, 8] = -dty
     phi[7, 6] = -dtz
@@ -111,7 +111,7 @@ def _state_transition_matrix_update(
     phi[8, 6] = dty
     phi[8, 7] = -dtx
 
-    # phi[3:6, 6:9] = -dt * R_nb @ S(f_b)
+    # Equivalent to: phi[3:6, 6:9] = -R_nb @ S(dvel)
     phi[3, 6] = -dvz * r01 + dvy * r02
     phi[4, 6] = -dvz * r11 + dvy * r12
     phi[5, 6] = -dvz * r21 + dvy * r22

From 479661171f26c9170dcdb86b9b9f0fff58bd5307 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 13:46:06 +0200
Subject: [PATCH 204/217] rename yaw methods + set default radians in signed
 smalles angle

---
 src/smsfusion/_ins/_ahrs.py        |  6 +++---
 src/smsfusion/_ins/_aiding.py      |  4 ++--
 src/smsfusion/_ins/_ains.py        |  6 +++---
 src/smsfusion/_ins/_ains_legacy.py | 22 ++++++++++++++++------
 src/smsfusion/_ins/_common.py      | 23 ++++++++++++-----------
 src/smsfusion/_ins/_vru.py         |  6 +++---
 tests/test_ins/test_common.py      |  6 +++---
 tests/test_ins_legacy.py           | 18 +++++++++++-------
 8 files changed, 53 insertions(+), 38 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index b382287b..f0a879c3 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -9,12 +9,12 @@
 
 from ._aiding import _aiding_update_gref, _aiding_update_head, _aiding_update_vel
 from ._common import (
-    _dhda_head,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
+    _yaw_gradient,
 )
 
 _P0 = (
@@ -167,7 +167,7 @@ def _measurement_matrix_init(
     vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
     H = np.zeros((7, 9))
     H[0:3, 0:3] = np.eye(3)  # velocity
-    H[3:4, 3:6] = _dhda_head(q_nb)  # heading
+    H[3:4, 3:6] = _yaw_gradient(q_nb)  # heading
     H[4:7, 3:6] = _skew_symmetric(vg_b)  # gravity reference vector
     return H
 
@@ -492,7 +492,7 @@ def update(
                 raise ValueError("'head_var' is required for heading aiding.")
 
             # Update measurement matrix (heading row)
-            self._H[3, 6:9] = _dhda_head(self._q_nb)
+            self._H[3, 6:9] = _yaw_gradient(self._q_nb)
 
             # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
diff --git a/src/smsfusion/_ins/_aiding.py b/src/smsfusion/_ins/_aiding.py
index 0042075a..0625578e 100644
--- a/src/smsfusion/_ins/_aiding.py
+++ b/src/smsfusion/_ins/_aiding.py
@@ -5,10 +5,10 @@
 from smsfusion._vectorops import _normalize
 
 from ._common import (
-    _h_head,
     _kalman_update_scalar,
     _kalman_update_sequential,
     _signed_smallest_angle,
+    _yaw_from_quaternion,
 )
 
 
@@ -72,7 +72,7 @@ def _aiding_update_head(
         head_meas = (np.pi / 180.0) * head_meas
         head_var = (np.pi / 180.0) ** 2 * head_var
 
-    dz = _signed_smallest_angle(head_meas - _h_head(q_nb))
+    dz = _signed_smallest_angle(head_meas - _yaw_from_quaternion(q_nb))
     _kalman_update_scalar(dx, P, dz, head_var, H)  # -> update dx and P (in place)
 
 
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index 26955f5f..bb8b4d71 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -14,12 +14,12 @@
     _aiding_update_vel,
 )
 from ._common import (
-    _dhda_head,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
+    _yaw_gradient,
 )
 
 _P0 = (
@@ -182,7 +182,7 @@ def _measurement_matrix_init(
     H[0:3, 0:3] = np.eye(3)  # position
     H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     H[3:6, 3:6] = np.eye(3)  # velocity
-    H[6:7, 6:9] = _dhda_head(q_nb)  # heading
+    H[6:7, 6:9] = _yaw_gradient(q_nb)  # heading
     H[7:10, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
     return H
 
@@ -557,7 +557,7 @@ def update(
                 raise ValueError("'head_var' is required for heading aiding.")
 
             # Update measurement matrix (heading row)
-            self._H[6, 6:9] = _dhda_head(self._q_nb)
+            self._H[6, 6:9] = _yaw_gradient(self._q_nb)
 
             # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
diff --git a/src/smsfusion/_ins/_ains_legacy.py b/src/smsfusion/_ins/_ains_legacy.py
index 8960647b..977e2962 100644
--- a/src/smsfusion/_ins/_ains_legacy.py
+++ b/src/smsfusion/_ins/_ains_legacy.py
@@ -6,7 +6,11 @@
 from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
-from smsfusion._ins._common import _dhda_head, _h_head, _signed_smallest_angle
+from smsfusion._ins._common import (
+    _signed_smallest_angle,
+    _yaw_from_quaternion,
+    _yaw_gradient,
+)
 from smsfusion._transforms import (
     _angular_matrix_from_quaternion,
     _euler_from_quaternion,
@@ -700,9 +704,11 @@ def _update_H_g_ref(self, R_nm: NDArray[np.float64]) -> NDArray[np.float64]:
         self._H[6:9, 6:9] = S(R_nm.T @ self._vg_ref_n)
         return self._H[6:9]
 
-    def _update_H_head(self, q_nm: NDArray[np.float64]) -> NDArray[np.float64]:
+    def _update_yaw_from_quaternion(
+        self, q_nm: NDArray[np.float64]
+    ) -> NDArray[np.float64]:
         """Update and return part of H matrix relevant for heading aiding."""
-        self._H[9:10, 6:9] = _dhda_head(q_nm)
+        self._H[9:10, 6:9] = _yaw_gradient(q_nm)
         return self._H[9:10]
 
     @staticmethod
@@ -776,7 +782,7 @@ def _align_vertical(self, f_ins, head, head_degrees):
             Specifies whether the heading is given in degrees or radians.
         """
         if head is None:
-            head = _h_head(self.quaternion())
+            head = _yaw_from_quaternion(self.quaternion())
         else:
             if head_degrees:
                 head = (np.pi / 180.0) * head
@@ -926,12 +932,16 @@ def update(
 
             head_var_ = np.asarray([head_var], dtype=float, order="C")
             dz_head = np.asarray(
-                [_signed_smallest_angle(head - _h_head(q_ins_nm), degrees=False)],
+                [
+                    _signed_smallest_angle(
+                        head - _yaw_from_quaternion(q_ins_nm), degrees=False
+                    )
+                ],
                 dtype=float,
                 order="C",
             )
 
-            H_head = self._update_H_head(q_ins_nm)
+            H_head = self._update_yaw_from_quaternion(q_ins_nm)
             dx, P = self._update_dx_P(dx, P, dz_head, head_var_, H_head, I_)
 
         self._dx[:] = dx.ravel().copy()
diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index 2219d904..9532282c 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -6,16 +6,16 @@
 
 
 @njit  # type: ignore[misc]
-def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
+def _yaw_gradient(q: NDArray[np.float64]) -> NDArray[np.float64]:
     """
-    Compute yaw angle gradient wrt to the unit quaternion.
+    Compute yaw/heading angle gradient wrt to the unit quaternion.
 
     Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
     unit quaternion here to avoid singularities.
 
     Parameters
     ----------
-    q : numpy.ndarray, shape (3,)
+    q : numpy.ndarray, shape (4,)
         Unit quaternion.
 
     Returns
@@ -45,9 +45,9 @@ def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
 
 
 @njit  # type: ignore[misc]
-def _h_head(q: NDArray[np.float64]) -> float:
+def _yaw_from_quaternion(q: NDArray[np.float64]) -> float:
     """
-    Compute yaw angle from unit quaternion.
+    Compute yaw/heading angle from unit quaternion.
 
     Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
     unit quaternion here to avoid singularities.
@@ -74,21 +74,22 @@ def _h_head(q: NDArray[np.float64]) -> float:
 
 
 @njit  # type: ignore[misc]
-def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
+def _signed_smallest_angle(angle: float, degrees: bool = False) -> float:
     """
-    Convert the given angle to the smallest angle between [-180., 180) degrees.
+    Convert the given angle to the smallest angle between [-pi, pi) radians or [-180, 180)
+    degrees.
 
     Parameters
     ----------
     angle : float
-        Value of angle.
-    degrees : bool, default True
-        Specify whether ``angle`` is given degrees or radians.
+        Angle in radians or degrees depending on the ``degrees`` parameter.
+    degrees : bool, optional
+        Specify whether ``angle`` is given degrees or radians (default).
 
     Returns
     -------
     float
-        The smallest angle between [-180., 180) degrees (or  [-pi, pi] radians).
+        The smallest angle between [-pi, pi) radians or [-180, 180) degrees.
     """
     base = 180.0 if degrees else np.pi
     return (angle + base) % (2.0 * base) - base  # type: ignore[no-any-return]
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index fd40486f..9d66476a 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -9,12 +9,12 @@
 
 from ._aiding import _aiding_update_gref, _aiding_update_head
 from ._common import (
-    _dhda_head,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
     _update_quaternion_with_gibbs2,
     _update_quaternion_with_rotvec,
+    _yaw_gradient,
 )
 
 _P0 = (
@@ -133,7 +133,7 @@ def _measurement_matrix_init(
     """
     vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
     H = np.zeros((4, 6))
-    H[0:1, 0:3] = _dhda_head(q_nb)  # heading
+    H[0:1, 0:3] = _yaw_gradient(q_nb)  # heading
     H[1:4, 0:3] = _skew_symmetric(vg_b)  # gravity reference vector
     return H
 
@@ -348,7 +348,7 @@ def update(
                 raise ValueError("'head_var' is required for heading aiding.")
 
             # Update measurement matrix (heading row)
-            self._H[0, 0:3] = _dhda_head(self._q_nb)
+            self._H[0, 0:3] = _yaw_gradient(self._q_nb)
 
             # Update (a posteriori) estimates with heading aiding
             _aiding_update_head(  # -> update dx and P (in place)
diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py
index 1a440209..77f7ee8e 100644
--- a/tests/test_ins/test_common.py
+++ b/tests/test_ins/test_common.py
@@ -33,7 +33,7 @@
     ],
 )
 def test__dhda(quaternion, dhda_expect):
-    dhda_out = _common._dhda_head(quaternion)
+    dhda_out = _common._yaw_gradient(quaternion)
     np.testing.assert_allclose(dhda_out, dhda_expect)
 
 
@@ -45,7 +45,7 @@ def test__dhda(quaternion, dhda_expect):
         np.radians([10.0, 95.0, 1.0]),
     ],
 )
-def test__h_head(angles):
+def test__yaw_from_quaternion(angles):
     alpha, beta, gamma = np.radians((0.0, 0.0, 15.0))
 
     quaternion = Rotation.from_euler(
@@ -53,7 +53,7 @@ def test__h_head(angles):
     ).as_quat()
     quaternion = np.r_[quaternion[3], quaternion[:3]]
 
-    gamma_expect = _common._h_head(quaternion)
+    gamma_expect = _common._yaw_from_quaternion(quaternion)
     assert gamma_expect == pytest.approx(gamma)
 
 
diff --git a/tests/test_ins_legacy.py b/tests/test_ins_legacy.py
index f0aa54d9..920faa19 100644
--- a/tests/test_ins_legacy.py
+++ b/tests/test_ins_legacy.py
@@ -27,7 +27,11 @@
     gravity,
 )
 from smsfusion._ins._ains_legacy import INSMixin, _roll_pitch_from_acc
-from smsfusion._ins._common import _dhda_head, _h_head, _signed_smallest_angle
+from smsfusion._ins._common import (
+    _signed_smallest_angle,
+    _yaw_from_quaternion,
+    _yaw_gradient,
+)
 from smsfusion._transforms import (
     _angular_matrix_from_quaternion,
     _rot_matrix_from_quaternion,
@@ -602,7 +606,7 @@ def test_update_twise(self, ins):
         np.radians([10.0, 95.0, 1.0]),
     ],
 )
-def test__h_head(angles):
+def test__yaw_from_quaternion(angles):
     alpha, beta, gamma = np.radians((0.0, 0.0, 15.0))
 
     quaternion = Rotation.from_euler(
@@ -610,7 +614,7 @@ def test__h_head(angles):
     ).as_quat()
     quaternion = np.r_[quaternion[3], quaternion[:3]]
 
-    gamma_expect = _h_head(quaternion)
+    gamma_expect = _yaw_from_quaternion(quaternion)
     assert gamma_expect == pytest.approx(gamma)
 
 
@@ -638,7 +642,7 @@ def test__h_head(angles):
     ],
 )
 def test__dhda(quaternion, dhda_expect):
-    dhda_out = _dhda_head(quaternion)
+    dhda_out = _yaw_gradient(quaternion)
     np.testing.assert_allclose(dhda_out, dhda_expect)
 
 
@@ -1242,11 +1246,11 @@ def test__update_H_g_ref(self, ains):
         H_expect[0:3, 6:9] = S(R_nm.T @ ains._vg_ref_n)
         np.testing.assert_allclose(H_out, H_expect)
 
-    def test__update_H_head(self, ains):
+    def test__update_yaw_from_quaternion(self, ains):
         q = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True)
-        H_out = ains._update_H_head(q)
+        H_out = ains._update_yaw_from_quaternion(q)
         H_expect = np.zeros((1, 15))
-        H_expect[0, 6:9] = _dhda_head(q)
+        H_expect[0, 6:9] = _yaw_gradient(q)
         np.testing.assert_allclose(H_out, H_expect)
 
     def test_update_return_self(self, ains):

From 1abdb2b245a22c3fda515ed6f8b5157e1e3237d9 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Wed, 26 Aug 2026 14:16:01 +0200
Subject: [PATCH 205/217] adjust angle thresholds bench tests

---
 tests/test_ins/test_ahrs.py | 6 +++---
 tests/test_ins/test_ains.py | 6 +++---
 tests/test_ins/test_vru.py  | 6 +++---
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 6d7fd309..9a6242de 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -317,9 +317,9 @@ def rmse(ref, est):
         assert vx_rmse <= 0.1
         assert vy_rmse <= 0.1
         assert vz_rmse <= 0.1
-        assert np.degrees(roll_rmse) <= 0.5
-        assert np.degrees(pitch_rmse) <= 0.5
-        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(yaw_rmse) <= 0.6
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index cf9f9d5f..ab56e566 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -402,9 +402,9 @@ def rmse(ref, est):
         assert vx_rmse <= 0.1
         assert vy_rmse <= 0.1
         assert vz_rmse <= 0.1
-        assert np.degrees(roll_rmse) <= 0.5
-        assert np.degrees(pitch_rmse) <= 0.5
-        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(yaw_rmse) <= 0.6
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 53d5fb9e..c8bf0e45 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -267,9 +267,9 @@ def rmse(ref, est):
             bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
         )
 
-        assert np.degrees(roll_rmse) <= 0.5
-        assert np.degrees(pitch_rmse) <= 0.5
-        assert np.degrees(yaw_rmse) <= 0.5
+        assert np.degrees(roll_rmse) <= 0.6
+        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(yaw_rmse) <= 0.6
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01

From c18a7fa352cdfb9e7286d1069cc7054df2932b1c Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 10:37:50 +0200
Subject: [PATCH 206/217] move gref aiding vru

---
 src/smsfusion/_ins/_vru.py | 44 +++++++++++++++++++-------------------
 tests/test_ins/test_vru.py |  4 ++--
 2 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 9d66476a..9e63bad1 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -133,8 +133,8 @@ def _measurement_matrix_init(
     """
     vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
     H = np.zeros((4, 6))
-    H[0:1, 0:3] = _yaw_gradient(q_nb)  # heading
-    H[1:4, 0:3] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[0:3, 0:3] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[3:4, 0:3] = _yaw_gradient(q_nb)  # heading
     return H
 
 
@@ -343,42 +343,42 @@ def update(
         # Project (a priori) error covariance matrix estimate ahead
         _project_covariance_ahead(self._P, self._phi, self._Q)  # -> update P (in place)
 
-        if head is not None:
-            if head_var is None:
-                raise ValueError("'head_var' is required for heading aiding.")
-
-            # Update measurement matrix (heading row)
-            self._H[0, 0:3] = _yaw_gradient(self._q_nb)
-
-            # Update (a posteriori) estimates with heading aiding
-            _aiding_update_head(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[0],
-                self._q_nb,
-                head,
-                head_var,
-                head_degrees,
-            )
-
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
             # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
-            self._H[1:4, 0:3] = _skew_symmetric(vg_b)
+            self._H[0:3, 0:3] = _skew_symmetric(vg_b)
 
             # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
-                self._H[1:4],
+                self._H[0:3],
                 vg_b,
                 dvel,
                 np.asarray(gref_var),
             )
 
+        if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            # Update measurement matrix (heading row)
+            self._H[3, 0:3] = _yaw_gradient(self._q_nb)
+
+            # Update (a posteriori) estimates with heading aiding
+            _aiding_update_head(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[3],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
+
         # Reset state -> update q_nb, bg_b and dx (in place)
         _reset(self._dx, self._q_nb, self._bg_b)
 
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index c8bf0e45..da1c75f0 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -68,8 +68,8 @@ def test_measurement_matrix_init():
     nz2vg = 1.0
 
     expect = np.zeros((4, 6))
-    expect[0:1, 0:3] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
-    expect[1:4, 0:3] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[0:3, 0:3] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[3:4, 0:3] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
 
     np.testing.assert_array_equal(_measurement_matrix_init(q_nb, nz2vg), expect)
 

From 6ab22cb6d7ebb1b51eaaef584b37e2650e960691 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 10:42:35 +0200
Subject: [PATCH 207/217] move gref aiding for ahrs and ains

---
 src/smsfusion/_ins/_ahrs.py | 44 +++++++++++++++++------------------
 src/smsfusion/_ins/_ains.py | 46 ++++++++++++++++++-------------------
 tests/test_ins/test_ahrs.py |  4 ++--
 tests/test_ins/test_ains.py |  4 ++--
 4 files changed, 49 insertions(+), 49 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index f0a879c3..b188696f 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -167,8 +167,8 @@ def _measurement_matrix_init(
     vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
     H = np.zeros((7, 9))
     H[0:3, 0:3] = np.eye(3)  # velocity
-    H[3:4, 3:6] = _yaw_gradient(q_nb)  # heading
-    H[4:7, 3:6] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[3:6, 3:6] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[6:7, 3:6] = _yaw_gradient(q_nb)  # heading
     return H
 
 
@@ -487,42 +487,42 @@ def update(
                 np.asarray(vel_var),
             )
 
-        if head is not None:
-            if head_var is None:
-                raise ValueError("'head_var' is required for heading aiding.")
-
-            # Update measurement matrix (heading row)
-            self._H[3, 6:9] = _yaw_gradient(self._q_nb)
-
-            # Update (a posteriori) estimates with heading aiding
-            _aiding_update_head(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[3],
-                self._q_nb,
-                head,
-                head_var,
-                head_degrees,
-            )
-
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
             # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
-            self._H[4:7, 3:6] = _skew_symmetric(vg_b)
+            self._H[3:6, 3:6] = _skew_symmetric(vg_b)
 
             # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
-                self._H[4:7],
+                self._H[3:6],
                 vg_b,
                 dvel,
                 np.asarray(gref_var),
             )
 
+        if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            # Update measurement matrix (heading row)
+            self._H[6, 6:9] = _yaw_gradient(self._q_nb)
+
+            # Update (a posteriori) estimates with heading aiding
+            _aiding_update_head(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[6],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
+
         # Reset state -> update v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._v_n, self._q_nb, self._bg_b)
 
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index bb8b4d71..32b0a243 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -174,7 +174,7 @@ def _measurement_matrix_init(
 
     Returns
     -------
-    ndarray, shape (7, 12)
+    ndarray, shape (10, 12)
         Linearized measurement matrix.
     """
     vg_b = _gref_b_from_quat(q_nb, nav_frame_factor)  # gravity reference vector
@@ -182,8 +182,8 @@ def _measurement_matrix_init(
     H[0:3, 0:3] = np.eye(3)  # position
     H[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     H[3:6, 3:6] = np.eye(3)  # velocity
-    H[6:7, 6:9] = _yaw_gradient(q_nb)  # heading
-    H[7:10, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[6:9, 6:9] = _skew_symmetric(vg_b)  # gravity reference vector
+    H[9:10, 6:9] = _yaw_gradient(q_nb)  # heading
     return H
 
 
@@ -552,42 +552,42 @@ def update(
                 np.asarray(vel_var),
             )
 
-        if head is not None:
-            if head_var is None:
-                raise ValueError("'head_var' is required for heading aiding.")
-
-            # Update measurement matrix (heading row)
-            self._H[6, 6:9] = _yaw_gradient(self._q_nb)
-
-            # Update (a posteriori) estimates with heading aiding
-            _aiding_update_head(  # -> update dx and P (in place)
-                self._dx,
-                self._P,
-                self._H[6],
-                self._q_nb,
-                head,
-                head_var,
-                head_degrees,
-            )
-
         if gref is True:
             if gref_var is None:
                 raise ValueError("'gref_var' is required for gravity reference aiding.")
 
             # Update measurement matrix (gravity reference vector rows)
             vg_b = _gref_b_from_quat(self._q_nb, self._nz2vg)
-            self._H[7:10, 6:9] = _skew_symmetric(vg_b)
+            self._H[6:9, 6:9] = _skew_symmetric(vg_b)
 
             # Update (a posteriori) estimates with gravity reference vector aiding
             _aiding_update_gref(  # -> update dx and P (in place)
                 self._dx,
                 self._P,
-                self._H[7:10],
+                self._H[6:9],
                 vg_b,
                 dvel,
                 np.asarray(gref_var),
             )
 
+        if head is not None:
+            if head_var is None:
+                raise ValueError("'head_var' is required for heading aiding.")
+
+            # Update measurement matrix (heading row)
+            self._H[9, 6:9] = _yaw_gradient(self._q_nb)
+
+            # Update (a posteriori) estimates with heading aiding
+            _aiding_update_head(  # -> update dx and P (in place)
+                self._dx,
+                self._P,
+                self._H[9],
+                self._q_nb,
+                head,
+                head_var,
+                head_degrees,
+            )
+
         # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place)
         _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b)
 
diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 9a6242de..be08f937 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -80,8 +80,8 @@ def test_measurement_matrix_init():
 
     expect = np.zeros((7, 9))
     expect[0:3, 0:3] = np.eye(3)
-    expect[3:4, 3:6] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
-    expect[4:7, 3:6] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[3:6, 3:6] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[6:7, 3:6] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
 
     np.testing.assert_array_equal(_measurement_matrix_init(q_nb, nz2vg), expect)
 
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index ab56e566..c2a5108d 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -90,8 +90,8 @@ def test_measurement_matrix_init():
     expect[0:3, 0:3] = np.eye(3)
     expect[0:3, 6:9] = -_rot_matrix_from_quaternion(q_nb) @ _skew_symmetric(lever_arm)
     expect[3:6, 3:6] = np.eye(3)
-    expect[6, 6:9] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
-    expect[7:10, 6:9] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[6:9, 6:9] = _skew_symmetric(_gref_b_from_quat(q_nb, nz2vg))
+    expect[9:10, 6:9] = np.array([0.0, 0.0, 1.0])  # kappa -> zero due to unit quat
 
     np.testing.assert_array_equal(
         _measurement_matrix_init(q_nb, lever_arm, nz2vg), expect

From fb438879526f4f96f7699a759128d074f3531d03 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 10:57:06 +0200
Subject: [PATCH 208/217] fix half-sample shift in VRU

---
 tests/test_ins/test_vru.py | 32 ++++++++++++++------------------
 1 file changed, 14 insertions(+), 18 deletions(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index da1c75f0..4cf4f925 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -252,27 +252,23 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        vel_ref = vel_ref[:-1, :]
-        euler_ref = euler_ref[:-1, :]
-        bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
+        euler_ref = euler_ref[1:, :]
+        bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
             return np.sqrt(np.mean((ref - est) ** 2, axis=0))
 
-        roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
-        bgx_rmse, bgy_rmse, bgz_rmse = rmse(
-            bias_gyro_ref[warmup:], bias_gyro_est[warmup:]
-        )
+        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
+
+        assert np.degrees(roll_rmse) <= 0.4
+        assert np.degrees(pitch_rmse) <= 0.4
+        assert np.degrees(bgx_rmse) <= 0.02
+        assert np.degrees(bgy_rmse) <= 0.02
 
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
-        assert np.degrees(yaw_rmse) <= 0.6
-        assert np.degrees(bgx_rmse) <= 0.01
-        assert np.degrees(bgy_rmse) <= 0.01
-        assert np.degrees(bgz_rmse) <= 0.01
 
     @pytest.mark.parametrize(
         "benchmark_gen",
@@ -314,10 +310,10 @@ def test_benchmark_default_aiding(self, benchmark_gen):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
+        euler_ref = euler_ref[1:, :]
         bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
@@ -326,7 +322,7 @@ def rmse(ref, est):
         roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
         bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
 
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(roll_rmse) <= 0.4
+        assert np.degrees(pitch_rmse) <= 0.4
         assert np.degrees(bgx_rmse) <= 0.02
         assert np.degrees(bgy_rmse) <= 0.02

From 123f348c93d87b6f2cdc6cda0be51373ada4131b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:02:07 +0200
Subject: [PATCH 209/217] test vru bench check yaw also

---
 tests/test_ins/test_vru.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 4cf4f925..76d2e606 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -261,13 +261,15 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         def rmse(ref, est):
             return np.sqrt(np.mean((ref - est) ** 2, axis=0))
 
-        roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
-        bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
+        roll_rmse, pitch_rmse, yaw_rmse = rmse(euler_ref[warmup:], euler_est[warmup:])
+        bgx_rmse, bgy_rmse, bgz_rmse = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
 
         assert np.degrees(roll_rmse) <= 0.4
         assert np.degrees(pitch_rmse) <= 0.4
+        assert np.degrees(yaw_rmse) <= 0.4
         assert np.degrees(bgx_rmse) <= 0.02
         assert np.degrees(bgy_rmse) <= 0.02
+        assert np.degrees(bgz_rmse) <= 0.02
 
 
     @pytest.mark.parametrize(

From 2ad40c0af6ae00c7c8b7206a26fc21b47c7b6799 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:07:50 +0200
Subject: [PATCH 210/217] fix half sample shift ahrs bench test

---
 tests/test_ins/test_ahrs.py | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index be08f937..6e9ab5ca 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -301,8 +301,8 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        vel_ref = vel_ref[:-1, :]
-        euler_ref = euler_ref[:-1, :]
+        vel_ref = vel_ref[1:, :]
+        euler_ref = euler_ref[1:, :]
         bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
@@ -317,9 +317,9 @@ def rmse(ref, est):
         assert vx_rmse <= 0.1
         assert vy_rmse <= 0.1
         assert vz_rmse <= 0.1
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
-        assert np.degrees(yaw_rmse) <= 0.6
+        assert np.degrees(roll_rmse) <= 0.1
+        assert np.degrees(pitch_rmse) <= 0.1
+        assert np.degrees(yaw_rmse) <= 0.1
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
@@ -367,7 +367,7 @@ def test_benchmark_default_aiding(self, benchmark_gen):
         # Half-sample shift (compensates for the delay introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
+        euler_ref = euler_ref[1:, :]
         bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
@@ -376,7 +376,7 @@ def rmse(ref, est):
         roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
         bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
 
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
-        assert np.degrees(bgx_rmse) <= 0.02
-        assert np.degrees(bgy_rmse) <= 0.02
+        assert np.degrees(roll_rmse) <= 0.2
+        assert np.degrees(pitch_rmse) <= 0.2
+        assert np.degrees(bgx_rmse) <= 0.01
+        assert np.degrees(bgy_rmse) <= 0.01

From c093ea959e0fad0bdb98507558a9bc974669047b Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:14:08 +0200
Subject: [PATCH 211/217] fix half sample shift ains bench tests

---
 tests/test_ins/test_ahrs.py |  4 ++--
 tests/test_ins/test_ains.py | 22 +++++++++++-----------
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index 6e9ab5ca..ca7f0c3a 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -297,7 +297,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
@@ -364,7 +364,7 @@ def test_benchmark_default_aiding(self, benchmark_gen):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
         euler_ref = euler_ref[1:, :]
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index c2a5108d..13658423 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -376,14 +376,14 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         pos_est = resample_poly(pos_est, 2, 1)[1:-1:2]
         vel_est = resample_poly(vel_est, 2, 1)[1:-1:2]
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        pos_ref = pos_ref[:-1, :]
-        vel_ref = vel_ref[:-1, :]
-        euler_ref = euler_ref[:-1, :]
+        pos_ref = pos_ref[1:, :]
+        vel_ref = vel_ref[1:, :]
+        euler_ref = euler_ref[1:, :]
         bias_gyro_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
@@ -402,9 +402,9 @@ def rmse(ref, est):
         assert vx_rmse <= 0.1
         assert vy_rmse <= 0.1
         assert vz_rmse <= 0.1
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
-        assert np.degrees(yaw_rmse) <= 0.6
+        assert np.degrees(roll_rmse) <= 0.1
+        assert np.degrees(pitch_rmse) <= 0.1
+        assert np.degrees(yaw_rmse) <= 0.1
         assert np.degrees(bgx_rmse) <= 0.01
         assert np.degrees(bgy_rmse) <= 0.01
         assert np.degrees(bgz_rmse) <= 0.01
@@ -449,10 +449,10 @@ def test_benchmark_default_aiding(self, benchmark_gen):
         euler_est = np.array(euler_est)
         bias_gyro_est = np.array(bias_gyro_est)
 
-        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        # Half-sample shift (compensates for the time shift introduced by Euler integration)
         euler_est = resample_poly(euler_est, 2, 1)[1:-1:2]
         bias_gyro_est = resample_poly(bias_gyro_est, 2, 1)[1:-1:2]
-        euler_ref = euler_ref[:-1, :]
+        euler_ref = euler_ref[1:, :]
         bg_ref = np.tile(bg, (len(bias_gyro_est), 1))
 
         def rmse(ref, est):
@@ -461,7 +461,7 @@ def rmse(ref, est):
         roll_rmse, pitch_rmse, _ = rmse(euler_ref[warmup:], euler_est[warmup:])
         bgx_rmse, bgy_rmse, _ = rmse(bg_ref[warmup:], bias_gyro_est[warmup:])
 
-        assert np.degrees(roll_rmse) <= 0.6
-        assert np.degrees(pitch_rmse) <= 0.6
+        assert np.degrees(roll_rmse) <= 0.2
+        assert np.degrees(pitch_rmse) <= 0.2
         assert np.degrees(bgx_rmse) <= 0.02
         assert np.degrees(bgy_rmse) <= 0.02

From a4ae09dd3cbeb059b593858070c73e4a1a5db42e Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:14:45 +0200
Subject: [PATCH 212/217] black

---
 tests/test_ins/test_vru.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 76d2e606..1f19d305 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -271,7 +271,6 @@ def rmse(ref, est):
         assert np.degrees(bgy_rmse) <= 0.02
         assert np.degrees(bgz_rmse) <= 0.02
 
-
     @pytest.mark.parametrize(
         "benchmark_gen",
         [

From 89c9a1e8ada38ed9a919b7f454bb0ff8b8decca2 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:19:52 +0200
Subject: [PATCH 213/217] small fix

---
 tests/test_ins/test_vru.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index 1f19d305..bc859f81 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -205,7 +205,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
         warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
         # Reference signals (without noise)
-        t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+        t, _, _, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
 
         # IMU and aiding measurements (with noise)
         head_std = np.radians(0.1)  # rad

From dde75decf7f7dad0ba728d52fb9a89143336456f Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:37:47 +0200
Subject: [PATCH 214/217] dvel dtheta simple approximation docstring

---
 src/smsfusion/_ins/_ahrs.py | 23 ++++++++++++++---------
 src/smsfusion/_ins/_ains.py | 25 +++++++++++++++----------
 src/smsfusion/_ins/_vru.py  | 17 +++++++++++------
 3 files changed, 40 insertions(+), 25 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index b188696f..77089380 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -398,12 +398,12 @@ def update(
         dtheta: ArrayLike,
         degrees: bool = False,
         vel: ArrayLike | None = (0.0, 0.0, 0.0),
-        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
+        vel_var: ArrayLike = (100.0, 100.0, 100.0),
         head: float | None = None,
-        head_var: float | None = None,
+        head_var: float = 0.001,
         head_degrees: bool = False,
         gref: bool = False,
-        gref_var: ArrayLike | None = None,
+        gref_var: ArrayLike = (0.0001, 0.0001, 0.0001),
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -411,10 +411,14 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,)
-            Velocity increment (sculling integral) in m/s.
+            Velocity increment (sculling integral) in m/s. The simple approximation,
+            ``dvel = f * dt``, where ``f`` is the specific force measurement in m/s^2
+            and ``dt`` is the time step in seconds, is sufficient for most applications.
         dtheta : array_like, shape (3,)
-            Attitude increment (coning integral) in radians or degrees depending
-            on the ``degrees`` flag.
+            Attitude increment (coning integral) in radians or degrees, depending
+            on the ``degrees`` flag. The simple approximation, ``dtheta = w * dt``,
+            where ``w`` is the angular rate in rad/s (or deg/s) and ``dt`` is the
+            time step in seconds is sufficient for most applications.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
@@ -431,8 +435,9 @@ def update(
             If ``None``, compass aiding is not used.
         head_var : float, optional
             Variance of heading measurement noise in radians^2 or degrees^2 depending
-            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
-        head_degrees : bool, default False
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``. Defaults
+            to 0.001 radians^2.
+        head_degrees : bool, optional
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees
             and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
         gref : bool, optional
@@ -440,7 +445,7 @@ def update(
             direction of gravity as aiding. Defaults to ``False``.
         gref_var : array_like, shape (3,), optional
             Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding.
+            Required for gravity reference vector aiding. Defaults to (0.0001, 0.0001, 0.0001).
 
         Returns
         -------
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index 32b0a243..50820a7d 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -437,14 +437,14 @@ def update(
         dtheta: ArrayLike,
         degrees: bool = False,
         pos: ArrayLike | None = (0.0, 0.0, 0.0),
-        pos_var: ArrayLike | None = (1_000_000.0, 1_000_000.0, 1_000_000.0),
+        pos_var: ArrayLike = (1_000_000.0, 1_000_000.0, 1_000_000.0),
         vel: ArrayLike | None = (0.0, 0.0, 0.0),
-        vel_var: ArrayLike | None = (100.0, 100.0, 100.0),
+        vel_var: ArrayLike = (100.0, 100.0, 100.0),
         head: float | None = None,
-        head_var: float | None = None,
+        head_var: float = 0.001,
         head_degrees: bool = False,
         gref: bool = False,
-        gref_var: ArrayLike | None = None,
+        gref_var: ArrayLike = (0.0001, 0.0001, 0.0001),
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -452,10 +452,14 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,)
-            Velocity increment (sculling integral) in m/s.
+            Velocity increment (sculling integral) in m/s. The simple approximation,
+            ``dvel = f * dt``, where ``f`` is the specific force measurement in m/s^2
+            and ``dt`` is the time step in seconds, is sufficient for most applications.
         dtheta : array_like, shape (3,)
-            Attitude increment (coning integral) in radians or degrees depending
-            on the ``degrees`` flag.
+            Attitude increment (coning integral) in radians or degrees, depending
+            on the ``degrees`` flag. The simple approximation, ``dtheta = w * dt``,
+            where ``w`` is the angular rate in rad/s (or deg/s) and ``dt`` is the
+            time step in seconds is sufficient for most applications.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
@@ -478,8 +482,9 @@ def update(
             If ``None``, compass aiding is not used.
         head_var : float, optional
             Variance of heading measurement noise in radians^2 or degrees^2 depending
-            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
-        head_degrees : bool, default False
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``. Defaults
+            to 0.001 radians^2.
+        head_degrees : bool, optional
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees
             and degrees^2, or radians and radians^2. Defaults to radians and radians^2.
         gref : bool, optional
@@ -487,7 +492,7 @@ def update(
             direction of gravity as aiding. Defaults to ``False``.
         gref_var : array_like, shape (3,), optional
             Variance of gravity reference vector measurement noise (dimensionless).
-            Required for gravity reference vector aiding.
+            Required for gravity reference vector aiding. Defaults to (0.0001, 0.0001, 0.0001).
 
         Returns
         -------
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 9e63bad1..40214468 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -284,10 +284,10 @@ def update(
         dtheta: ArrayLike,
         degrees: bool = False,
         head: float | None = None,
-        head_var: float | None = None,
+        head_var: float = 0.001,
         head_degrees: bool = False,
         gref: bool = True,
-        gref_var: ArrayLike | None = (0.0001, 0.0001, 0.0001),
+        gref_var: ArrayLike = (0.0001, 0.0001, 0.0001),
     ) -> Self:
         """
         Update state estimates with IMU and aiding measurements.
@@ -295,10 +295,14 @@ def update(
         Parameters
         ----------
         dvel : array_like, shape (3,)
-            Velocity increment (sculling integral) in m/s.
+            Velocity increment (sculling integral) in m/s. The simple approximation,
+            ``dvel = f * dt``, where ``f`` is the specific force measurement in m/s^2
+            and ``dt`` is the time step in seconds, is sufficient for most applications.
         dtheta : array_like, shape (3,)
-            Attitude increment (coning integral) in radians or degrees depending
-            on the ``degrees`` flag.
+            Attitude increment (coning integral) in radians or degrees, depending
+            on the ``degrees`` flag. The simple approximation, ``dtheta = w * dt``,
+            where ``w`` is the angular rate in rad/s (or deg/s) and ``dt`` is the
+            time step in seconds is sufficient for most applications.
         degrees : bool, optional
             Specifies whether the unit of the attitude increment, ``dtheta``, is
             degrees or radians. Defaults to radians.
@@ -309,7 +313,8 @@ def update(
             If ``None``, compass aiding is not used.
         head_var : float, optional
             Variance of heading measurement noise in radians^2 or degrees^2 depending
-            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``.
+            on the ``head_degrees`` flag. Ignored if ``head`` is ``None``. Defaults
+            to 0.001 radians^2.
         head_degrees : bool, default False
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees
             and degrees^2, or radians and radians^2. Defaults to radians and radians^2.

From 216707db7a7a2dde94402d806ee4ff7b9a7f1cb7 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 11:52:24 +0200
Subject: [PATCH 215/217] move gravity nav to common module

---
 src/smsfusion/_ins/_ahrs.py   | 26 +-------------------------
 src/smsfusion/_ins/_ains.py   | 26 +-------------------------
 src/smsfusion/_ins/_common.py | 25 +++++++++++++++++++++++++
 tests/test_ins/test_common.py |  6 ++++++
 4 files changed, 33 insertions(+), 50 deletions(-)

diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 77089380..5742ca07 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -9,6 +9,7 @@
 
 from ._aiding import _aiding_update_gref, _aiding_update_head, _aiding_update_vel
 from ._common import (
+    _gravity_nav,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
@@ -172,31 +173,6 @@ def _measurement_matrix_init(
     return H
 
 
-def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
-    """
-    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
-
-    Parameters
-    ----------
-    g : float
-        Gravitational acceleration in m/s^2.
-    nav_frame : {'NED', 'ENU'}
-        Navigation frame in which the gravity vector is expressed.
-
-    Returns
-    -------
-    ndarray, shape (3,)
-        Gravity vector expressed in the navigation frame.
-    """
-    if nav_frame.lower() == "ned":
-        g_n = np.array([0.0, 0.0, g])
-    elif nav_frame.lower() == "enu":
-        g_n = np.array([0.0, 0.0, -g])
-    else:
-        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
-    return g_n
-
-
 @njit  # type: ignore[misc]
 def _reset(
     dx: NDArray[np.float64],
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index 50820a7d..eec7bd78 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -14,6 +14,7 @@
     _aiding_update_vel,
 )
 from ._common import (
+    _gravity_nav,
     _gref_b_from_quat,
     _nz2vg,
     _project_covariance_ahead,
@@ -187,31 +188,6 @@ def _measurement_matrix_init(
     return H
 
 
-def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
-    """
-    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
-
-    Parameters
-    ----------
-    g : float
-        Gravitational acceleration in m/s^2.
-    nav_frame : {'NED', 'ENU'}
-        Navigation frame in which the gravity vector is expressed.
-
-    Returns
-    -------
-    ndarray, shape (3,)
-        Gravity vector expressed in the navigation frame.
-    """
-    if nav_frame.lower() == "ned":
-        g_n = np.array([0.0, 0.0, g])
-    elif nav_frame.lower() == "enu":
-        g_n = np.array([0.0, 0.0, -g])
-    else:
-        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
-    return g_n
-
-
 @njit  # type: ignore[misc]
 def _reset(
     dx: NDArray[np.float64],
diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py
index 9532282c..03710818 100644
--- a/src/smsfusion/_ins/_common.py
+++ b/src/smsfusion/_ins/_common.py
@@ -372,3 +372,28 @@ def _gref_b_from_quat(
     z = 1.0 - 2.0 * (q_nb[1] ** 2 + q_nb[2] ** 2)
 
     return nav_frame_factor * np.array([x, y, z])
+
+
+def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]:
+    """
+    Gravity vector expressed in the navigation frame ('NED' or 'ENU').
+
+    Parameters
+    ----------
+    g : float
+        Gravitational acceleration in m/s^2.
+    nav_frame : {'NED', 'ENU'}
+        Navigation frame in which the gravity vector is expressed.
+
+    Returns
+    -------
+    ndarray, shape (3,)
+        Gravity vector expressed in the navigation frame.
+    """
+    if nav_frame.lower() == "ned":
+        g_n = np.array([0.0, 0.0, g])
+    elif nav_frame.lower() == "enu":
+        g_n = np.array([0.0, 0.0, -g])
+    else:
+        raise ValueError(f"Unknown navigation frame: {nav_frame}.")
+    return g_n
diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py
index 77f7ee8e..236c7fbb 100644
--- a/tests/test_ins/test_common.py
+++ b/tests/test_ins/test_common.py
@@ -219,6 +219,12 @@ def test__nz2vg():
     assert _common._nz2vg("ENU") == -1.0
 
 
+def test__gravity_nav():
+    g = 9.81
+    assert np.allclose(_common._gravity_nav(g, "NED"), np.array([0.0, 0.0, g]))
+    assert np.allclose(_common._gravity_nav(g, "ENU"), np.array([0.0, 0.0, -g]))
+
+
 def test_kalman_update_sequential():
 
     rng = np.random.default_rng(42)

From 9e6bc86a1c59c4591b68a0519a2a7bfbb278ee54 Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 13:17:30 +0200
Subject: [PATCH 216/217] rename version 2 filters to PVAMEKF, VAMEKF and AMEKF

---
 src/smsfusion/__init__.py      | 26 +++++++++++++-------------
 src/smsfusion/_ins/__init__.py | 19 ++++++++++++++++---
 src/smsfusion/_ins/_ahrs.py    |  9 +++------
 src/smsfusion/_ins/_ains.py    | 10 ++++------
 src/smsfusion/_ins/_vru.py     |  9 +++------
 tests/test_ins/test_ahrs.py    | 24 ++++++++++++------------
 tests/test_ins/test_ains.py    | 26 +++++++++++++-------------
 tests/test_ins/test_vru.py     | 27 +++++++++++++--------------
 8 files changed, 77 insertions(+), 73 deletions(-)

diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index f0afe5f0..9f22b33c 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -2,13 +2,13 @@
 from ._coning_sculling import ConingScullingAlg, ConingScullingAlgCalibrated
 from ._ins import (
     AHRS,
+    AMEKF,
+    PVAMEKF,
+    VAMEKF,
     VRU,
-    AHRSv2,
     AidedINS,
-    AINSv2,
     FixedNED,
     StrapdownINS,
-    VRUv2,
     gravity,
 )
 from ._smoothing import FixedIntervalSmoother
@@ -16,20 +16,20 @@
 
 __all__ = [
     "AHRS",
-    "AHRSv2",
+    "AMEKF",
+    "PVAMEKF",
+    "VAMEKF",
+    "VRU",
     "AidedINS",
-    "AINSv2",
-    "benchmark",
-    "constants",
-    "calibrate",
+    "ConingScullingAlg",
+    "ConingScullingAlgCalibrated",
     "FixedIntervalSmoother",
     "FixedNED",
+    "StrapdownINS",
+    "benchmark",
+    "calibrate",
+    "constants",
     "gravity",
     "noise",
-    "StrapdownINS",
-    "VRU",
-    "VRUv2",
     "quaternion_from_euler",
-    "ConingScullingAlg",
-    "ConingScullingAlgCalibrated",
 ]
diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index ce58166e..a7bddf12 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,5 +1,18 @@
-from ._ahrs import AHRS as AHRSv2
-from ._ains import AINS as AINSv2
+from ._ahrs import VAMEKF
+from ._ains import PVAMEKF
 from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS
 from ._utils import FixedNED, euler_from_acc, gravity
-from ._vru import VRU as VRUv2
+from ._vru import AMEKF
+
+__all__ = [
+    "AHRS",
+    "AMEKF",
+    "PVAMEKF",
+    "VAMEKF",
+    "VRU",
+    "AidedINS",
+    "FixedNED",
+    "StrapdownINS",
+    "euler_from_acc",
+    "gravity",
+]
diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py
index 5742ca07..4d7022c1 100644
--- a/src/smsfusion/_ins/_ahrs.py
+++ b/src/smsfusion/_ins/_ahrs.py
@@ -222,12 +222,9 @@ def _project_state_ahead(
     _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
 
 
-class AHRS:
+class VAMEKF:
     """
-    Attitude and Heading Reference System (AHRS).
-
-    This class provides velocity, attitude and gyro bias estimation using a multiplicative
-    extended Kalman filter (MEKF).
+    Multiplicative extended Kalman filter (MEKF) for velocity and attitude estimation.
 
     Parameters
     ----------
@@ -425,7 +422,7 @@ def update(
 
         Returns
         -------
-        AHRS
+        VAMEKF
             A reference to the instance itself after the update.
         """
 
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_ains.py
index eec7bd78..b9c579fb 100644
--- a/src/smsfusion/_ins/_ains.py
+++ b/src/smsfusion/_ins/_ains.py
@@ -244,12 +244,10 @@ def _project_state_ahead(
     _update_quaternion_with_rotvec(q_nb, dtheta)  # -> update q_nb (in place)
 
 
-class AINS:
+class PVAMEKF:
     """
-    Aided inertial navigation system (AINS).
-
-    This class provides position, velocity, attitude and gyro bias estimation using
-    a multiplicative extended Kalman filter (MEKF).
+    Multiplicative extended Kalman filter (MEKF) for position, velocity and attitude
+    estimation.
 
     Parameters
     ----------
@@ -472,7 +470,7 @@ def update(
 
         Returns
         -------
-        AINS
+        PVAMEKF
             A reference to the instance itself after the update.
         """
 
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_vru.py
index 40214468..f86e83d6 100644
--- a/src/smsfusion/_ins/_vru.py
+++ b/src/smsfusion/_ins/_vru.py
@@ -160,12 +160,9 @@ def _reset(
     dx[:] = 0.0
 
 
-class VRU:
+class AMEKF:
     """
-    Vertical Reference Unit (VRU).
-
-    This class provides attitude and gyro bias estimation using a multiplicative
-    extended Kalman filter (MEKF).
+    Multiplicative extended Kalman filter (MEKF) for attitude estimation.
 
     Parameters
     ----------
@@ -327,7 +324,7 @@ def update(
 
         Returns
         -------
-        VRU
+        AMEKF
             A reference to the instance itself after the update.
         """
 
diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_ahrs.py
index ca7f0c3a..1e489d7e 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_ahrs.py
@@ -4,7 +4,7 @@
 
 import smsfusion as sf
 from smsfusion._ins._ahrs import (
-    AHRS,
+    VAMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
     _reset,
@@ -126,7 +126,7 @@ def test_reset():
     )
 
 
-class Test_AHRS:
+class Test_VAMEKF:
 
     def test_init(self):
 
@@ -139,7 +139,7 @@ def test_init(self):
         gbs = 0.0003
         gbc = 123.0
 
-        mekf = AHRS(
+        mekf = VAMEKF(
             51.2,
             v0=v0,
             q0=q0,
@@ -175,7 +175,7 @@ def test_init(self):
         assert mekf._P is not P0  # copy
 
     def test_init_default(self):
-        mekf = AHRS(10.0)
+        mekf = VAMEKF(10.0)
         assert mekf._fs == pytest.approx(10.0)
         assert mekf._dt == pytest.approx(0.1)
         assert mekf._g == 9.80665
@@ -193,33 +193,33 @@ def test_init_default(self):
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
     def test_nav_frame(self, nav_frame, scale):
-        mekf = AHRS(10.0, nav_frame=nav_frame)
+        mekf = VAMEKF(10.0, nav_frame=nav_frame)
         assert mekf._nav_frame == nav_frame.lower()
         np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
 
     def test_velocity(self):
         v0 = np.array([1.0, 2.0, 3.0])
-        mekf = AHRS(10.0, v0=v0)
+        mekf = VAMEKF(10.0, v0=v0)
         np.testing.assert_allclose(mekf.velocity(), v0)
         assert mekf.velocity() is not mekf._v_n  # copy
 
     def test_quaternion(self):
         euler = np.array([10.0, 20.0, 30.0])
         q0 = sf.quaternion_from_euler(euler, degrees=True)
-        mekf = AHRS(10.0, q0=q0)
+        mekf = VAMEKF(10.0, q0=q0)
         np.testing.assert_allclose(mekf.quaternion(), q0)
         assert mekf.quaternion() is not mekf._q_nb  # copy
 
     def test_euler(self):
         euler = np.array([10.0, 20.0, 30.0])
-        mekf = AHRS(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        mekf = VAMEKF(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
         np.testing.assert_allclose(mekf.euler(), np.radians(euler))
         np.testing.assert_allclose(mekf.euler(degrees=True), euler)
         np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
 
     def test_bias_gyro(self):
         bg0 = np.array([0.01, -0.02, 0.03])
-        mekf = AHRS(10.0, bg0=bg0)
+        mekf = VAMEKF(10.0, bg0=bg0)
         np.testing.assert_allclose(mekf.bias_gyro(), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
@@ -227,7 +227,7 @@ def test_bias_gyro(self):
 
     def test_P(self):
         P0 = 0.1 * np.eye(9)
-        mekf = AHRS(10.0, P0=P0)
+        mekf = VAMEKF(10.0, P0=P0)
         np.testing.assert_allclose(mekf.P, P0)
         assert mekf.P is not mekf._P  # copy
 
@@ -262,7 +262,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
             gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
-        mekf = AHRS(
+        mekf = VAMEKF(
             fs_imu,
             v0=vel_ref[0],
             q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
@@ -349,7 +349,7 @@ def test_benchmark_default_aiding(self, benchmark_gen):
 
         # MEKF
         q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-        mekf = AHRS(fs_imu, q0=q0)
+        mekf = VAMEKF(fs_imu, q0=q0)
 
         euler_est, bias_gyro_est = [], []
         for f_i, w_i in zip(acc_meas, gyro_meas):
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_ains.py
index 13658423..f50fdecd 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_ains.py
@@ -4,7 +4,7 @@
 
 import smsfusion as sf
 from smsfusion._ins._ains import (
-    AINS,
+    PVAMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
     _reset,
@@ -182,7 +182,7 @@ def test_reset():
     )
 
 
-class Test_AINS:
+class Test_PVAMEKF:
 
     def test_init(self):
 
@@ -196,7 +196,7 @@ def test_init(self):
         gbs = 0.0003
         gbc = 123.0
 
-        mekf = AINS(
+        mekf = PVAMEKF(
             51.2,
             p0=p0,
             v0=v0,
@@ -237,7 +237,7 @@ def test_init(self):
         assert mekf._P is not P0  # copy
 
     def test_init_default(self):
-        mekf = AINS(10.0)
+        mekf = PVAMEKF(10.0)
         assert mekf._fs == pytest.approx(10.0)
         assert mekf._dt == pytest.approx(0.1)
         assert mekf._g == 9.80665
@@ -257,39 +257,39 @@ def test_init_default(self):
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
     def test_nav_frame(self, nav_frame, scale):
-        mekf = AINS(10.0, nav_frame=nav_frame)
+        mekf = PVAMEKF(10.0, nav_frame=nav_frame)
         assert mekf._nav_frame == nav_frame.lower()
         np.testing.assert_allclose(mekf._g_n, np.array([0.0, 0.0, mekf._g * scale]))
 
     def test_position(self):
         p0 = np.array([1.0, 2.0, 3.0])
-        mekf = AINS(10.0, p0=p0)
+        mekf = PVAMEKF(10.0, p0=p0)
         np.testing.assert_allclose(mekf.position(), p0)
         assert mekf.position() is not mekf._p_n  # copy
 
     def test_velocity(self):
         v0 = np.array([1.0, 2.0, 3.0])
-        mekf = AINS(10.0, v0=v0)
+        mekf = PVAMEKF(10.0, v0=v0)
         np.testing.assert_allclose(mekf.velocity(), v0)
         assert mekf.velocity() is not mekf._v_n  # copy
 
     def test_quaternion(self):
         euler = np.array([10.0, 20.0, 30.0])
         q0 = sf.quaternion_from_euler(euler, degrees=True)
-        mekf = AINS(10.0, q0=q0)
+        mekf = PVAMEKF(10.0, q0=q0)
         np.testing.assert_allclose(mekf.quaternion(), q0)
         assert mekf.quaternion() is not mekf._q_nb  # copy
 
     def test_euler(self):
         euler = np.array([10.0, 20.0, 30.0])
-        mekf = AINS(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        mekf = PVAMEKF(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
         np.testing.assert_allclose(mekf.euler(), np.radians(euler))
         np.testing.assert_allclose(mekf.euler(degrees=True), euler)
         np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
 
     def test_bias_gyro(self):
         bg0 = np.array([0.01, -0.02, 0.03])
-        mekf = AINS(10.0, bg0=bg0)
+        mekf = PVAMEKF(10.0, bg0=bg0)
         np.testing.assert_allclose(mekf.bias_gyro(), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
@@ -297,7 +297,7 @@ def test_bias_gyro(self):
 
     def test_P(self):
         P0 = 0.1 * np.eye(12)
-        mekf = AINS(10.0, P0=P0)
+        mekf = PVAMEKF(10.0, P0=P0)
         np.testing.assert_allclose(mekf.P, P0)
         assert mekf.P is not mekf._P  # copy
 
@@ -334,7 +334,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
             gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
-        mekf = AINS(
+        mekf = PVAMEKF(
             fs_imu,
             p0=pos_ref[0],
             v0=vel_ref[0],
@@ -434,7 +434,7 @@ def test_benchmark_default_aiding(self, benchmark_gen):
 
         # MEKF
         q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-        mekf = AINS(fs_imu, q0=q0)
+        mekf = PVAMEKF(fs_imu, q0=q0)
 
         euler_est, bias_gyro_est = [], []
         for f_i, w_i in zip(acc_meas, gyro_meas):
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_vru.py
index bc859f81..8c4dcb16 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_vru.py
@@ -5,7 +5,7 @@
 import smsfusion as sf
 from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._ins._vru import (
-    VRU,
+    AMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
     _reset,
@@ -108,19 +108,18 @@ def test_reset():
     )
 
 
-class Test_VRU:
+class Test_AMEKF:
 
     def test_init(self):
 
         q0 = sf.quaternion_from_euler(np.random.random(3), degrees=False)
         bg0 = np.random.random(3)
         P0 = 0.1 * np.eye(6)
-        vrw = 0.0001
         arw = 0.0002
         gbs = 0.0003
         gbc = 123.0
 
-        mekf = VRU(
+        mekf = AMEKF(
             51.2,
             q0=q0,
             bg0=bg0,
@@ -148,7 +147,7 @@ def test_init(self):
         assert mekf._P is not P0  # copy
 
     def test_init_default(self):
-        mekf = VRU(10.0)
+        mekf = AMEKF(10.0)
         assert mekf._fs == pytest.approx(10.0)
         assert mekf._dt == pytest.approx(0.1)
         assert mekf._nav_frame == "ned"
@@ -160,28 +159,28 @@ def test_init_default(self):
         np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(6))
 
-    @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
-    def test_nav_frame(self, nav_frame, scale):
-        mekf = VRU(10.0, nav_frame=nav_frame)
+    @pytest.mark.parametrize("nav_frame", ["NED", "ENU"])
+    def test_nav_frame(self, nav_frame):
+        mekf = AMEKF(10.0, nav_frame=nav_frame)
         assert mekf._nav_frame == nav_frame.lower()
 
     def test_quaternion(self):
         euler = np.array([10.0, 20.0, 30.0])
         q0 = sf.quaternion_from_euler(euler, degrees=True)
-        mekf = VRU(10.0, q0=q0)
+        mekf = AMEKF(10.0, q0=q0)
         np.testing.assert_allclose(mekf.quaternion(), q0)
         assert mekf.quaternion() is not mekf._q_nb  # copy
 
     def test_euler(self):
         euler = np.array([10.0, 20.0, 30.0])
-        mekf = VRU(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
+        mekf = AMEKF(10.0, q0=sf.quaternion_from_euler(euler, degrees=True))
         np.testing.assert_allclose(mekf.euler(), np.radians(euler))
         np.testing.assert_allclose(mekf.euler(degrees=True), euler)
         np.testing.assert_allclose(mekf.euler(degrees=False), np.radians(euler))
 
     def test_bias_gyro(self):
         bg0 = np.array([0.01, -0.02, 0.03])
-        mekf = VRU(10.0, bg0=bg0)
+        mekf = AMEKF(10.0, bg0=bg0)
         np.testing.assert_allclose(mekf.bias_gyro(), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=False), bg0)
         np.testing.assert_allclose(mekf.bias_gyro(degrees=True), np.degrees(bg0))
@@ -189,7 +188,7 @@ def test_bias_gyro(self):
 
     def test_P(self):
         P0 = 0.1 * np.eye(6)
-        mekf = VRU(10.0, P0=P0)
+        mekf = AMEKF(10.0, P0=P0)
         np.testing.assert_allclose(mekf.P, P0)
         assert mekf.P is not mekf._P  # copy
 
@@ -222,7 +221,7 @@ def test_benchmark(self, benchmark_gen, gyro_degrees):
             gyro_meas = np.degrees(gyro_meas)
 
         # MEKF
-        mekf = VRU(
+        mekf = AMEKF(
             fs_imu,
             q0=sf.quaternion_from_euler(euler_ref[0], degrees=False),
             gyro_noise_density=err_gyro["N"],
@@ -296,7 +295,7 @@ def test_benchmark_default_aiding(self, benchmark_gen):
 
         # MEKF
         q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
-        mekf = VRU(fs_imu, q0=q0)
+        mekf = AMEKF(fs_imu, q0=q0)
 
         euler_est, bias_gyro_est = [], []
         for f_i, w_i in zip(acc_meas, gyro_meas):

From 132dea78d9820cfa62ab1fe986eec897c6eaa79d Mon Sep 17 00:00:00 2001
From: "Vegard R. Solum" <vegard.solum@4subsea.com>
Date: Thu, 27 Aug 2026 13:27:29 +0200
Subject: [PATCH 217/217] rename mekf modules based on the new names

---
 src/smsfusion/_ins/__init__.py                   | 6 +++---
 src/smsfusion/_ins/{_vru.py => _amekf.py}        | 0
 src/smsfusion/_ins/{_ains.py => _pvamekf.py}     | 0
 src/smsfusion/_ins/{_ahrs.py => _vamekf.py}      | 0
 tests/test_ins/{test_vru.py => test_amekf.py}    | 6 +++---
 tests/test_ins/{test_ains.py => test_pvamekf.py} | 6 +++---
 tests/test_ins/{test_ahrs.py => test_vamekf.py}  | 6 +++---
 7 files changed, 12 insertions(+), 12 deletions(-)
 rename src/smsfusion/_ins/{_vru.py => _amekf.py} (100%)
 rename src/smsfusion/_ins/{_ains.py => _pvamekf.py} (100%)
 rename src/smsfusion/_ins/{_ahrs.py => _vamekf.py} (100%)
 rename tests/test_ins/{test_vru.py => test_amekf.py} (99%)
 rename tests/test_ins/{test_ains.py => test_pvamekf.py} (99%)
 rename tests/test_ins/{test_ahrs.py => test_vamekf.py} (99%)

diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py
index a7bddf12..e8c09d95 100644
--- a/src/smsfusion/_ins/__init__.py
+++ b/src/smsfusion/_ins/__init__.py
@@ -1,8 +1,8 @@
-from ._ahrs import VAMEKF
-from ._ains import PVAMEKF
 from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS
+from ._amekf import AMEKF
+from ._pvamekf import PVAMEKF
 from ._utils import FixedNED, euler_from_acc, gravity
-from ._vru import AMEKF
+from ._vamekf import VAMEKF
 
 __all__ = [
     "AHRS",
diff --git a/src/smsfusion/_ins/_vru.py b/src/smsfusion/_ins/_amekf.py
similarity index 100%
rename from src/smsfusion/_ins/_vru.py
rename to src/smsfusion/_ins/_amekf.py
diff --git a/src/smsfusion/_ins/_ains.py b/src/smsfusion/_ins/_pvamekf.py
similarity index 100%
rename from src/smsfusion/_ins/_ains.py
rename to src/smsfusion/_ins/_pvamekf.py
diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_vamekf.py
similarity index 100%
rename from src/smsfusion/_ins/_ahrs.py
rename to src/smsfusion/_ins/_vamekf.py
diff --git a/tests/test_ins/test_vru.py b/tests/test_ins/test_amekf.py
similarity index 99%
rename from tests/test_ins/test_vru.py
rename to tests/test_ins/test_amekf.py
index 8c4dcb16..11e67313 100644
--- a/tests/test_ins/test_vru.py
+++ b/tests/test_ins/test_amekf.py
@@ -3,8 +3,7 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._common import _gref_b_from_quat
-from smsfusion._ins._vru import (
+from smsfusion._ins._amekf import (
     AMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
@@ -12,6 +11,7 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
+from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
@@ -156,7 +156,7 @@ def test_init_default(self):
         assert mekf._gbc == pytest.approx(50.0)
         np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
         np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-        np.testing.assert_allclose(mekf.P, np.array(sf._ins._vru._P0))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._amekf._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(6))
 
     @pytest.mark.parametrize("nav_frame", ["NED", "ENU"])
diff --git a/tests/test_ins/test_ains.py b/tests/test_ins/test_pvamekf.py
similarity index 99%
rename from tests/test_ins/test_ains.py
rename to tests/test_ins/test_pvamekf.py
index f50fdecd..611d590d 100644
--- a/tests/test_ins/test_ains.py
+++ b/tests/test_ins/test_pvamekf.py
@@ -3,7 +3,8 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._ains import (
+from smsfusion._ins._common import _gref_b_from_quat
+from smsfusion._ins._pvamekf import (
     PVAMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
@@ -11,7 +12,6 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
-from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._transforms import _rot_matrix_from_quaternion
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
@@ -252,7 +252,7 @@ def test_init_default(self):
         np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
         np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
         np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ains._P0))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._pvamekf._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(12))
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))
diff --git a/tests/test_ins/test_ahrs.py b/tests/test_ins/test_vamekf.py
similarity index 99%
rename from tests/test_ins/test_ahrs.py
rename to tests/test_ins/test_vamekf.py
index 1e489d7e..ba28a978 100644
--- a/tests/test_ins/test_ahrs.py
+++ b/tests/test_ins/test_vamekf.py
@@ -3,7 +3,8 @@
 from scipy.signal import resample_poly
 
 import smsfusion as sf
-from smsfusion._ins._ahrs import (
+from smsfusion._ins._common import _gref_b_from_quat
+from smsfusion._ins._vamekf import (
     VAMEKF,
     _measurement_matrix_init,
     _process_noise_covariance_matrix,
@@ -11,7 +12,6 @@
     _state_transition_matrix_init,
     _state_transition_matrix_update,
 )
-from smsfusion._ins._common import _gref_b_from_quat
 from smsfusion._vectorops import _skew_symmetric
 from smsfusion.benchmark import (
     benchmark_full_pva_beat_202311A,
@@ -188,7 +188,7 @@ def test_init_default(self):
         np.testing.assert_allclose(mekf.velocity(), np.zeros(3))
         np.testing.assert_allclose(mekf.quaternion(), np.array([1.0, 0.0, 0.0, 0.0]))
         np.testing.assert_allclose(mekf.bias_gyro(), np.zeros(3))
-        np.testing.assert_allclose(mekf.P, np.array(sf._ins._ahrs._P0))
+        np.testing.assert_allclose(mekf.P, np.array(sf._ins._vamekf._P0))
         np.testing.assert_allclose(mekf._dx, np.zeros(9))
 
     @pytest.mark.parametrize("nav_frame, scale", (["NED", 1.0], ["ENU", -1.0]))