diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py
index 48ec7ea4..1da80bfc 100644
--- a/src/smsfusion/__init__.py
+++ b/src/smsfusion/__init__.py
@@ -1,3 +1,4 @@
+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
diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py
new file mode 100644
index 00000000..f56ac3b6
--- /dev/null
+++ b/src/smsfusion/_ins_v2.py
@@ -0,0 +1,679 @@
+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 _normalize, _skew_symmetric
+
+
+@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.
+    """
+
+    # 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,
+) -> 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
+
+
+@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],
+    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 (9, 9)
+        State transition matrix.
+    """
+    phi = np.eye(9)
+    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
+
+
+@njit  # type: ignore[misc]
+def _update_state_transition_matrix(
+    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 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[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_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 (9, 9)
+        Process noise covariance matrix.
+    """
+    Q = np.zeros((9, 9))
+    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
+
+
+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, 0:3] = np.eye(3)  # velocity
+    dhdx[3:4, 3:6] = _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(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.
+    """
+    v_n[:] += dx[0:3]
+    _update_quaternion_with_gibbs2(q_nb, dx[3:6])
+    bg_b[:] += dx[6:9]
+    dx[:] = 0.0
+
+
+class AHRSv2:
+    """
+    Attitude and Heading Reference System (AHRS) using a multiplicative extended
+    Kalman filter (MEKF).
+
+    Parameters
+    ----------
+    fs : float
+        Sampling rate in Hz.
+    v : 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
+        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.
+
+    """
+
+    def __init__(
+        self,
+        fs: float,
+        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),
+        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()
+        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
+        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).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._Q = _process_noise_covariance_matrix(
+            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 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]:
+        """
+        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_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,
+        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
+        ----------
+        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.
+        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.
+        """
+
+        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
+        R_nb = _rot_matrix_from_quaternion(self._q_nb)
+        _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)
+        _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 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
+        _reset(self._v_n, self._q_nb, self._bg_b, self._dx)
+
+        return self
diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py
new file mode 100644
index 00000000..a21ddc75
--- /dev/null
+++ b/tests/test_ins_v2.py
@@ -0,0 +1,106 @@
+import numpy as np
+import pytest
+from scipy.signal import resample_poly
+
+import smsfusion as sf
+from smsfusion._ins_v2 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