diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index e00d3aa1..0dac21df 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,11 +1,12 @@ from . import benchmark, calibrate, constants, noise from ._coning_sculling import ConingScullingAlg, ConingScullingAlgCalibrated -from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity +from ._ins import AHRS, VRU, AHRSv2, AidedINS, FixedNED, StrapdownINS, VRUv2, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler __all__ = [ "AHRS", + "AHRSv2", "AidedINS", "benchmark", "constants", @@ -16,6 +17,7 @@ "noise", "StrapdownINS", "VRU", + "VRUv2", "quaternion_from_euler", "ConingScullingAlg", "ConingScullingAlgCalibrated", diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py new file mode 100644 index 00000000..bc526a66 --- /dev/null +++ b/src/smsfusion/_ins/__init__.py @@ -0,0 +1,4 @@ +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 diff --git a/src/smsfusion/_ins/_ahrs.py b/src/smsfusion/_ins/_ahrs.py new file mode 100644 index 00000000..41b359f7 --- /dev/null +++ b/src/smsfusion/_ins/_ahrs.py @@ -0,0 +1,457 @@ +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 +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, 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), +) + + +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 (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 _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 (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 + 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 (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_init(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Measurement matrix. + + Parameters + ---------- + q_nb : ndarray, shape (4,) + Unit quaternion. + + Returns + ------- + ndarray, shape (4, 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 + + +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], + 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. + + 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] + 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 AHRS: + """ + Attitude and Heading Reference System (AHRS) using a multiplicative extended + Kalman filter (MEKF). + + Parameters + ---------- + fs : float + Sampling rate in Hz. + 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 + 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, + 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", + ) -> 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(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._dx = np.zeros(9) + + # 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) + + 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, + 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 = 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. + 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 + ------- + AHRS + 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._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 vel is not None: + 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._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 + 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..b6d94a13 --- /dev/null +++ b/src/smsfusion/_ins/_aiding.py @@ -0,0 +1,107 @@ +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_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], + 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/_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/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py new file mode 100644 index 00000000..3a985918 --- /dev/null +++ b/src/smsfusion/_ins/_common.py @@ -0,0 +1,381 @@ +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 # type: ignore[misc] +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 _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 + + +@njit # type: ignore[misc] +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/_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 new file mode 100644 index 00000000..78214527 --- /dev/null +++ b/src/smsfusion/_ins/_vru.py @@ -0,0 +1,330 @@ +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), +) + + +@njit # type: ignore[misc] +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 + + +@njit # type: ignore[misc] +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 (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 + + +@njit # type: ignore[misc] +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)). + 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, + 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._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.pi / 180.0) * dtheta + + 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) + + # Project (a priori) state estimates ahead + self._q_nb = _update_quaternion_with_rotvec(self._q_nb, dtheta) + + # 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/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 new file mode 100644 index 00000000..ab21d85f --- /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._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 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_calibrate.py b/tests/test_calibrate.py index 25a8b5c5..5b81a88c 100644 --- a/tests/test_calibrate.py +++ b/tests/test_calibrate.py @@ -62,8 +62,8 @@ def test_exact_alternate(self, xyz_ref): @pytest.mark.parametrize( "xyz_ref", [ - np.random.default_rng().random(size=(400, 3)), - np.random.default_rng().random(size=(4000, 3)), + np.random.default_rng(23).random(size=(400, 3)), + np.random.default_rng(32).random(size=(4000, 3)), ], ) def test_noisy(self, xyz_ref): 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_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 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 diff --git a/tests/test_ins_/test_common.py b/tests/test_ins_/test_common.py new file mode 100644 index 00000000..4a916883 --- /dev/null +++ b/tests/test_ins_/test_common.py @@ -0,0 +1,221 @@ +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 + ) + + +@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 new file mode 100644 index 00000000..957b2799 --- /dev/null +++ b/tests/test_ins_/test_utils.py @@ -0,0 +1,135 @@ +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, -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_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 + 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 + 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) + +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", + [ + (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 diff --git a/tests/test_ins_/test_vru.py b/tests/test_ins_/test_vru.py new file mode 100644 index 00000000..d8197b0b --- /dev/null +++ b/tests/test_ins_/test_vru.py @@ -0,0 +1,193 @@ +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, _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 + 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_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)