diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 9f22b33c..76091773 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,31 +1,21 @@ from . import benchmark, calibrate, constants, noise from ._coning_sculling import ConingScullingAlg, ConingScullingAlgCalibrated from ._ins import ( - AHRS, AMEKF, PVAMEKF, VAMEKF, - VRU, - AidedINS, FixedNED, - StrapdownINS, gravity, ) -from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler __all__ = [ - "AHRS", "AMEKF", "PVAMEKF", "VAMEKF", - "VRU", - "AidedINS", "ConingScullingAlg", "ConingScullingAlgCalibrated", - "FixedIntervalSmoother", "FixedNED", - "StrapdownINS", "benchmark", "calibrate", "constants", diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py index e8c09d95..7338061a 100644 --- a/src/smsfusion/_ins/__init__.py +++ b/src/smsfusion/_ins/__init__.py @@ -1,18 +1,13 @@ -from ._ains_legacy import AHRS, VRU, AidedINS, StrapdownINS from ._amekf import AMEKF from ._pvamekf import PVAMEKF from ._utils import FixedNED, euler_from_acc, gravity from ._vamekf import VAMEKF __all__ = [ - "AHRS", "AMEKF", "PVAMEKF", "VAMEKF", - "VRU", - "AidedINS", "FixedNED", - "StrapdownINS", "euler_from_acc", "gravity", ] diff --git a/src/smsfusion/_ins/_ains_legacy.py b/src/smsfusion/_ins/_ains_legacy.py deleted file mode 100644 index 977e2962..00000000 --- a/src/smsfusion/_ins/_ains_legacy.py +++ /dev/null @@ -1,1276 +0,0 @@ -from __future__ import annotations - -from typing import Any, Self - -import numpy as np -from numba import njit -from numpy.typing import ArrayLike, NDArray - -from smsfusion._ins._common import ( - _signed_smallest_angle, - _yaw_from_quaternion, - _yaw_gradient, -) -from smsfusion._transforms import ( - _angular_matrix_from_quaternion, - _euler_from_quaternion, - _quaternion_from_euler, - _rot_matrix_from_quaternion, -) -from smsfusion._vectorops import _normalize, _quaternion_product, _skew_symmetric -from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0 - - -def _roll_pitch_from_acc(f, nav_frame): - """ - Estimate roll and pitch angles from specific force (i.e., accelerometer) measurement. - - Parameters - ---------- - f: array-like - Specific force (i.e., acceleration) measurement vector (fx, fy, fz). - nav_frame: {'NED', 'ENU'} - Navigation frame. Should be either 'NED' or 'ENU'. - - Returns - ------- - roll: float - Estimated roll angle in radians. - pitch: float - Estimated pitch angle in radians. - """ - - fx, fy, fz = f - - if nav_frame.lower() == "ned": - roll = np.arctan2(-fy, -fz) - pitch = np.arctan2(fx, np.sqrt(fy**2 + fz**2)) - elif nav_frame.lower() == "enu": - roll = np.arctan2(fy, fz) - pitch = -np.arctan2(fx, np.sqrt(fy**2 + fz**2)) - else: - raise ValueError("Invalid navigation frame. Should be 'NED' or 'ENU'.") - - return roll, pitch - - -class INSMixin: - """ - Mixin class for inertial navigation systems (INS). - - Requires that the inheriting class has an `_x` attribute which is a 1D numpy array - of length 16 containing the following elements in order: - - * Position in x, y, z directions (3 elements). - * Velocity in x, y, z directions (3 elements). - * Attitude as unit quaternion (4 elements). - * Accelerometer bias in x, y, z directions (3 elements). - * Gyroscope bias in x, y, z directions (3 elements). - """ - - _x: NDArray[np.float64] # state array of length 16 - - @property - def _pos(self) -> NDArray[np.float64]: - return self._x[0:3] - - @_pos.setter - def _pos(self, p: ArrayLike) -> None: - self._x[0:3] = p - - @property - def _vel(self) -> NDArray[np.float64]: - return self._x[3:6] - - @_vel.setter - def _vel(self, v: ArrayLike) -> None: - self._x[3:6] = v - - @property - def _q_nm(self) -> NDArray[np.float64]: - return self._x[6:10] - - @_q_nm.setter - def _q_nm(self, q_nm: ArrayLike) -> None: - self._x[6:10] = q_nm - - @property - def _bias_acc(self) -> NDArray[np.float64]: - return self._x[10:13] - - @_bias_acc.setter - def _bias_acc(self, b_acc: ArrayLike) -> None: - self._x[10:13] = b_acc - - @property - def _bias_gyro(self) -> NDArray[np.float64]: - return self._x[13:16] - - @_bias_gyro.setter - def _bias_gyro(self, b_gyro: ArrayLike) -> None: - self._x[13:16] = b_gyro - - @property - def x(self) -> NDArray[np.float64]: - """ - Get current state vector estimate. - - Returns - ------- - numpy.ndarray, shape (16,) - State vector, containing the following elements in order: - - * Position in x, y, z directions (3 elements). - * Velocity in x, y, z directions (3 elements). - * Attitude as unit quaternion (4 elements). - * Accelerometer bias in x, y, z directions (3 elements). - * Gyroscope bias in x, y, z directions (3 elements). - """ - return self._x.copy() - - def position(self) -> NDArray[np.float64]: - """ - Get current position estimate. - - Returns - ------- - numpy.ndarray, shape (3,) - Position state vector, containing position in x-, y-, and z-direction - (in that order). - """ - return self._pos.copy() - - def velocity(self) -> NDArray[np.float64]: - """ - Get current velocity estimate. - - Returns - ------- - numpy.ndarray, shape (3,) - Velocity state vector, containing (linear) velocity in x-, y-, and z-direction - (in that order). - """ - return self._vel.copy() - - def euler(self, degrees: bool = False) -> NDArray[np.float64]: - """ - Get current attitude estimate as Euler angles (see Notes). - - Parameters - ---------- - degrees : bool, default False - Whether to return the Euler angles in degrees or radians. - - Returns - ------- - numpy.ndarray, shape (3,) - Euler angles, specifically: alpha (roll), beta (pitch) and gamma (yaw) - in that order. - - Notes - ----- - The Euler angles describe how to transition from the 'navigation' frame - ('NED' or 'ENU) to the 'body' frame through three consecutive intrinsic - and passive rotations in the ZYX order: - - #. A rotation by an angle gamma (often called yaw) about the z-axis. - #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. - #. A final rotation by an angle alpha (often called roll) about the x-axis. - - This sequence of rotations is used to describe the orientation of the 'body' frame - relative to the 'navigation' frame ('NED' or 'ENU) in 3D space. - - Intrinsic rotations mean that the rotations are with respect to the changing - coordinate system; as one rotation is applied, the next is about the axis of - the newly rotated system. - - Passive rotations mean that the frame itself is rotating, not the object - within the frame. - """ - q = self.quaternion() - theta = _euler_from_quaternion(q) - - if degrees: - theta = (180.0 / np.pi) * theta - - return theta # type: ignore[no-any-return] - - def quaternion(self) -> NDArray[np.float64]: - """ - Get current attitude estimate as unit quaternion (from-body-to-navigation-frame). - - Returns - ------- - numpy.ndarray, shape (4,) - Attitude as unit quaternion. Given as ``[q1, q2, q3, q4]``, where - ``q1`` is the real part and ``q2``, ``q3`` and ``q4`` are the three - imaginary parts. - """ - return self._q_nm.copy() - - def bias_acc(self) -> NDArray[np.float64]: - """ - Get current accelerometer bias estimate. - - Returns - ------- - numpy.ndarray, shape (3,) - Accelerometer bias vector, containing biases in x-, y-, and z-direction - (in that order). - """ - return self._bias_acc.copy() - - def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]: - """ - Get current gyroscope bias estimate. - - Parameters - ---------- - degrees : bool, default False - Whether to return the bias in deg/s or rad/s. - - Returns - ------- - numpy.ndarray, shape (3,) - Gyroscope bias vector, containing biases in x-, y-, and z-direction - (in that order). - """ - b_gyro = self._bias_gyro.copy() - if degrees: - b_gyro = (180.0 / np.pi) * b_gyro - return b_gyro - - -class StrapdownINS(INSMixin): - """ - Strapdown inertial navigation system (INS). - - This class provides an interface for estimating position, velocity and attitude - of a moving body by integrating the *strapdown navigation equations*. - - Parameters - ---------- - fs : float - Sampling rate in Hz. - x0 : array-like, shape (16,) - Initial state vector containing the following elements in order: - - * Position in x, y, z directions (3 elements). - * Velocity in x, y, z directions (3 elements). - * Attitude as unit quaternion (4 elements). - * Accelerometer bias in x, y, z directions (3 elements). - * Gyroscope bias in x, y, z directions (3 elements). - g : float, default 9.80665 - The gravitational acceleration. Default is 'standard gravity' of 9.80665. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. - - Notes - ----- - The quaternion provided as part of the initial state will be normalized to - ensure unity. - """ - - def __init__( - self, fs: float, x0: ArrayLike, g: float = 9.80665, nav_frame="NED" - ) -> None: - self._fs = fs - self._dt = 1.0 / fs - - self._x0 = np.asarray_chkfinite(x0).reshape(16).copy() - self._x0[6:10] = _normalize(self._x0[6:10]) - self._x = self._x0.copy() - self._g = g - self._nav_frame = nav_frame.lower() - - if self._nav_frame == "ned": - self._g_n = np.array([0.0, 0.0, g]) - elif self._nav_frame == "enu": - self._g_n = np.array([0.0, 0.0, -g]) - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - - def reset(self, x_new: ArrayLike) -> None: - """ - Reset current state with a new one. - - Parameters - ---------- - x_new : numpy.ndarray, shape (10,) - New state vector, containing the following elements in order: - - * Position in x-, y-, and z-direction (3 elements). - * Velocity in x-, y-, and z-direction (3 elements). - * Attitude as unit quaternion (4 elements). Should be given as - [q1, q2, q3, q4], where q1 is the real part and q1, q2 and q3 - are the three imaginary parts. - - Notes - ----- - The quaternion provided as part of the new state will be normalized to - ensure unity. - """ - self._x = np.asarray_chkfinite(x_new).reshape(16).copy() - self._x[6:10] = _normalize(self._x[6:10]) - - def update( - self, - f_imu: ArrayLike, - w_imu: ArrayLike, - degrees: bool = False, - ) -> Self: - """ - Update the INS states by integrating the *strapdown navigation equations*. - - Assuming constant inputs (i.e., accelerations and angular velocities) over - the sampling period. - - The states are updated according to:: - - p[k+1] = p[k] + h * v[k] + 0.5 * dt * a[k] - - v[k+1] = v[k] + dt * a[k] - - q[k+1] = q[k] + dt * T(q[k]) * w_ins[k] - - with bias compensated IMU measurements:: - - f_ins[k] = f_imu[k] - b_acc[k] - - w_ins[k] = w_imu[k] - b_gyro[k] - - and:: - - a[k] = R(q[k]) * f_ins[k] + g - - g = [0, 0, 9.81]^T - - Parameters - ---------- - f_imu : array-like, shape (3,) - Specific force measurements (i.e., accelerations + gravity), given - as [f_x, f_y, f_z]^T where f_x, f_y and f_z are - acceleration measurements in x-, y-, and z-direction, respectively. - w_imu : array-like, shape (3,) - Angular rate measurements, given as [w_x, w_y, w_z]^T where - w_x, w_y and w_z are angular rates about the x-, y-, - and z-axis, respectively. - degrees : bool, default False - Specify whether the angular rates are given in degrees or radians. - - Returns - ------- - StrapdownINS : - A reference to the instance itself after the update. - """ - f_imu = np.asarray(f_imu, dtype=float) - w_imu = np.asarray(w_imu, dtype=float) - - if degrees: - w_imu = (np.pi / 180.0) * w_imu - - # Bias compensated IMU measurements - f_ins = f_imu - self._bias_acc - w_ins = w_imu - self._bias_gyro - - q_nm = self._q_nm - R_nm = _rot_matrix_from_quaternion(q_nm) # body-to-ned - T = _angular_matrix_from_quaternion(q_nm) - - # State propagation (assuming constant linear acceleration and angular velocity) - acc = R_nm @ f_ins + self._g_n - self._pos = self._pos + self._dt * self._vel - self._vel = self._vel + self._dt * acc - q_nm = q_nm + self._dt * T @ w_ins - self._q_nm = _normalize(q_nm) - - return self - - -class AidedINS(INSMixin): - """ - Aided inertial navigation system (AINS) using a multiplicative extended - Kalman filter (MEKF). - - Parameters - ---------- - fs : float - Sampling rate in Hz. - x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0` - Initial (a priori) 16-element INS state estimate: - - * Position (x, y, z) - 3 elements - * Velocity (x, y, z) - 3 elements - * Attitude (unit quaternion) - 4 elements - * Accelerometer bias (x, y, z) - 3 elements - * Gyroscope bias (x, y, z) - 3 elements - - Defaults to a zero vector, but with the attitude part as a unit quaternion - (i.e., no rotation). - P0_prior : array-like (shape (12, 12) or (15, 15)), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`) - Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a - small diagonal matrix will be used. If the accelerometer bias is excluded from the - error estimate (see ``ignore_bias_acc``), the covariance matrix should be of shape - (12, 12), otherwise (15, 15). - err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2` - Dictionary containing accelerometer noise parameters with keys: - - * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz). - * ``B``: Bias stability in m/s^2. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2` - Dictionary containing gyroscope noise parameters with keys: - - * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz). - * ``B``: Bias stability in rad/s. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - g : float, default 9.80665 - The gravitational acceleration. Default is 'standard gravity' of 9.80665. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. Furthermore, the aiding heading angle is - also interpreted relative to this frame according to the right-hand rule. - lever_arm : array-like, shape (3,), default numpy.zeros(3) - Lever-arm vector describing the location of position aiding (in meters) relative - to the IMU expressed in the IMU's measurement frame. For instance, the location - of the GNSS antenna relative to the IMU. By default it is assumed that the - aiding position coincides with the IMU's origin. - ignore_bias_acc : bool, default True - Determines whether the accelerometer bias should be included in the error estimate. - If set to ``True``, the accelerometer bias provided in ``x0`` during initialization - will remain fixed and not updated. This option is useful in situations where the - accelerometer bias is unobservable, such as when there is insufficient aiding - information or minimal dynamic motion, making bias estimation unreliable. Note - that this will reduce the error-state dimension from 15 to 12, and hence also the - error covariance matrix, **P**, from dimension (15, 15) to (12, 12). When set to - ``False``, the P0_prior argument must have shape (15, 15). - cold_start : bool, default True - Whether to start the AINS filter in a 'cold' (default) or 'warm' state. - A cold state indicates that the provided initial conditions are uncertain, - and possibly far from the true state. Thus, to reduce the risk of divergence, - an initial vertical alignment (i.e., roll and pitch calibration) is performed - using accelerometer measurements and the known direction of gravity during - the first measurement update. The IMU should remain stationary with negligible - linear acceleration during a cold start; otherwise, divergence may occur. - A warm start, on the other hand, assumes accurate initial conditions, and - initializes the Kalman filter immediately without any initial roll and pitch - calibration. - """ - - # Permutation matrix for reordering error-state bias terms, such that: - # [pos, vel, quat, b_gyro, b_acc]^T = T_dx @ [pos, vel, quat, b_acc, b_gyro]^T - _T_dx = np.zeros((15, 15)) - _T_dx[:9, :9] = np.eye(9) - _T_dx[9:12, 12:15] = np.eye(3) - _T_dx[12:15, 9:12] = np.eye(3) - - # Permutation matrix for reordering white noise bias terms, such that: - # [acc, gyro, b_gyro, b_acc]^T = T_wn @ [acc, gyro, b_acc, b_gyro]^T - _T_wn = np.zeros((12, 12)) - _T_wn[:6, :6] = np.eye(6) - _T_wn[6:9, 9:12] = np.eye(3) - _T_wn[9:12, 6:9] = np.eye(3) - - def __init__( - self, - fs: float, - x0_prior: ArrayLike = X0, - P0_prior: ArrayLike = P0, - err_acc: dict[str, float] = ERR_ACC_MOTION2, - err_gyro: dict[str, float] = ERR_GYRO_MOTION2, - g: float = 9.80665, - nav_frame: str = "NED", - lever_arm: ArrayLike = np.zeros(3), - ignore_bias_acc: bool = True, - cold_start: bool = True, - ) -> None: - self._fs = fs - self._dt = 1.0 / fs - self._err_acc = err_acc - self._err_gyro = err_gyro - self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy() - self._ignore_bias_acc = ignore_bias_acc - self._cold = cold_start - self._dq_prealloc = np.array([2.0, 0.0, 0.0, 0.0]) # Preallocation - - # Strapdown algorithm / INS state - self._ins = StrapdownINS(self._fs, x0_prior, g=g, nav_frame=nav_frame) - self._vg_ref_n = _normalize(self._ins._g_n) # gravity reference vector - - # Total state estimate - self._x = self._ins.x - - # Error state estimate (after reset) - self._dx_prealloc = np.zeros(15) # always zero, but used in sequential update - - # Initialize Kalman filter - self._P_prior = np.asarray_chkfinite(P0_prior).copy(order="C") - self._P = self._P_prior.copy(order="C") - - # Verify error covariance matrix shape - if ignore_bias_acc and self._P_prior.shape != (12, 12): - raise ValueError( - f"P0_prior must be of shape (12, 12) when ignore_bias_acc is set to True. Was {self._P_prior.shape}." - ) - if not ignore_bias_acc and self._P_prior.shape != (15, 15): - raise ValueError( - f"P0_prior must be of shape (15, 15) when ignore_bias_acc is set to False. Was {self._P_prior.shape}." - ) - - # Prepare system matrices - q0 = self._ins._q_nm - self._F = self._prep_F(err_acc, err_gyro, q0) - self._G = self._prep_G(q0) - self._H = self._prep_H() - self._W = self._prep_W(err_acc, err_gyro) - self._I = np.eye(15, order="C") - - # Filter out the accelerometer bias terms from the system matrices (if ignored) - if self._ignore_bias_acc: - dx_dim = 12 - wn_dim = 9 - self._F = (self._T_dx @ self._F @ self._T_dx.T)[:dx_dim, :dx_dim] - self._G = (self._T_dx @ self._G @ self._T_wn)[:dx_dim, :wn_dim] - self._H = (self._H @ self._T_dx)[:, :dx_dim] - self._W = (self._T_wn @ self._W @ self._T_wn.T)[:wn_dim, :wn_dim] - self._I = self._I[:dx_dim, :dx_dim] - self._dx_prealloc = self._dx_prealloc[:dx_dim] - - # Error-state estimate (before reset) - self._dx = np.empty_like(self._dx_prealloc) # needed for smoothing only - - # State transition matrix - self._phi = np.empty_like(self._F) # needed for smoothing only - - @property - def x_prior(self) -> NDArray[np.float64]: - """ - Next a priori state vector estimate. - - Returns - ------- - numpy.ndarray, shape (16,) - A priori state vector estimate, containing the following elements in order: - - * Position in x, y, z directions (3 elements). - * Velocity in x, y, z directions (3 elements). - * Attitude as unit quaternion (4 elements). - * Accelerometer bias in x, y, z directions (3 elements). - * Gyroscope bias in x, y, z directions (3 elements). - """ - return self._ins.x - - def dump( - self, - ) -> dict[str, np.float64 | list[np.float64] | dict[str, np.float64] | bool]: - """ - Dump the configuration and current state of the AINS to a dictionary. The dumped - parameters can be used to restore the AINS to its current state. - - Returns - ------- - dict - A dictionary containing the configuration and current state of the AINS. - """ - params = { - "fs": self._fs, - "x0_prior": self.x_prior.tolist(), - "P0_prior": self.P_prior.tolist(), - "err_acc": self._err_acc, - "err_gyro": self._err_gyro, - "g": self._ins._g, - "nav_frame": self._ins._nav_frame, - "lever_arm": self._lever_arm.tolist(), - "ignore_bias_acc": self._ignore_bias_acc, - "cold_start": self._cold, - } - return params - - @property - def P(self) -> NDArray[np.float64]: - """ - Error covariance matrix, **P**. I.e., the error covariance matrix associated with - the Kalman filter's updated (a posteriori) error-state estimate. - """ - P = self._P.copy() - return P - - @property - def P_prior(self) -> NDArray[np.float64]: - """ - Next (a priori) estimate of the error covariance matrix, **P**. I.e., the error - covariance matrix associated with the Kalman filter's projected (a priori) - error-state estimate. - """ - P_prior = self._P_prior.copy() - return P_prior - - @staticmethod - def _prep_F( - err_acc: dict[str, float], - err_gyro: dict[str, float], - q_nm: NDArray[np.float64], - ) -> NDArray[np.float64]: - """ - Prepare linearized state matrix, F. - """ - - beta_acc = 1.0 / err_acc["tau_cb"] - beta_gyro = 1.0 / err_gyro["tau_cb"] - - # Temporary placeholder vectors (to be replaced each timestep) - f_ins = np.array([0.0, 0.0, 0.0]) - w_ins = np.array([0.0, 0.0, 0.0]) - - S = _skew_symmetric # alias skew symmetric matrix - R_nm = _rot_matrix_from_quaternion(q_nm) # body-to-ned rotation matrix - - # State transition matrix - F = np.zeros((15, 15)) - F[0:3, 3:6] = np.eye(3) - F[3:6, 6:9] = -R_nm @ S(f_ins) # NB! update each time step - F[3:6, 9:12] = -R_nm # NB! update each time step - F[6:9, 6:9] = -S(w_ins) # NB! update each time step - F[6:9, 12:15] = -np.eye(3) - F[9:12, 9:12] = -beta_acc * np.eye(3) - F[12:15, 12:15] = -beta_gyro * np.eye(3) - - return F - - def _update_F( - self, - R_nm: NDArray[np.float64], - f_ins: NDArray[np.float64], - w_ins: NDArray[np.float64], - ) -> None: - """Update linearized state transition matrix, F.""" - S = _skew_symmetric # alias skew symmetric matrix - - # Update matrix - self._F[3:6, 6:9] = -R_nm @ S(f_ins) # NB! update each time step - self._F[6:9, 6:9] = -S(w_ins) # NB! update each time step - if not self._ignore_bias_acc: - self._F[3:6, 9:12] = -R_nm # NB! update each time step - - @staticmethod - def _prep_G(q_nm: NDArray[np.float64]) -> NDArray[np.float64]: - """Prepare (white noise) input matrix, G.""" - R_nm = _rot_matrix_from_quaternion(q_nm) # body-to-ned rotation matrix - - # Input (white noise) matrix - G = np.zeros((15, 12)) - G[3:6, 0:3] = -R_nm # NB! update each time step - G[6:9, 3:6] = -np.eye(3) - G[9:12, 6:9] = np.eye(3) - G[12:15, 9:12] = np.eye(3) - return G - - def _update_G(self, R_nm: NDArray[np.float64]) -> None: - """Update (white noise) input matrix, G.""" - - # Update matrix - self._G[3:6, 0:3] = -R_nm - - @staticmethod - def _prep_H() -> NDArray[np.float64]: - """Prepare linearized measurement matrix, H. Values are placeholders only""" - H = np.zeros((10, 15)) - H[0:3, 0:3] = np.eye(3) # position - H[3:6, 3:6] = np.eye(3) # velocity - return H - - def _update_H_pos( - self, R_nm: NDArray[np.float64], lever_arm: NDArray[np.float64] - ) -> NDArray[np.float64]: - """Update and return part of H matrix relevant for position aiding.""" - S = _skew_symmetric - self._H[0:3, 6:9] = -R_nm @ S(lever_arm) - return self._H[0:3] - - def _update_H_vel(self) -> NDArray[np.float64]: - """Update and return part of H matrix relevant for velocity aiding.""" - return self._H[3:6] - - def _update_H_g_ref(self, R_nm: NDArray[np.float64]) -> NDArray[np.float64]: - """Update and return part of H matrix relevant for g_ref aiding.""" - S = _skew_symmetric - self._H[6:9, 6:9] = S(R_nm.T @ self._vg_ref_n) - return self._H[6:9] - - def _update_yaw_from_quaternion( - self, q_nm: NDArray[np.float64] - ) -> NDArray[np.float64]: - """Update and return part of H matrix relevant for heading aiding.""" - self._H[9:10, 6:9] = _yaw_gradient(q_nm) - return self._H[9:10] - - @staticmethod - def _prep_W( - err_acc: dict[str, float], err_gyro: dict[str, float] - ) -> NDArray[np.float64]: - """Prepare white noise power spectral density matrix""" - N_acc = err_acc["N"] - sigma_acc = err_acc["B"] - beta_acc = 1.0 / err_acc["tau_cb"] - N_gyro = err_gyro["N"] - sigma_gyro = err_gyro["B"] - beta_gyro = 1.0 / err_gyro["tau_cb"] - - # White noise power spectral density matrix - W = np.eye(12) - W[0:3, 0:3] *= N_acc**2 - W[3:6, 3:6] *= N_gyro**2 - W[6:9, 6:9] *= 2.0 * sigma_acc**2 * beta_acc - W[9:12, 9:12] *= 2.0 * sigma_gyro**2 * beta_gyro - return W - - def _reset_ins(self, dx: NDArray[np.float64]) -> None: - """Combine states and reset INS""" - da = dx[6:9] - self._dq_prealloc[1:4] = da - dq = (1.0 / np.sqrt(4.0 + da.T @ da)) * self._dq_prealloc - self._ins._x[:3] = self._ins._x[:3] + dx[:3] - self._ins._x[3:6] = self._ins._x[3:6] + dx[3:6] - self._ins._x[6:10] = _quaternion_product(self._ins._x[6:10], dq) - self._ins._x[6:10] = _normalize(self._ins._x[6:10]) - self._ins._x[-3:] = self._ins._x[-3:] + dx[-3:] - if not self._ignore_bias_acc: - self._ins._x[10:13] = self._ins._x[10:13] + dx[9:12] - self._dx_prealloc[:] = np.zeros(dx.size) - - @staticmethod - @njit # type: ignore[misc] - def _update_dx_P( - dx: NDArray[np.float64], - P: NDArray[np.float64], - dz: NDArray[np.float64], - var: NDArray[np.float64], - H: NDArray[np.float64], - I_: NDArray[np.float64], - ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: - for i, (dz_i, var_i) in enumerate(zip(dz, var)): - H_i = np.ascontiguousarray(H[i, :]) - K_i = P @ H_i.T / (H_i @ P @ H_i.T + var_i) - dx += K_i * (dz_i - H_i @ dx) - K_i = np.ascontiguousarray(K_i[:, np.newaxis]) # as 2D array - H_i = np.ascontiguousarray(H_i[np.newaxis, :]) # as 2D array - P = (I_ - K_i @ H_i) @ P @ (I_ - K_i @ H_i).T + var_i * K_i @ K_i.T - return dx, P - - def _align_vertical(self, f_ins, head, head_degrees): - """ - Vertical alignment. - - Estimate the attitude (roll and pitch) of the IMU sensor relative to the - navigation frame using accelerometer measurements and the known direction - of gravity. Assumes a static sensor; i.e., negligible linear acceleration. - - Parameters - ---------- - f_ins : array-like, shape (3,) - Bias-compensated specific force measurements (fx, fy, fz). - head : float, optional - Heading of measurement frame relative to navigation frame. - head_degrees : bool, default False - Specifies whether the heading is given in degrees or radians. - """ - if head is None: - head = _yaw_from_quaternion(self.quaternion()) - else: - if head_degrees: - head = (np.pi / 180.0) * head - - roll, pitch = _roll_pitch_from_acc(f_ins, self._ins._nav_frame) - self._ins._x[6:10] = _quaternion_from_euler(np.array([roll, pitch, head])) - self._x[:] = self._ins._x - - def update( - self, - f_imu: ArrayLike, - w_imu: ArrayLike, - degrees: bool = False, - pos: ArrayLike | None = None, - pos_var: ArrayLike | None = None, - vel: ArrayLike | None = None, - vel_var: ArrayLike | None = None, - head: float | None = None, - head_var: float | None = None, - head_degrees: bool = True, - g_ref: bool = False, - g_var: ArrayLike | None = None, - ) -> Self: - """ - Update/correct the AINS' state estimate with aiding measurements, and project - ahead using IMU measurements. - - If no aiding measurements are provided, the AINS is simply propagated ahead - using dead reckoning with the IMU measurements. - - Parameters - ---------- - f_imu : array-like, shape (3,) - Specific force measurements (i.e., accelerations + gravity), given - as [f_x, f_y, f_z]^T where f_x, f_y and f_z are - acceleration measurements in x-, y-, and z-direction, respectively. - w_imu : array-like, shape (3,) - Angular rate measurements, given as [w_x, w_y, w_z]^T where - w_x, w_y and w_z are angular rates about the x-, y-, - and z-axis, respectively. - degrees : bool, default False - Specifies whether the unit of ``w_imu`` are in degrees or radians. - pos : array-like, shape (3,), optional - Position aiding measurement in m. If ``None``, position aiding is not used. - pos_var : array-like, shape (3,), optional - Variance of position measurement noise in m^2. Required for ``pos``. - vel : array-like, shape (3,), optional - Velocity aiding measurement in m/s. If ``None``, velocity aiding is not used. - vel_var : array-like, shape (3,), optional - Variance of velocity measurement noise in (m/s)^2. Required for ``vel``. - head : float, optional - Heading measurement. I.e., the yaw angle of the 'body' frame relative to the - assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. - If ``None``, compass aiding is not used. See ``head_degrees`` for units. - head_var : float, optional - Variance of heading measurement noise. Units must be compatible with ``head``. - See ``head_degrees`` for units. Required for ``head``. - head_degrees : bool, default False - Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, - or radians and radians^2. Default is in radians and radians^2. - g_ref : bool, optional, default False - Specifies whether the gravity reference vector is used as an aiding measurement. - g_var : array-like, shape (3,), optional - Variance of gravitational reference vector measurement noise. Required for - ``g_ref``. - - Returns - ------- - AidedINS - A reference to the instance itself after the update. - """ - - f_imu = np.asarray(f_imu, dtype=float) - w_imu = np.asarray(w_imu, dtype=float) - - if degrees: - w_imu = (np.pi / 180.0) * w_imu - - # Bias compensated IMU measurements - f_ins = f_imu - self._ins._bias_acc - w_ins = w_imu - self._ins._bias_gyro - - # Initial vertical alignment (i.e., roll and pitch calibration) - if self._cold: - self._align_vertical(f_ins, head, head_degrees) - self._cold = False - - # Current INS state estimates - pos_ins = self._ins._pos - vel_ins = self._ins._vel - q_ins_nm = self._ins._q_nm - R_ins_nm = _rot_matrix_from_quaternion(q_ins_nm) # body-to-inertial rot matrix - - # Aliases - dx = self._dx_prealloc # zeros - dt = self._dt - F = self._F - G = self._G - W = self._W - P = self._P_prior - I_ = self._I - - # Lever arm vector - IMU-to-aiding - lever_arm = self._lever_arm - - # Update system matrices - self._update_F(R_ins_nm, f_ins, w_ins) - self._update_G(R_ins_nm) - - # Update with available aiding measurements - if pos is not None: - if pos_var is None: - raise ValueError("'pos_var' not provided.") - - pos = np.asarray(pos, dtype=float, order="C") - pos_var = np.asarray(pos_var, dtype=float, order="C") - dz_pos = pos - pos_ins - R_ins_nm @ lever_arm - H_pos = self._update_H_pos(R_ins_nm, lever_arm) - dx, P = self._update_dx_P(dx, P, dz_pos, pos_var, H_pos, I_) - - if vel is not None: - if vel_var is None: - raise ValueError("'vel_var' not provided.") - - vel = np.asarray(vel, dtype=float, order="C") - vel_var = np.asarray(vel_var, dtype=float, order="C") - dz_vel = vel - vel_ins - H_vel = self._update_H_vel() - dx, P = self._update_dx_P(dx, P, dz_vel, vel_var, H_vel, I_) - - if g_ref: - if g_var is None: - raise ValueError("'g_var' not provided.") - vg_meas_m = -_normalize(f_ins) - g_var = np.asarray(g_var, dtype=float, order="C") - dz_g = vg_meas_m - R_ins_nm.T @ self._vg_ref_n - H_g = self._update_H_g_ref(R_ins_nm) - dx, P = self._update_dx_P(dx, P, dz_g, g_var, H_g, I_) - - if head is not None: - if head_var is None: - raise ValueError("'head_var' not provided.") - - if head_degrees: - head = (np.pi / 180.0) * head - head_var = (np.pi / 180.0) ** 2 * head_var - - head_var_ = np.asarray([head_var], dtype=float, order="C") - dz_head = np.asarray( - [ - _signed_smallest_angle( - head - _yaw_from_quaternion(q_ins_nm), degrees=False - ) - ], - dtype=float, - order="C", - ) - - H_head = self._update_yaw_from_quaternion(q_ins_nm) - dx, P = self._update_dx_P(dx, P, dz_head, head_var_, H_head, I_) - - self._dx[:] = dx.ravel().copy() - - # Reset INS state - if dx.any(): - self._reset_ins(dx.ravel()) - - # Discretize system - self._phi[:] = I_ + dt * F # state transition matrix - Q = dt * G @ W @ G.T # process noise covariance matrix - - # Update current state - self._x[:] = self._ins._x - self._P[:] = P - - # Project ahead - self._ins.update(f_imu, w_imu, degrees=False) - self._P_prior[:] = self._phi @ P @ self._phi.T + Q - - return self - - -class VRU(AidedINS): - """ - Vertical Reference Unit (VRU) based on a multiplicative extended Kalman filter - (MEKF). - - VRU is intended for applicatoins with negligble sustained linear accelerations. - For applications with sustained linear accelerations, accurate position and/or - velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for - those cases. - - This class inherits from :class:`smsfusion.AidedINS` but applies sensible - defaults for vertical reference applications and simplifies the interface by - hiding non-essential configuration options. - - Velocity aiding is set to zero with a default standard deviation of 10 m/s. - Position aiding also assumes zero values but with high uncertainty (default - standard deviation of 1000 m), making it effectively non-constraining. Heading - aiding is completely disabled. - - Parameters - ---------- - fs : float - Sampling rate in Hz. - x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0` - Initial (a priori) 16-element INS state estimate: - - * Position (x, y, z) - 3 elements - * Velocity (x, y, z) - 3 elements - * Attitude (unit quaternion) - 4 elements - * Accelerometer bias (x, y, z) - 3 elements - * Gyroscope bias (x, y, z) - 3 elements - - Defaults to a zero vector, but with the attitude part as a unit quaternion - (i.e., no rotation). - P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`) - Initial (a priori) estimate of the error covariance matrix, **P**. - err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2` - Dictionary containing accelerometer noise parameters with keys: - - * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz). - * ``B``: Bias stability in m/s^2. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2` - Dictionary containing gyroscope noise parameters with keys: - - * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz). - * ``B``: Bias stability in rad/s. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - g : float, default 9.80665 - The gravitational acceleration. Default is 'standard gravity' of 9.80665. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. Furthermore, the aiding heading angle is - also interpreted relative to this frame according to the right-hand rule. - cold_start : bool, default True - Whether to start the AINS filter in a 'cold' (default) or 'warm' state. - A cold state indicates that the provided initial conditions are uncertain, - and possibly far from the true state. Thus, to reduce the risk of divergence, - an initial vertical alignment (i.e., roll and pitch calibration) is performed - using accelerometer measurements and the known direction of gravity during - the first measurement update. The IMU should remain stationary with negligible - linear acceleration during a cold start; otherwise, divergence may occur. - A warm start, on the other hand, assumes accurate initial conditions, and - initializes the Kalman filter immediately without any initial roll and pitch - calibration. - **kwargs : - Ignored. For compatibility with parent class. - """ - - def __init__( - self, - fs: float, - x0_prior: ArrayLike = X0, - P0_prior: ArrayLike = P0, - err_acc: dict[str, float] = ERR_ACC_MOTION2, - err_gyro: dict[str, float] = ERR_GYRO_MOTION2, - g: float = 9.80665, - nav_frame: str = "NED", - cold_start: bool = True, - **kwargs: dict[str, Any], - ) -> None: - super().__init__( - fs=fs, - x0_prior=x0_prior, - P0_prior=P0_prior, - err_acc=err_acc, - err_gyro=err_gyro, - g=g, - nav_frame=nav_frame, - lever_arm=np.zeros(3), - ignore_bias_acc=True, - cold_start=cold_start, - ) - - def update( - self, - f_imu: ArrayLike, - w_imu: ArrayLike, - degrees: bool = False, - pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]), - vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]), - ) -> Self: - """ - Update/correct the VRU's state estimate with pseudo aiding measurements - (i.e., zero velocity and zero position with corresponding variances), and - project ahead using IMU measurements. - - Parameters - ---------- - f_imu : array-like, shape (3,) - Specific force measurements (i.e., accelerations + gravity), given - as [f_x, f_y, f_z]^T where f_x, f_y and f_z are - acceleration measurements in x-, y-, and z-direction, respectively. - w_imu : array-like, shape (3,) - Angular rate measurements, given as [w_x, w_y, w_z]^T where - w_x, w_y and w_z are angular rates about the x-, y-, - and z-axis, respectively. - degrees : bool, default False - Specifies whether the unit of ``w_imu`` are in degrees or radians. - pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6] - Variance of position measurement noise in m^2. Defaults to - standard deviation of 1000 m, while assuming zero position. - vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] - Variance of velocity measurement noise in (m/s)^2. Defaults to - standard deviation of 10 m/s, while assuming zero velocity. - - Returns - ------- - VRU - A reference to the instance itself after the update. - """ - return super().update( - f_imu, - w_imu, - degrees=degrees, - pos=np.array([0.0, 0.0, 0.0]), - pos_var=pos_var, - vel=np.array([0.0, 0.0, 0.0]), - vel_var=vel_var, - head=None, - head_var=None, - ) - - -class AHRS(AidedINS): - """ - Attitude and Heading Reference System (AHRS) based on a multiplicative extended - Kalman filter (MEKF). - - AHRS is intended for applicatoins with negligble sustained linear accelerations. - For applications with sustained linear accelerations, accurate position and/or - velocity aiding is required. :class:`smsfusion.AidedINS` is recommended for - those cases. - - This class inherits from :class:`smsfusion.AidedINS` but applies sensible - defaults for attitude heading reference applications and simplifies the - interface by hiding non-essential configuration options. - - Velocity aiding is set to zero with a default standard deviation of 10 m/s. - Position aiding also assumes zero values but with high uncertainty (default - standard deviation of 1000 m), making it effectively non-constraining. - - Parameters - ---------- - fs : float - Sampling rate in Hz. - x0_prior : array-like, shape (16,), default :const:`smsfusion.constants.X0` - Initial (a priori) 16-element INS state estimate: - - * Position (x, y, z) - 3 elements - * Velocity (x, y, z) - 3 elements - * Attitude (unit quaternion) - 4 elements - * Accelerometer bias (x, y, z) - 3 elements - * Gyroscope bias (x, y, z) - 3 elements - - Defaults to a zero vector, but with the attitude part as a unit quaternion - (i.e., no rotation). - P0_prior : array-like, shape (12, 12), default np.eye(12) * 1e-6 (:const:`smsfusion.constants.P0`) - Initial (a priori) estimate of the error covariance matrix, **P**. - err_acc : dict of {str: float}, default :const:`smsfusion.constants.ERR_ACC_MOTION2` - Dictionary containing accelerometer noise parameters with keys: - - * ``N``: White noise power spectral density in (m/s^2)/sqrt(Hz). - * ``B``: Bias stability in m/s^2. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - err_gyro : dict of {str: float}, default :const:`smsfusion.constants.ERR_GYRO_MOTION2` - Dictionary containing gyroscope noise parameters with keys: - - * ``N``: White noise power spectral density in (rad/s)/sqrt(Hz). - * ``B``: Bias stability in rad/s. - * ``tau_cb``: Bias correlation time in seconds. - - Defaults to error characteristics of SMS Motion gen. 2. - g : float, default 9.80665 - The gravitational acceleration. Default is 'standard gravity' of 9.80665. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. Furthermore, the aiding heading angle is - also interpreted relative to this frame according to the right-hand rule. - cold_start : bool, default True - Whether to start the AINS filter in a 'cold' (default) or 'warm' state. - A cold state indicates that the provided initial conditions are uncertain, - and possibly far from the true state. Thus, to reduce the risk of divergence, - an initial vertical alignment (i.e., roll and pitch calibration) is performed - using accelerometer measurements and the known direction of gravity during - the first measurement update. The IMU should remain stationary with negligible - linear acceleration during a cold start; otherwise, divergence may occur. - A warm start, on the other hand, assumes accurate initial conditions, and - initializes the Kalman filter immediately without any initial roll and pitch - calibration. - **kwargs : - Ignored. For compatibility with parent class. - """ - - def __init__( - self, - fs: float, - x0_prior: ArrayLike = X0, - P0_prior: ArrayLike = P0, - err_acc: dict[str, float] = ERR_ACC_MOTION2, - err_gyro: dict[str, float] = ERR_GYRO_MOTION2, - g: float = 9.80665, - nav_frame: str = "NED", - cold_start: bool = True, - **kwargs: dict[str, Any], - ) -> None: - super().__init__( - fs=fs, - x0_prior=x0_prior, - P0_prior=P0_prior, - err_acc=err_acc, - err_gyro=err_gyro, - g=g, - nav_frame=nav_frame, - lever_arm=np.zeros(3), - ignore_bias_acc=True, - cold_start=cold_start, - ) - - def update( - self, - f_imu: ArrayLike, - w_imu: ArrayLike, - degrees: bool = False, - head: float | None = None, - head_var: float | None = None, - head_degrees: bool = True, - pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]), - vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]), - ) -> Self: - """ - Update/correct the AHRS' state estimate with pseudo aiding measurements - (i.e., zero velocity and zero position with corresponding variances), and - project ahead using IMU measurements. - - Parameters - ---------- - f_imu : array-like, shape (3,) - Specific force measurements (i.e., accelerations + gravity), given - as [f_x, f_y, f_z]^T where f_x, f_y and f_z are - acceleration measurements in x-, y-, and z-direction, respectively. - w_imu : array-like, shape (3,) - Angular rate measurements, given as [w_x, w_y, w_z]^T where - w_x, w_y and w_z are angular rates about the x-, y-, - and z-axis, respectively. - degrees : bool, default False - Specifies whether the unit of ``w_imu`` are in degrees or radians. - head : float, optional - Heading measurement. I.e., the yaw angle of the 'body' frame relative to the - assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. - If ``None``, compass aiding is not used. See ``head_degrees`` for units. - head_var : float, optional - Variance of heading measurement noise. Units must be compatible with ``head``. - See ``head_degrees`` for units. Required for ``head``. - head_degrees : bool, default False - Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, - or radians and radians^2. Default is in radians and radians^2. - pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6] - Variance of position measurement noise in m^2. Defaults to - standard deviation of 1000 m, while assuming zero position. - vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] - Variance of velocity measurement noise in (m/s)^2. Defaults to - standard deviation of 10 m/s, while assuming zero velocity. - - Returns - ------- - AHRS - A reference to the instance itself after the update. - """ - return super().update( - f_imu, - w_imu, - degrees=degrees, - pos=np.array([0.0, 0.0, 0.0]), - pos_var=pos_var, - vel=np.array([0.0, 0.0, 0.0]), - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=head_degrees, - ) diff --git a/src/smsfusion/_smoothing.py b/src/smsfusion/_smoothing.py deleted file mode 100644 index 76234d61..00000000 --- a/src/smsfusion/_smoothing.py +++ /dev/null @@ -1,313 +0,0 @@ -from typing import Self -from warnings import warn - -import numpy as np -from numba import njit -from numpy.typing import NDArray - -from ._ins import AHRS, VRU, AidedINS -from ._transforms import _euler_from_quaternion -from ._vectorops import _normalize, _quaternion_product - - -class FixedIntervalSmoother: - """ - Fixed-interval smoothing for AidedINS. - - This class wraps an instance of AidedINS (or a subclass like AHRS or VRU), - and maintains a time-ordered buffer of state and error covariance estimates - as measurements are processed via the ``update()`` method. A backward sweep - over the buffered data using the Rauch-Tung-Striebel (RTS) algorithm [1] is - performed to refine the filter estimates. - - Parameters - ---------- - ains : AidedINS or AHRS or VRU - The underlying AidedINS instance used for forward filtering. - cov_smoothing : bool, default True - Whether to include the error covariance matrix, `P`, in the smoothing process. - Disabling the covariance smoothing has no effect on the smoothed state estimates, - and can reduce computation time if smoothed covariances are not required. - - References - ---------- - [1] R. G. Brown and P. Y. C. Hwang, "Random signals and applied Kalman - filtering with MATLAB exercises", 4th ed. Wiley, pp. 208-212, 2012. - """ - - def __init__(self, ains: AidedINS | AHRS | VRU, cov_smoothing: bool = True) -> None: - warn( - "FixedIntervalSmoother is experimental and may change or be removed in the future.", - UserWarning, - ) - self._ains = ains - self._cov_smoothing = cov_smoothing - - # Buffers for storing state and covariance estimates from forward sweep - self._x_buf = [] # state estimates (w/o smoothing) - self._P_buf = [] # error covariance estimates (w/o smoothing) - self._dx_buf = [] # error-state estimates (w/o smoothing) - self._P_prior_buf = [] # a priori error covariance estimates (w/o smoothing) - self._phi_buf = [] # state transition matrix - - # Smoothed state and covariance estimates - self._x = np.empty((0, 16), dtype="float64") - self._P = np.empty((0, *self._ains.P.shape), dtype="float64") - - @property - def ains(self) -> AidedINS | AHRS | VRU: - """ - The underlying AidedINS instance used for forward filtering. - - Returns - ------- - AidedINS or AHRS or VRU - The AidedINS instance. - """ - return self._ains - - def update(self, *args, **kwargs) -> Self: - """ - Update the AINS with measurements, and append the current AINS state to - the smoother's internal buffer. - - Parameters - ---------- - *args : tuple - Positional arguments to be passed on to ``ains.update()``. - **kwargs : dict - Keyword arguments to be passed on to ``ains.update()``. - """ - self._P_prior_buf.append(self._ains.P_prior) - self._ains.update(*args, **kwargs) - self._x_buf.append(self._ains.x) - self._P_buf.append(self._ains.P) - self._dx_buf.append(self._ains._dx.copy()) - self._phi_buf.append(self._ains._phi.copy()) - return self - - def clear(self) -> None: - """ - Clear the internal buffer of state estimates. This resets the smoother, - and prepares for a new interval of measurements. - """ - self._x_buf.clear() - self._dx_buf.clear() - self._P_buf.clear() - self._P_prior_buf.clear() - self._phi_buf.clear() - - def _smooth(self): - n_samples = len(self._x_buf) - if n_samples == 0: - self._x = np.empty((0, 16), dtype="float64") - self._P = np.empty((0, *self._ains.P.shape), dtype="float64") - elif n_samples == 1: - self._x = np.asarray(self._x_buf) - self._P = np.asarray(self._P_buf) - elif n_samples != len(self._x): - x, P = _rts_backward_sweep( - self._x_buf, - self._dx_buf, - self._P_buf, - self._P_prior_buf, - self._phi_buf, - self._cov_smoothing, - ) - self._x = np.asarray(x) - self._P = np.asarray(P) - - @property - def x(self) -> NDArray: - """ - Smoothed state vector estimates. - - Returns - ------- - np.ndarray, shape (N, 15) or (N, 12) - State estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - self._smooth() - return self._x.copy() - - @property - def P(self) -> NDArray: - """ - Error covariance matrix estimates. - - If ``cov_smoothing=True``, smoothed error covariance estimates are returned. - Otherwise, the forward filter covariance estimates are returned. - - Returns - ------- - np.ndarray, shape (N, 15, 15) or (N, 12, 12) - Error covariance matrix estimates for each of the N time steps where - the smoother has been updated with measurements. - """ - self._smooth() - return self._P.copy() - - def position(self) -> NDArray: - """ - Smoothed position estimates. - - Returns - ------- - np.ndarray, shape (N, 3) - Position estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - x = self.x - if x.size == 0: - return np.empty((0, 3), dtype="float64") - return self.x[:, :3] - - def velocity(self) -> NDArray: - """ - Smoothed velocity estimates. - - Returns - ------- - np.ndarray, shape (N, 3) - Velocity estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - x = self.x - if x.size == 0: - return np.empty((0, 3), dtype="float64") - return self.x[:, 3:6] - - def quaternion(self) -> NDArray: - """ - Smoothed unit quaternion estimates. - - Returns - ------- - np.ndarray, shape (N, 4) - Unit quaternion estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - x = self.x - if x.size == 0: - return np.empty((0, 4), dtype="float64") - return self.x[:, 6:10] - - def bias_acc(self) -> NDArray: - """ - Smoothed accelerometer bias estimates. - - Returns - ------- - np.ndarray, shape (N, 3) - Accelerometer bias estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - x = self.x - if x.size == 0: - return np.empty((0, 3), dtype="float64") - return x[:, 10:13] - - def bias_gyro(self, degrees: bool = False) -> NDArray: - """ - Smoothed gyroscope bias estimates. - - Returns - ------- - np.ndarray, shape (N, 3) - Gyroscope bias estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - x = self.x - if x.size == 0: - return np.empty((0, 3), dtype="float64") - - bg = self.x[:, 13:16] - return np.degrees(bg) if degrees else bg - - def euler(self, degrees: bool = False) -> NDArray: - """ - Smoothed Euler angles estimates. - - Returns - ------- - np.ndarray, shape (N, 3) - Euler angles estimates for each of the N time steps where the smoother has - been updated with measurements. - """ - q = self.quaternion() - if q.size == 0: - return np.empty((0, 3), dtype="float64") - - theta = np.array([_euler_from_quaternion(q_i) for q_i in q]) - return np.degrees(theta) if degrees else theta - - -@njit # type: ignore[misc] -def _rts_backward_sweep( - x: list[NDArray], - dx: list[NDArray], - P: list[NDArray], - P_prior: list[NDArray], - phi: list[NDArray], - cov_smoothing: bool, -) -> tuple[list[NDArray], list[NDArray]]: - """ - Perform a backward sweep with the RTS algorithm [1]. - - Parameters - ---------- - x : NDArray, shape (n_samples, 16) - The state vector. - dx : NDArray, shape (n_samples, 15) or (n_samples, 12) - The error state vector. - P : NDArray, shape (n_samples, 15, 15) or (n_samples, 12, 12) - The covariance matrix. - P_prior : NDArray, shape (n_samples, 15, 15) or (n_samples, 12, 12) - The a priori covariance matrix. - phi : NDArray, shape (n_samples, 15, 15) or (n_samples, 12, 12) - The state transition matrix. - cov_smoothing : bool - Whether to include the error covariance matrix in the smoothing process. - - Returns - ------- - x_smth : NDArray, shape (n_samples, 15) or (n_samples, 12) - The smoothed state vector. - P_smth : NDArray, shape (n_samples, 15, 15) or (n_samples, 12, 12) - The smoothed covariance matrix if include_cov is True, otherwise None. - - References - ---------- - [1] R. G. Brown and P. Y. C. Hwang, "Random signals and applied Kalman - filtering with MATLAB exercises", 4th ed. Wiley, pp. 208-212, 2012. - """ - - x = x.copy() - dx = dx.copy() - P = P.copy() - - q_prealloc = np.array([2.0, 0.0, 0.0, 0.0]) # Preallocation - - # Backward sweep - for k in range(len(x) - 2, -1, -1): - # Smoothed error-state estimate and corresponding covariance - A = P[k] @ phi[k].T @ np.linalg.inv(P_prior[k + 1]) - ddx = A @ dx[k + 1] - dx[k] += ddx - if cov_smoothing: - P[k] += A @ (P[k + 1] - P_prior[k + 1]) @ A.T - - # Reset - dda = ddx[6:9] - q_prealloc[1:] = dda - ddq = (1.0 / np.sqrt(4.0 + dda.T @ dda)) * q_prealloc - x[k][:3] = x[k][:3] + ddx[:3] - x[k][3:6] = x[k][3:6] + ddx[3:6] - x[k][6:10] = _quaternion_product(x[k][6:10], ddq) - x[k][6:10] = _normalize(x[k][6:10]) - x[k][-3:] = x[k][-3:] + ddx[-3:] - if dx[k].size == 15: - x[k][10:13] = x[k][10:13] + ddx[9:12] - - return x, P diff --git a/tests/test_ins_legacy.py b/tests/test_ins_legacy.py deleted file mode 100644 index 920faa19..00000000 --- a/tests/test_ins_legacy.py +++ /dev/null @@ -1,2070 +0,0 @@ -""" -IMPORTANT ---------- - -SciPy Rotation implementation is used as reference in tests. However, SciPy -operates with active rotations, whereas passive rotations are considered here. Keep in -mind that passive rotations is simply the inverse active rotations and vice versa. -""" - -import json -from pathlib import Path - -import numpy as np -import pytest -from pandas import read_parquet -from pytest import approx -from scipy.signal import resample_poly -from scipy.spatial.transform import Rotation - -import smsfusion as sf -from smsfusion._ins import ( - AHRS, - VRU, - AidedINS, - FixedNED, - StrapdownINS, - gravity, -) -from smsfusion._ins._ains_legacy import INSMixin, _roll_pitch_from_acc -from smsfusion._ins._common import ( - _signed_smallest_angle, - _yaw_from_quaternion, - _yaw_gradient, -) -from smsfusion._transforms import ( - _angular_matrix_from_quaternion, - _rot_matrix_from_quaternion, - quaternion_from_euler, -) -from smsfusion._vectorops import _normalize, _quaternion_product, _skew_symmetric -from smsfusion.benchmark import ( - benchmark_full_pva_beat_202311A, - benchmark_full_pva_chirp_202311A, -) -from smsfusion.constants import ERR_ACC_MOTION2, ERR_GYRO_MOTION2, P0, X0 -from smsfusion.noise import IMUNoise, white_noise - - -@pytest.mark.parametrize( - "euler", - [ - np.radians([10.0, 45.0, 0.0]), - np.radians([0.0, 0.0, 0.0]), - np.radians([90.0, 0.0, 0.0]), - np.radians([180.0, 0.0, 0.0]), - np.radians([130.0, -28.0, 90.0]), - ], -) -def test__roll_pitch_from_acc(euler): - R_nm = _rot_matrix_from_quaternion(quaternion_from_euler(euler)) # body-to-nav - g = gravity() - - # North-East-Down (NED) frame - g_ned = np.array([0.0, 0.0, -g]) - acc_ned = R_nm.T @ g_ned - roll_pitch_ned = _roll_pitch_from_acc(acc_ned, nav_frame="NED") - np.testing.assert_allclose(roll_pitch_ned, euler[:2]) - - # North-East-Up (ENU) frame - g_enu = np.array([0.0, 0.0, g]) - acc_enu = R_nm.T @ g_enu - roll_pitch_enu = _roll_pitch_from_acc(acc_enu, nav_frame="ENU") - np.testing.assert_allclose(roll_pitch_enu, euler[:2]) - - -@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 _signed_smallest_angle(angle, degrees=degrees) == pytest.approx(angle_expect) - - -@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 = gravity(mu) - assert g_out == pytest.approx(g_expect) - - -@pytest.fixture -def ains_ref_data(): - """Reference data for AINS testing.""" - return read_parquet( - Path(__file__).parent / "testdata" / "ains_ahrs_imu.parquet", engine="pyarrow" - ) - - -@pytest.mark.filterwarnings("ignore") -class Test_INSMixin: - @pytest.fixture - def x(self): - p = np.array([1.0, 2.0, 3.0]) - v = np.array([4.0, 5.0, 6.0]) - q = np.array([1.0, 0.0, 0.0, 0.0]) - ba = np.array([7.0, 8.0, 9.0]) - bg = np.array([10.0, 11.0, 12.0]) - x = np.r_[p, v, q, ba, bg] - return x - - @pytest.fixture - def ins(self, x): - class INS(INSMixin): - def __init__(self, x): - self._x = x - - return INS(x) - - def test_x(self, x, ins): - x_out = ins.x - x_expect = x - assert x_out.shape == (16,) - assert x_out is not ins._x - np.testing.assert_array_equal(x_out, x_expect) - - def test_position(self, x, ins): - p_out = ins.position() - p_expect = np.array([1.0, 2.0, 3.0]) - assert p_out.shape == (3,) - assert p_out is not ins._pos - np.testing.assert_array_equal(p_out, p_expect) - - def test_velocity(self, x, ins): - v_out = ins.velocity() - v_expect = np.array([4.0, 5.0, 6.0]) - assert v_out.shape == (3,) - assert v_out is not ins._vel - np.testing.assert_array_equal(v_out, v_expect) - - def test_quaternion(self, x, ins): - q_out = ins.quaternion() - q_expect = np.array([1.0, 0.0, 0.0, 0.0]) - assert q_out.shape == (4,) - assert q_out is not ins._q_nm - np.testing.assert_array_equal(q_out, q_expect) - - def test_euler(self, x, ins): - theta_out = ins.euler() - theta_expect = np.array([0.0, 0.0, 0.0]) - assert theta_out.shape == (3,) - np.testing.assert_array_equal(theta_out, theta_expect) - - def test_bias_acc(self, x, ins): - ba_out = ins.bias_acc() - ba_expect = np.array([7.0, 8.0, 9.0]) - assert ba_out.shape == (3,) - assert ba_out is not ins._bias_acc - np.testing.assert_array_equal(ba_out, ba_expect) - - def test_bias_gyro(self, x, ins): - bg_out = ins.bias_gyro() - bg_expect = np.array([10.0, 11.0, 12.0]) - assert bg_out.shape == (3,) - assert bg_out is not ins._bias_gyro - np.testing.assert_array_equal(bg_out, bg_expect) - - -@pytest.mark.filterwarnings("ignore") -class Test_StrapdownINS: - @pytest.fixture - def x0(self): - p0 = np.array([0.0, 0.0, 0.0]) - v0 = np.array([0.0, 0.0, 0.0]) - q0 = np.array([1.0, 0.0, 0.0, 0.0]) - ba0 = np.array([0.0, 0.0, 0.0]) - bg0 = np.array([0.0, 0.0, 0.0]) - x0 = np.r_[p0, v0, q0, ba0, bg0] - return x0 - - @pytest.fixture - def ins(self, x0): - fs = 10.0 - ins = StrapdownINS(fs, x0) - return ins - - @pytest.fixture - def x0_nonzero(self): - p0 = np.array([1.0, 2.0, 3.0]) - v0 = np.array([4.0, 5.0, 6.0]) - q0 = np.array([1.0, 0.0, 0.0, 0.0]) - ba0 = np.array([7.0, 8.0, 9.0]) - bg0 = np.array([10.0, 11.0, 12.0]) - x0 = np.r_[p0, v0, q0, ba0, bg0] - return x0 - - def test__init__(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero, g=9.81, nav_frame="NED") - - assert isinstance(ins, INSMixin) - assert ins._fs == 10.24 - assert ins._g == approx(9.81) - assert ins._nav_frame == "ned" - np.testing.assert_array_equal(ins._g_n, [0.0, 0.0, 9.81]) - np.testing.assert_array_equal(ins._x0, x0_nonzero) - np.testing.assert_array_equal(ins._x, x0_nonzero) - - def test__init__enu(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero, g=9.81, nav_frame="ENU") - assert ins._nav_frame == "enu" - np.testing.assert_array_equal(ins._g_n, [0.0, 0.0, -9.81]) - - def test_x(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - x_out = ins.x - x_expect = x0_nonzero - - assert x_out.shape == (16,) - assert x_out is not ins._x - np.testing.assert_array_equal(x_out, x_expect) - - def test_position(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - p_out = ins.position() - p_expect = np.array([1.0, 2.0, 3.0]) - - assert p_out.shape == (3,) - assert p_out is not ins._pos - np.testing.assert_array_equal(p_out, p_expect) - - def test_velocity(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - v_out = ins.velocity() - v_expect = np.array([4.0, 5.0, 6.0]) - - assert v_out.shape == (3,) - assert v_out is not ins._vel - np.testing.assert_array_equal(v_out, v_expect) - - def test_quaternion(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - q_out = ins.quaternion() - q_expect = np.array([1.0, 0.0, 0.0, 0.0]) - - assert q_out.shape == (4,) - assert q_out is not ins._q_nm - np.testing.assert_array_equal(q_out, q_expect) - - def test_euler(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - theta_out = ins.euler() - theta_expect = np.array([0.0, 0.0, 0.0]) - - assert theta_out.shape == (3,) - np.testing.assert_array_equal(theta_out, theta_expect) - - def test_bias_acc(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - ba_out = ins.bias_acc() - ba_expect = np.array([7.0, 8.0, 9.0]) - - assert ba_out.shape == (3,) - assert ba_out is not ins._vel - np.testing.assert_array_equal(ba_out, ba_expect) - - def test_bias_gyro(self, x0_nonzero): - ins = StrapdownINS(10.24, x0_nonzero) - - bg_out = ins.bias_gyro() - bg_expect = np.array([10.0, 11.0, 12.0]) - - assert bg_out.shape == (3,) - assert bg_out is not ins._vel - np.testing.assert_array_equal(bg_out, bg_expect) - - def test_reset(self, ins): - x = np.random.random(16) - x[6:10] = x[6:10] / np.linalg.norm(x[6:10]) # unit quaternion - ins.reset(x) - - np.testing.assert_allclose(ins.x, x) - - def test_reset_2d(self, ins): - x = np.random.random(16).reshape(-1, 1) - x[6:10] = x[6:10] / np.linalg.norm(x[6:10]) # unit quaternion - ins.reset(x) - - np.testing.assert_allclose(ins.x, x.flatten()) - - def test_update_return_self(self, ins): - g = 9.80665 - f = np.array([0.0, 0.0, -g]) - w = np.array([0.0, 0.0, 0.0]) - - update_return = ins.update(f, w) - assert update_return is ins - - def test_update_ned(self, x0): - - ins = StrapdownINS(10.0, x0, g=9.81, nav_frame="NED") - - g_n = np.array([0.0, 0.0, 9.81]) - f = np.array([1.0, 2.0, 3.0]) - g_n - w = np.array([0.04, 0.05, 0.06]) - - x0_out = ins.x - ins.update(f, w) - x1_out = ins.x - - x0_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - x1_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.99999, - 0.002, - 0.0025, - 0.003, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - - np.testing.assert_allclose(x0_out, x0_expect, atol=1e-6) - np.testing.assert_allclose(x1_out, x1_expect, atol=1e-6) - - def test_update_enu(self, x0): - - ins = StrapdownINS(10.0, x0, g=9.81, nav_frame="ENU") - - g_n = np.array([0.0, 0.0, -9.81]) - f = np.array([1.0, 2.0, 3.0]) - g_n - w = np.array([0.04, 0.05, 0.06]) - - x0_out = ins.x - ins.update(f, w) - x1_out = ins.x - - x0_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - x1_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.99999, - 0.002, - 0.0025, - 0.003, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - - np.testing.assert_allclose(x0_out, x0_expect, atol=1e-6) - np.testing.assert_allclose(x1_out, x1_expect, atol=1e-6) - - def test_update_deg(self, ins): - g_n = ins._g_n - f_imu = np.array([1.0, 2.0, 3.0]) - g_n - w_imu = np.array([4.0, 5.0, 6.0]) - - x0_out = ins.x - ins.update(f_imu, w_imu, degrees=True) - x1_out = ins.x - - x0_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - x1_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.999971, - 0.003491, - 0.004363, - 0.005236, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - - np.testing.assert_allclose(x0_out, x0_expect, atol=1e-6) - np.testing.assert_allclose(x1_out, x1_expect, atol=1e-6) - - def test_update_with_bias(self, x0): - ba = np.array([0.1, 0.2, 0.3]) - bg = np.array([0.4, 0.5, 0.6]) - x0[10:13] = ba - x0[13:16] = bg - ins = StrapdownINS(10.0, x0) - g_n = ins._g_n - f = np.array([1.0, 2.0, 3.0]) + ba - g_n # IMU measurements w/bias - w = np.array([0.04, 0.05, 0.06]) + bg # IMU measurements w/bias - - x0_out = ins.x - ins.update(f, w, degrees=False) - x1_out = ins.x - - x0_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.6, - ] - ) - x1_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.99999, - 0.002, - 0.0025, - 0.003, - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.6, - ] - ) - - np.testing.assert_allclose(x0_out, x0_expect, atol=1e-6) - np.testing.assert_allclose(x1_out, x1_expect, atol=1e-6) - - def test_update_twise(self, ins): - g_n = ins._g_n - f_imu = np.array([1.0, 2.0, 3.0]) - g_n - w_imu = np.array([0.004, 0.005, 0.006]) - - x0_out = ins.x - ins.update(f_imu, w_imu, degrees=False) - x1_out = ins.x - ins.update(f_imu, w_imu, degrees=False) - x2_out = ins.x - - x0_expect = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - ] - ) - - dt = 1.0 / ins._fs - # Calculate x1 - R0_expect = np.eye(3) - T0_expect = _angular_matrix_from_quaternion(x0_expect[6:10]) - a0_expect = R0_expect @ f_imu + g_n - x1_expect = np.zeros(16) - x1_expect[0:3] = x0_expect[0:3] + dt * x0_expect[3:6] - x1_expect[3:6] = x0_expect[3:6] + dt * a0_expect - x1_expect[6:10] = x0_expect[6:10] + dt * T0_expect @ w_imu - x1_expect[6:10] = x1_expect[6:10] / np.linalg.norm(x1_expect[6:10]) - - # Calculate x2 by forward Euler - R1_expect = _rot_matrix_from_quaternion(x1_expect[6:10]) - T1_expect = _angular_matrix_from_quaternion(x1_expect[6:10]) - a1_expect = R1_expect @ f_imu + g_n - x2_expect = np.zeros(16) - x2_expect[0:3] = x1_expect[0:3] + dt * x1_expect[3:6] - x2_expect[3:6] = x1_expect[3:6] + dt * a1_expect - x2_expect[6:10] = x1_expect[6:10] + dt * T1_expect @ w_imu - x2_expect[6:10] = x2_expect[6:10] / np.linalg.norm(x2_expect[6:10]) - - np.testing.assert_allclose(x0_out, x0_expect.flatten(), atol=1e-8) - np.testing.assert_allclose(x1_out, x1_expect.flatten(), atol=1e-8) - np.testing.assert_allclose(x2_out, x2_expect.flatten(), atol=1e-8) - - -@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__yaw_from_quaternion(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 = _yaw_from_quaternion(quaternion) - assert gamma_expect == pytest.approx(gamma) - - -@pytest.mark.parametrize( - "quaternion, dhda_expect", - [ - ( - 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 = _yaw_gradient(quaternion) - np.testing.assert_allclose(dhda_out, dhda_expect) - - -class Test_AidedINS: - @staticmethod - def quaternion(alpha=-10.0, beta=5.0, gamma=25.0, degrees=True): - """ - Convert Euler to quaternions using SciPy. - """ - q = Rotation.from_euler("ZYX", (gamma, beta, alpha), degrees=degrees).as_quat() - q = np.r_[q[3], q[:3]] - return q - - @staticmethod - def rot_matrix_from_quaternion(q): - """ - Convert quaternion to rotation matrix using SciPy. - """ - q = np.r_[q[1:], q[0]] - return Rotation.from_quat(q).as_matrix() - - @pytest.fixture - def ains(self): - fs = 10.24 - - p_init = np.array([0.1, 0.0, 0.0]) - v_init = np.array([0.0, -0.1, 0.0]) - - q_init = self.quaternion() - - bias_acc_init = np.array([0.0, 0.0, 0.1]) - bias_gyro_init = np.array([-0.1, 0.0, 0.0]) - - x0 = np.r_[p_init, v_init, q_init, bias_acc_init, bias_gyro_init] - P0_prior = 1e-6 * np.eye(15) - - err_acc = {"N": 4.0e-4, "B": 2.0e-4, "tau_cb": 50} - err_gyro = { - "N": (np.pi) / 180.0 * 2.0e-3, - "B": (np.pi) / 180.0 * 8.0e-4, - "tau_cb": 50, - } - - ains = AidedINS( - fs, - x0, - P0_prior, - err_acc, - err_gyro, - lever_arm=np.ones(3), - ignore_bias_acc=False, - nav_frame="NED", - cold_start=False, - ) - return ains - - @pytest.fixture - def ains_nobias(self): - fs = 10.24 - - p_init = np.array([0.1, 0.0, 0.0]) - v_init = np.array([0.0, -0.1, 0.0]) - - q_init = self.quaternion() - - bias_acc_init = np.array([0.0, 0.0, 0.1]) - bias_gyro_init = np.array([-0.1, 0.0, 0.0]) - - x0 = np.r_[p_init, v_init, q_init, bias_acc_init, bias_gyro_init] - P0_prior = 1e-6 * np.eye(12) - - err_acc = {"N": 4.0e-4, "B": 2.0e-4, "tau_cb": 50} - err_gyro = { - "N": (np.pi) / 180.0 * 2.0e-3, - "B": (np.pi) / 180.0 * 8.0e-4, - "tau_cb": 50, - } - - ains = AidedINS( - fs, - x0, - P0_prior, - err_acc, - err_gyro, - lever_arm=np.ones(3), - ignore_bias_acc=True, - nav_frame="NED", - cold_start=False, - ) - return ains - - def test__init__(self): - fs = 10.24 - - p_init = np.array([0.0, 0.0, 0.0]) - v_init = np.array([0.0, 0.0, 0.0]) - q_init = np.array([1.0, 0.0, 0.0, 0.0]) - bias_acc_init = np.array([0.0, 0.0, 0.0]) - bias_gyro_init = np.array([0.0, 0.0, 0.0]) - - x0 = np.r_[p_init, v_init, q_init, bias_acc_init, bias_gyro_init] - P0_prior = 1e-6 * np.eye(15) - - err_acc = {"N": 4.0e-4, "B": 2.0e-4, "tau_cb": 50} - err_gyro = { - "N": (np.pi) / 180.0 * 2.0e-3, - "B": (np.pi) / 180.0 * 8.0e-4, - "tau_cb": 50, - } - - ains = AidedINS( - fs, - x0, - P0_prior, - err_acc, - err_gyro, - lever_arm=(1, 2, 3), - g=9.81, - ignore_bias_acc=False, - cold_start=False, - ) - - assert isinstance(ains, AidedINS) - assert isinstance(ains, INSMixin) - assert ains._fs == 10.24 - assert ains._dt == 1.0 / 10.24 - assert ains._err_acc == err_acc - assert ains._err_gyro == err_gyro - assert isinstance(ains._ins, StrapdownINS) - assert ains._ignore_bias_acc is False - assert ains._cold is False - - np.testing.assert_allclose(ains._x, x0) - np.testing.assert_allclose(ains._ins._x, x0) - np.testing.assert_allclose(ains._P_prior, P0_prior) - np.testing.assert_allclose(ains._lever_arm, (1, 2, 3)) - - assert ains._P.shape == (15, 15) - - # Check that correct latitude (and thus gravity) is used - g_expect = np.array([0.0, 0.0, 9.81]) - np.testing.assert_allclose(ains._ins._g_n, g_expect) - - assert ains._F.shape == (15, 15) - assert ains._G.shape == (15, 12) - assert ains._W.shape == (12, 12) - assert ains._H.shape == (10, 15) - - def test__init__without_x0_P0_err(self): - # Test initialization without x0_prior, P0_prior and err_acc/err_gyro - ains = AidedINS(10.24) - - assert ains._err_acc == ERR_ACC_MOTION2 - assert ains._err_gyro == ERR_GYRO_MOTION2 - - np.testing.assert_allclose(ains._ins._x, X0) - np.testing.assert_allclose(ains._P_prior, P0) - assert ains._P.shape == (12, 12) - assert ains._F.shape == (12, 12) - assert ains._G.shape == (12, 9) - assert ains._W.shape == (9, 9) - assert ains._H.shape == (10, 12) - - @pytest.mark.parametrize("ignore_bias_acc", [True, False]) - def test__init__raises_P0_shape(self, ignore_bias_acc): - # Test initialization with invalid P0_prior - x0 = np.zeros(16) - x0[6] = 1.0 - if ignore_bias_acc: - P0_prior = np.eye(15) * 1e-6 - else: - P0_prior = P0 - with pytest.raises(ValueError): - AidedINS(10.24, x0, P0_prior, ignore_bias_acc=ignore_bias_acc) - - def test__init__nav_frame(self): - - g = gravity() - - # ENU - ains_enu = AidedINS(10.24, g=g, nav_frame="ENU") - assert ains_enu._ins._nav_frame == "enu" - np.testing.assert_allclose(ains_enu._ins._g_n, [0.0, 0.0, -g]) - np.testing.assert_allclose(ains_enu._vg_ref_n, [0.0, 0.0, -1.0]) - - # NED - ains_ned = AidedINS(10.24, nav_frame="NED") - assert ains_ned._ins._nav_frame == "ned" - np.testing.assert_allclose(ains_ned._ins._g_n, [0.0, 0.0, g]) - np.testing.assert_allclose(ains_ned._vg_ref_n, [0.0, 0.0, 1.0]) - - def test__init__ignore_bias_acc(self): - fs = 10.24 - - P0_prior = 1e-6 * np.eye(12) - ains = AidedINS(fs, P0_prior=P0_prior, ignore_bias_acc=True) - - assert ains._ignore_bias_acc is True - np.testing.assert_allclose(ains._P_prior, P0_prior) - assert ains._P_prior.shape == (12, 12) - assert ains._P.shape == (12, 12) - assert ains._F.shape == (12, 12) - assert ains._G.shape == (12, 9) - assert ains._W.shape == (9, 9) - assert ains._H.shape == (10, 12) - - def test__init__dont_ignore_bias_acc(self): - fs = 10.24 - - P0_prior = 1e-6 * np.eye(15) - ains = AidedINS(fs, P0_prior=P0_prior, ignore_bias_acc=False) - - assert ains._ignore_bias_acc is False - np.testing.assert_allclose(ains._P_prior, P0_prior) - assert ains._P_prior.shape == (15, 15) - assert ains._P.shape == (15, 15) - assert ains._F.shape == (15, 15) - assert ains._G.shape == (15, 12) - assert ains._W.shape == (12, 12) - assert ains._H.shape == (10, 15) - - def test__init__default_lever_arm(self): - ains = AidedINS(10.24) # use default lever arm - np.testing.assert_allclose(ains._lever_arm, np.zeros(3)) - - def test_dump(self, tmp_path, ains): - kwargs_out = ains.dump() - ains_b = AidedINS(**kwargs_out) - - with open(tmp_path / "ains.json", "w") as f: - json.dump(kwargs_out, f) - - assert isinstance(kwargs_out, dict) - assert ains_b._fs == ains._fs - assert ains_b._err_acc == ains._err_acc - assert ains_b._err_gyro == ains._err_gyro - np.testing.assert_allclose(ains_b._lever_arm, ains._lever_arm) - assert ains_b._ins._g == ains._ins._g - np.testing.assert_allclose(ains_b._ins._x, ains._ins._x) - np.testing.assert_allclose(ains_b._P_prior, ains._P_prior) - np.testing.assert_allclose(ains_b._ignore_bias_acc, ains._ignore_bias_acc) - assert ains_b._cold is ains._cold - - def test_x(self): - x = np.random.random(16) - x[6:10] = x[6:10] / np.linalg.norm(x[6:10]) # unit quaternion - ains = AidedINS(10.24, x0_prior=x) - - np.testing.assert_allclose(ains.x, x) - assert ains.x is not ains._x - - def test_position(self, ains): - x = np.random.random(16) - x[6:10] = x[6:10] / np.linalg.norm(x[6:10]) # unit quaternion - ains = AidedINS(10.24, x0_prior=x) - - pos_out = ains.position() - pos_expect = x[0:3] - - np.testing.assert_allclose(pos_out, pos_expect) - assert pos_out is not ains._pos - - def test_velocity(self, ains): - x = np.random.random(16) - x[6:10] = x[6:10] / np.linalg.norm(x[6:10]) # unit quaternion - ains = AidedINS(10.24, x0_prior=x) - - vel_out = ains.velocity() - vel_expect = x[3:6] - - np.testing.assert_allclose(vel_out, vel_expect) - assert vel_out is not ains._vel - - def test_euler(self): - - euler_deg = np.array([-10.0, 5.0, 25.0]) - euler_rad = np.radians(euler_deg) - q = quaternion_from_euler(euler_rad, degrees=False) - - x0 = np.zeros(16) - x0[6:10] = q - ains = AidedINS(10.24, x0_prior=x0) - - np.testing.assert_allclose(ains.euler(degrees=False), euler_rad) - np.testing.assert_allclose(ains.euler(degrees=True), euler_deg) - - def test_quaternion(self): - - q = np.random.random(4) - q /= np.linalg.norm(q) # normalize to unit quaternion - - x0 = np.zeros(16) - x0[6:10] = q - ains = AidedINS(10.24, x0_prior=x0) - - np.testing.assert_allclose(ains.quaternion(), q) - - def test__reset_ins(self, ains): - x_ins = np.array( - [ - 1.0, - 2.0, - 3.0, - 4.0, - 5.0, - 6.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.5, - ] - ) - dx = np.array( - [ - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.6, - 0.05, - 0.06, - 0.07, - 0.7, - 0.8, - 0.9, - 0.10, - 0.11, - 0.12, - ] - ) - - ains._ignore_bias_acc = False - ains._ins._x = x_ins.copy() - ains._reset_ins(dx) - x_out = ains._ins.x - - da = dx[6:9] - dq = (1.0 / np.sqrt(4.0 + da.T @ da)) * np.r_[2.0, da] - - x_expect = np.r_[ - x_ins[0:6] + dx[0:6], - _normalize(_quaternion_product(x_ins[6:10], dq)), - x_ins[10:13] + dx[9:12], - x_ins[13:16] + dx[12:15], - ] - - np.testing.assert_allclose(x_out, x_expect) - - def test__reset_ins_ignore_bias_acc(self, ains): - x_ins = np.array( - [ - 1.0, - 2.0, - 3.0, - 4.0, - 5.0, - 6.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.5, - ] - ) - dx = np.array( - [ - 0.1, - 0.2, - 0.3, - 0.4, - 0.5, - 0.6, - 0.05, - 0.06, - 0.07, - 0.7, - 0.8, - 0.9, - 0.10, - 0.11, - 0.12, - ] - ) - - ains._ignore_bias_acc = True - ains._ins._x = x_ins.copy() - ains._reset_ins(dx) - x_out = ains._ins.x - - da = dx[6:9] - dq = (1.0 / np.sqrt(4.0 + da.T @ da)) * np.r_[2.0, da] - - x_expect = np.r_[ - x_ins[0:6] + dx[0:6], - _normalize(_quaternion_product(x_ins[6:10], dq)), - x_ins[10:13], - x_ins[13:16] + dx[12:15], - ] - - np.testing.assert_allclose(x_out, x_expect) - - def test_x_prior(self, ains): - x_prior_out = ains.x_prior - x_prior_expect = ains._ins.x - np.testing.assert_allclose(x_prior_out, x_prior_expect) - - def test_P_prior(self, ains, ains_nobias): - - # With bias - P_prior_out = ains.P_prior - P_prior_expect = 1e-6 * np.eye(15) - np.testing.assert_allclose(P_prior_out, P_prior_expect) - assert P_prior_out is not ains._P_prior - - # Without bias - P_prior_out = ains_nobias.P_prior - P_prior_expect = 1e-6 * np.eye(12) - np.testing.assert_allclose(P_prior_out, P_prior_expect) - assert P_prior_out is not ains_nobias._P_prior - - def test_P(self, ains): - P = np.random.random((15, 15)) - ains._P = P - - # Permutation matrix for reordering bias terms - T = np.zeros((15, 15)) - T[:9, :9] = np.eye(9) - T[9:12, 12:15] = np.eye(3) - T[12:15, 9:12] = np.eye(3) - - P_out = ains.P - P_expect = P - - np.testing.assert_allclose(P_out, P_expect) - assert P_out is not ains._P - - def test__prep_F(self): - err_acc = {"N": 4.0e-4, "B": 2.0e-4, "tau_cb": 50} - err_gyro = { - "N": (np.pi) / 180.0 * 2.0e-3, - "B": (np.pi) / 180.0 * 8.0e-4, - "tau_cb": 50, - } - - quaternion = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - - F_matrix_out = AidedINS._prep_F(err_acc, err_gyro, quaternion) - - R = self.rot_matrix_from_quaternion # body-to-ned rotation matrix - S = _skew_symmetric # skew symmetric matrix - - # Dummy values - f_ins = np.array([0.0, 0.0, 0.0]) - w_ins = np.array([0.0, 0.0, 0.0]) - - # "State" matrix - F_matrix_expect = np.zeros((15, 15)) - F_matrix_expect[0:3, 3:6] = np.eye(3) - F_matrix_expect[3:6, 6:9] = -R(quaternion) @ S(f_ins) - F_matrix_expect[3:6, 9:12] = -R(quaternion) - F_matrix_expect[6:9, 6:9] = -S(w_ins) # NB! update each time step - F_matrix_expect[6:9, 12:15] = -np.eye(3) - F_matrix_expect[9:12, 9:12] = -(1.0 / err_acc["tau_cb"]) * np.eye(3) - F_matrix_expect[12:15, 12:15] = -(1.0 / err_gyro["tau_cb"]) * np.eye(3) - - np.testing.assert_allclose(F_matrix_out, F_matrix_expect, atol=1e-8) - - def test__update_F(self, ains): - quaternion_init = ains.quaternion() - - R = self.rot_matrix_from_quaternion # body-to-ned rotation matrix - S = _skew_symmetric # skew symmetric matrix - - # Dummy values - f_ins_init = np.array([0.0, 0.0, 0.0]) - w_ins_init = np.array([0.0, 0.0, 0.0]) - - F_matrix_init = ains._F.copy() - - quaternion = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - - f_ins = np.array([0.0, 0.0, -gravity()]) - w_ins = np.array([0.01, -0.01, 0.01]) - - ains._update_F(R(quaternion), f_ins, w_ins) - - delta_F_matrix_expect = np.zeros_like(F_matrix_init) - delta_F_matrix_expect[3:6, 6:9] = -R(quaternion) @ S(f_ins) - ( - -R(quaternion_init) @ S(f_ins_init) - ) - delta_F_matrix_expect[3:6, 9:12] = -R(quaternion) - (-R(quaternion_init)) - delta_F_matrix_expect[6:9, 6:9] = -S(w_ins) - (-S(w_ins_init)) - - np.testing.assert_allclose(ains._F - F_matrix_init, delta_F_matrix_expect) - - def test__prep_G(self): - quaternion = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - - G_matrix_out = AidedINS._prep_G(quaternion) - - R = self.rot_matrix_from_quaternion - - G_matrix_expect = np.zeros((15, 12)) - G_matrix_expect[3:6, 0:3] = -R(quaternion) # NB! update each time step - G_matrix_expect[6:9, 3:6] = -np.eye(3) - G_matrix_expect[9:12, 6:9] = np.eye(3) - G_matrix_expect[12:15, 9:12] = np.eye(3) - - np.testing.assert_allclose(G_matrix_out, G_matrix_expect, atol=1e-8) - - def test__update_G(self, ains): - quaternion_init = ains.quaternion() - - R = self.rot_matrix_from_quaternion # body-to-ned rotation matrix - - G_matrix_init = ains._G.copy() - - quaternion = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - - ains._update_G(R(quaternion)) - - delta_G_matrix_expect = np.zeros_like(G_matrix_init) - delta_G_matrix_expect[3:6, 0:3] = -R(quaternion) - (-R(quaternion_init)) - np.testing.assert_allclose( - ains._G - G_matrix_init, delta_G_matrix_expect, atol=1e-8 - ) - - def test__prep_W(self): - err_acc = {"N": 0.01, "B": 0.002, "tau_cb": 1000.0} - err_gyro = {"N": 0.03, "B": 0.004, "tau_cb": 2000.0} - - W_out = AidedINS._prep_W(err_acc, err_gyro) - - # White noise power spectral density matrix - W_expect = np.eye(12) - W_expect[0:3, 0:3] *= err_acc["N"] ** 2 - W_expect[3:6, 3:6] *= err_gyro["N"] ** 2 - W_expect[6:9, 6:9] *= 2.0 * err_acc["B"] ** 2 * (1.0 / err_acc["tau_cb"]) - W_expect[9:12, 9:12] *= 2.0 * err_gyro["B"] ** 2 * (1.0 / err_gyro["tau_cb"]) - - np.testing.assert_allclose(W_out, W_expect) - - def test__prep_H(self): - H_out = AidedINS._prep_H() - - H_expect = np.zeros((10, 15)) - H_expect[0:3, 0:3] = np.eye(3) # position - H_expect[3:6, 3:6] = np.eye(3) # velocity - - np.testing.assert_allclose(H_out, H_expect) - - def test__update_H_pos_lever_arm_zero(self, ains): - R = self.rot_matrix_from_quaternion - q = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - - H_out = ains._update_H_pos(R(q), np.zeros(3)) - H_expect = np.zeros((3, 15)) - H_expect[0:3, 0:3] = np.eye(3) # position - np.testing.assert_allclose(H_out, H_expect, atol=1e-8) - - def test__update_H_pos_lever_arm(self, ains): - R = self.rot_matrix_from_quaternion - S = _skew_symmetric # skew symmetric matrix - - q = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - lever_arm = np.array([1.0, 1.0, 1.0]) - - H_out = ains._update_H_pos(R(q), lever_arm) - H_expect = np.zeros((3, 15)) - H_expect[0:3, 0:3] = np.eye(3) # position - H_expect[0:3, 6:9] = -R(q) @ S(lever_arm) - - np.testing.assert_allclose(H_out, H_expect) - - def test__update_H_vel(self, ains): - H_out = ains._update_H_vel() - H_expect = np.zeros((3, 15)) - H_expect[0:3, 3:6] = np.eye(3) # velocity - np.testing.assert_allclose(H_out, H_expect) - - def test__update_H_g_ref(self, ains): - R = self.rot_matrix_from_quaternion - S = _skew_symmetric # skew symmetric matrix - - q = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - R_nm = R(q) - H_out = ains._update_H_g_ref(R_nm) - H_expect = np.zeros((3, 15)) - H_expect[0:3, 6:9] = S(R_nm.T @ ains._vg_ref_n) - np.testing.assert_allclose(H_out, H_expect) - - def test__update_yaw_from_quaternion(self, ains): - q = self.quaternion(alpha=0.0, beta=-12.0, gamma=45, degrees=True) - H_out = ains._update_yaw_from_quaternion(q) - H_expect = np.zeros((1, 15)) - H_expect[0, 6:9] = _yaw_gradient(q) - np.testing.assert_allclose(H_out, H_expect) - - def test_update_return_self(self, ains): - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - head = 0.0 - pos = np.zeros(3) - vel = np.zeros(3) - pos_var = np.ones(3) - vel_var = np.ones(3) - head_var = 1.0 - g_var = 0.1**2 * np.ones(3) - - update_return = ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - g_ref=True, - g_var=g_var, - ) - assert update_return is ains - - def test_update_var(self, ains): - """Update using aiding variances in update method.""" - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - - pos = np.zeros(3) - pos_var = np.ones(3) - vel = np.zeros(3) - vel_var = np.ones(3) - head = 0.0 - head_var = 1.0 - g_var = np.ones(3) - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - g_ref=True, - g_var=g_var, - ) - - def test_update_var_raises(self, ains): - """Check that update raise ValueError if no aiding variances are provided.""" - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - head = 0.0 - pos = np.zeros(3) - vel = np.zeros(3) - - with pytest.raises(ValueError): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=None, # no aiding variance provided - ) - - ains.update( - f_imu, - w_imu, - degrees=True, - vel=vel, - vel_var=None, # no aiding variance provided - ) - - ains.update( - f_imu, - w_imu, - degrees=True, - head=head, - head_var=None, # no aiding variance provided - head_degrees=True, - ) - - ains.update( - f_imu, - w_imu, - degrees=True, - g_ref=True, - g_var=None, # no aiding variance provided - ) - - def test_update_standstill(self): - - x0 = np.zeros(16) - x0[6] = 1.0 - P0_prior = 1e-6 * np.eye(15) - - ains = AidedINS(10.24, x0, P0_prior, ignore_bias_acc=False, cold_start=False) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - head = 0.0 - pos = np.zeros(3) - vel = np.zeros(3) - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=np.ones(3), - vel=vel, - vel_var=np.ones(3), - head=head, - head_var=0.1, - head_degrees=True, - g_ref=True, - g_var=np.ones(3), - ) - np.testing.assert_allclose(ains.x, x0) - - def test_update_irregular_aiding(self): - x0 = np.zeros(16) - x0[6] = 1.0 - P0_prior = 1e-6 * np.eye(15) - - ains = AidedINS(10.24, x0, P0_prior, ignore_bias_acc=False, cold_start=False) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - pos = np.zeros(3) - pos_var = np.ones(3) - vel = np.zeros(3) - vel_var = np.ones(3) - head = 0.0 - head_var = 1.0 - g_var = np.ones(3) - - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - g_ref=True, - g_var=g_var, - ) - np.testing.assert_allclose(ains.x, x0) - ains.update( - f_imu, - w_imu, - degrees=True, - ) - np.testing.assert_allclose(ains.x, x0) - - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - g_ref=True, - g_var=g_var, - ) - np.testing.assert_allclose(ains.x, x0) - - ains.update( - f_imu, - w_imu, - degrees=True, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - np.testing.assert_allclose(ains.x, x0) - - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - np.testing.assert_allclose(ains.x, x0) - - ains.update( - f_imu, - w_imu, - degrees=True, - head=head, - head_var=head_var, - head_degrees=True, - ) - np.testing.assert_allclose(ains.x, x0) - - ains.update(f_imu, w_imu, degrees=True, g_ref=True, g_var=g_var) - np.testing.assert_allclose(ains.x, x0) - - def test_update_ignore_bias_acc(self): - x0 = np.zeros(16) - x0[6] = 1.0 - - ains_a = AidedINS( - 10.24, - x0, - np.eye(15), - ignore_bias_acc=False, # include accelerometer bias - cold_start=False, - ) - - ains_b = AidedINS( - 10.24, - x0, - np.eye(12), - ignore_bias_acc=True, # ignore accelerometer bias - cold_start=False, - ) - - g = gravity() - f_imu = np.random.random(3) - np.array([0.0, 0.0, g]) - w_imu = np.random.random(3) - - pos = np.zeros(3) - pos_var = np.ones(3) - vel = np.zeros(3) - vel_var = np.ones(3) - head = np.random.random() - head_var = 1.0 - - ains_a.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - - ains_a.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - - ains_b.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - - ains_b.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - head_degrees=True, - ) - - assert not np.array_equal(ains_a.bias_acc(), x0[9:12]) # bias is updated - np.testing.assert_allclose(ains_b.bias_acc(), x0[9:12]) # no update - - def test_update_cold_start(self): - """ - Cold start with initial attitude far from true attitude. Cold start - roll and pitch calibration should fix this and avoid divergence. - """ - fs = 10.24 - n = int(60 * fs) # 1 min - - # True euler angles (far from initial state attitude) - euler_mean = np.array([-10.0, 45.0, 0.0]) # deg - - g = gravity() - q_mean = quaternion_from_euler(euler_mean, degrees=True) - R_mean = _rot_matrix_from_quaternion(q_mean) # body-to-ned - acc_mean = R_mean.T @ np.array([0.0, 0.0, -g]) # gravity only - - # Standstill reference signals - acc_ref = np.tile(acc_mean, (n, 1)) # m/s^2 - gyro_ref = np.zeros((n, 3)) # rad/s - pos_ref = np.zeros((n, 3)) # m - vel_ref = np.zeros((n, 3)) # m/s - euler_ref = np.tile(euler_mean, (n, 1)) # deg - - err_acc = sf.constants.ERR_ACC_MOTION2 - err_gyro = sf.constants.ERR_GYRO_MOTION2 - imu_noise = IMUNoise(err_acc, err_gyro, seed=0)(fs, n) - - pos_noise = 0.1 * np.random.default_rng(0).standard_normal((n, 3)) - vel_noise = 0.1 * np.random.default_rng(1).standard_normal((n, 3)) - head_noise = 0.1 * np.random.default_rng(2).standard_normal(n) - - f_imu = acc_ref + imu_noise[:, :3] - w_imu = gyro_ref + imu_noise[:, 3:] - pos_aid = pos_ref + pos_noise - vel_aid = vel_ref + vel_noise - head_aid = euler_ref[:, 2] + head_noise - - ains = AidedINS(fs, g=g, nav_frame="ned", cold_start=True) - - pos_est, vel_est, euler_est = [], [], [] - for i in range(n): - ains.update( - f_imu[i], - w_imu[i], - degrees=False, - pos=pos_aid[i], - pos_var=0.1**2 * np.ones(3), - vel=vel_aid[i], - vel_var=0.1**2 * np.ones(3), - head=head_aid[i], - head_var=0.5**2, - head_degrees=True, - ) - - pos_est.append(ains.position()) - vel_est.append(ains.velocity()) - euler_est.append(ains.euler(degrees=True)) - - pos_est = np.array(pos_est) - vel_est = np.array(vel_est) - euler_est = np.array(euler_est) - - np.testing.assert_allclose(pos_est, pos_ref, atol=0.2) - np.testing.assert_allclose(vel_est, vel_ref, atol=0.2) - np.testing.assert_allclose(euler_est, euler_ref, atol=0.2) - - @pytest.mark.parametrize( - "benchmark_gen", - [benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A], - ) - def test_benchmark(self, benchmark_gen): - fs_imu = 100.0 - fs_aiding = 1.0 - fs_ratio = np.ceil(fs_imu / fs_aiding) - warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning - compass_noise_std = 0.5 - gps_noise_std = 0.1 - vel_noise_std = 0.1 - - # Reference signals (without noise) - t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) - euler_ref = np.degrees(euler_ref) - gyro_ref = np.degrees(gyro_ref) - - # IMU measurements (with noise) - err_acc_true = { - "bc": (0.0, 0.0, 0.0), - "N": (4.0e-4, 4.0e-4, 4.5e-4), - "B": (1.5e-4, 1.5e-4, 3.0e-4), - "K": (4.5e-6, 4.5e-6, 1.5e-5), - "tau_cb": (50, 50, 30), - "tau_ck": (5e5, 5e5, 5e5), - } - err_gyro_true = { - "bc": (0.1, 0.2, 0.3), - "N": (1.9e-3, 1.9e-3, 1.7e-3), - "B": (7.5e-4, 4.0e-4, 8.8e-4), - "K": (2.5e-5, 2.5e-5, 4.0e-5), - "tau_cb": (50, 50, 50), - "tau_ck": (5e5, 5e5, 5e5), - } - noise_model = IMUNoise(err_acc=err_acc_true, err_gyro=err_gyro_true, seed=0) - imu_noise = noise_model(fs_imu, len(t)) - acc_noise = acc_ref + imu_noise[:, :3] - gyro_noise = gyro_ref + imu_noise[:, 3:] - - # Compass / heading (aiding) measurements - head_meas = euler_ref[:, 2] + white_noise( - compass_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=1 - ) - - # GPS / position (aiding) measurements - pos_noise = np.column_stack( - [ - white_noise( - gps_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=2 - ), - white_noise( - gps_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=3 - ), - white_noise( - gps_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=4 - ), - ] - ) - pos_meas = pos_ref + pos_noise - - # Velocity (aiding) measurements - vel_noise = np.column_stack( - [ - white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=5 - ), - white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=6 - ), - white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=7 - ), - ] - ) - vel_meas = vel_ref + vel_noise - - # MEKF - err_acc = {"N": 4.0e-4, "B": 1.5e-4, "K": 4.5e-6, "tau_cb": 50} - err_gyro = { - "N": (np.pi / 180.0) * 1.9e-3, - "B": (np.pi / 180.0) * 7.5e-4, - "tau_cb": 50, - } - P0_prior = np.eye(12) - x0 = np.zeros(16) - x0[0:3] = pos_ref[0] - x0[3:6] = vel_ref[0] - x0[6:10] = quaternion_from_euler(np.radians(euler_ref[0].flatten())) - mekf = AidedINS(fs_imu, x0, P0_prior, err_acc, err_gyro, ignore_bias_acc=True) - - # Apply filter - pos_out, vel_out, euler_out, bias_acc_out, bias_gyro_out = [], [], [], [], [] - for i, (acc_i, gyro_i, pos_i, vel_i, head_i) in enumerate( - zip(acc_noise, gyro_noise, pos_meas, vel_meas, head_meas) - ): - if not (i % fs_ratio): # with aiding - mekf.update( - acc_i, - gyro_i, - degrees=True, - pos=pos_i, - pos_var=gps_noise_std**2 * np.ones(3), - vel=vel_i, - vel_var=vel_noise_std**2 * np.ones(3), - head=head_i, - head_var=compass_noise_std**2, - head_degrees=True, - g_ref=True, - g_var=0.1**2 * np.ones(3), - ) - else: # without aiding - mekf.update(acc_i, gyro_i, degrees=True) - pos_out.append(mekf.position()) - vel_out.append(mekf.velocity()) - euler_out.append(mekf.euler(degrees=True)) - bias_acc_out.append(mekf.bias_acc()) - bias_gyro_out.append(mekf.bias_gyro(degrees=True)) - - pos_out = np.array(pos_out) - vel_out = np.array(vel_out) - euler_out = np.array(euler_out) - bias_acc_out = np.array(bias_acc_out) - bias_gyro_out = np.array(bias_gyro_out) - - # Half-sample shift (compensates for the delay introduced by Euler integration) - pos_out = resample_poly(pos_out, 2, 1)[1:-1:2] - pos_ref = pos_ref[:-1, :] - 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, :] - - pos_x_rms, pos_y_rms, pos_z_rms = np.std((pos_out - pos_ref)[warmup:], axis=0) - 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_acc_x_rms, bias_acc_y_rms, bias_acc_z_rms = np.std( - (bias_acc_out - err_acc_true["bc"])[warmup:], axis=0 - ) - bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std( - (bias_gyro_out - err_gyro_true["bc"])[warmup:], axis=0 - ) - - assert pos_x_rms <= 0.1 - assert pos_y_rms <= 0.1 - assert pos_z_rms <= 0.1 - assert vel_x_rms <= 0.02 - assert vel_y_rms <= 0.02 - assert vel_z_rms <= 0.02 - assert roll_rms <= 0.02 - assert pitch_rms <= 0.02 - assert yaw_rms <= 0.1 - assert bias_acc_x_rms <= 1e-3 - assert bias_acc_y_rms <= 1e-3 - assert bias_acc_z_rms <= 1e-3 - assert bias_gyro_x_rms <= 1e-3 - assert bias_gyro_y_rms <= 1e-3 - assert bias_gyro_z_rms <= 1e-3 - - -class Test_VRU: - - def test__init__no_x0_P0_err(self): - ains = VRU(10.24) - - assert ains._err_acc == ERR_ACC_MOTION2 - assert ains._err_gyro == ERR_GYRO_MOTION2 - np.testing.assert_allclose(ains.x, X0) - np.testing.assert_allclose(ains._P_prior, P0) - - def test_update_compare_to_ains(self): - """Update using aiding variances in update method.""" - fs = 10.24 - ains = AidedINS(fs) - vru = VRU(fs) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - - pos = np.zeros(3) - pos_var = np.ones(3) * 1.0e6 - vel = np.zeros(3) - vel_var = np.ones(3) * 100.0 - head = None - head_var = None - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - ) - - vru.update( - f_imu, - w_imu, - degrees=True, - ) - - np.testing.assert_allclose(ains.position(), vru.position()) - np.testing.assert_allclose(ains.velocity(), vru.velocity()) - np.testing.assert_allclose(ains.euler(), vru.euler()) - np.testing.assert_allclose(ains.quaternion(), vru.quaternion()) - np.testing.assert_allclose(ains.bias_acc(), vru.bias_acc()) - np.testing.assert_allclose(ains.bias_gyro(), vru.bias_gyro()) - - def test_update_compare_to_ains_other_aid(self): - """Update using aiding variances in update method.""" - fs = 10.24 - ains = AidedINS(fs) - vru = VRU(fs) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - - pos = np.zeros(3) - pos_var = np.ones(3) * 3.0e6 - vel = np.zeros(3) - vel_var = np.ones(3) * 10.0 - head = None - head_var = None - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - ) - - vru.update( - f_imu, - w_imu, - pos_var=np.ones(3) * 3.0e6, - vel_var=np.ones(3) * 10.0, - degrees=True, - ) - - np.testing.assert_allclose(ains.position(), vru.position()) - np.testing.assert_allclose(ains.velocity(), vru.velocity()) - np.testing.assert_allclose(ains.euler(), vru.euler()) - np.testing.assert_allclose(ains.quaternion(), vru.quaternion()) - np.testing.assert_allclose(ains.bias_acc(), vru.bias_acc()) - np.testing.assert_allclose(ains.bias_gyro(), vru.bias_gyro()) - - -class Test_AHRS: - def test__init__no_x0_P0_err(self): - ains = AHRS(10.24) - - assert ains._err_acc == ERR_ACC_MOTION2 - assert ains._err_gyro == ERR_GYRO_MOTION2 - np.testing.assert_allclose(ains.x, X0) - np.testing.assert_allclose(ains._P_prior, P0) - - def test_update_compare_to_ains(self): - """Update using aiding variances in update method.""" - fs = 10.24 - ains = AidedINS(fs) - ahrs = AHRS(fs) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - - pos = np.zeros(3) - pos_var = np.ones(3) * 1.0e6 - vel = np.zeros(3) - vel_var = np.ones(3) * 100.0 - head = None - head_var = None - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - ) - - ahrs.update( - f_imu, - w_imu, - degrees=True, - head=head, - head_var=head_var, - ) - - np.testing.assert_allclose(ains.position(), ahrs.position()) - np.testing.assert_allclose(ains.velocity(), ahrs.velocity()) - np.testing.assert_allclose(ains.euler(), ahrs.euler()) - np.testing.assert_allclose(ains.quaternion(), ahrs.quaternion()) - np.testing.assert_allclose(ains.bias_acc(), ahrs.bias_acc()) - np.testing.assert_allclose(ains.bias_gyro(), ahrs.bias_gyro()) - - def test_update_compare_to_ains_other_aid(self): - """Update using aiding variances in update method.""" - fs = 10.24 - ains = AidedINS(fs) - - ahrs = AHRS(fs) - - g = gravity() - f_imu = np.array([0.0, 0.0, -g]) - w_imu = np.zeros(3) - - pos = np.zeros(3) - pos_var = np.ones(3) * 3.0e6 - vel = np.zeros(3) - vel_var = np.ones(3) * 10.0 - head = None - head_var = None - - for _ in range(5): - ains.update( - f_imu, - w_imu, - degrees=True, - pos=pos, - pos_var=pos_var, - vel=vel, - vel_var=vel_var, - head=head, - head_var=head_var, - ) - - ahrs.update( - f_imu, - w_imu, - pos_var=np.ones(3) * 3.0e6, - vel_var=np.ones(3) * 10.0, - degrees=True, - head=head, - head_var=head_var, - ) - - np.testing.assert_allclose(ains.position(), ahrs.position()) - np.testing.assert_allclose(ains.velocity(), ahrs.velocity()) - np.testing.assert_allclose(ains.euler(), ahrs.euler()) - np.testing.assert_allclose(ains.quaternion(), ahrs.quaternion()) - np.testing.assert_allclose(ains.bias_acc(), ahrs.bias_acc()) - np.testing.assert_allclose(ains.bias_gyro(), ahrs.bias_gyro()) - - -class Test_FixedNed: - def test_init(self): - _ = 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 = 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 = 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_smoothing.py b/tests/test_smoothing.py deleted file mode 100644 index b71a4809..00000000 --- a/tests/test_smoothing.py +++ /dev/null @@ -1,353 +0,0 @@ -import numpy as np -import pytest -from scipy.signal import resample_poly - -import smsfusion as sf -from smsfusion import FixedIntervalSmoother -from smsfusion.benchmark import benchmark_full_pva_beat_202311A - - -class Test_FixedIntervalSmoother: - @pytest.fixture - def ains(self): - ains = sf.AidedINS(10.24) - return ains - - @pytest.fixture - def smoother(self, ains): - return FixedIntervalSmoother(ains) - - def test__init__(self, ains): - smoother = FixedIntervalSmoother(ains) - assert smoother._ains is ains - assert smoother._cov_smoothing is True - assert smoother.x.size == 0 - assert smoother.P.size == 0 - assert smoother.position().size == 0 - assert smoother.velocity().size == 0 - assert smoother.quaternion().size == 0 - assert smoother.bias_acc().size == 0 - assert smoother.bias_gyro().size == 0 - - def test_ains(self, ains): - smoother = FixedIntervalSmoother(ains) - assert smoother.ains is ains - - def test_update(self, smoother): - g = sf.gravity() - - smoother.update( - np.array([0.0, 0.0, -g]), - np.zeros(3), - degrees=True, - ) - assert smoother.x.shape == (1, 16) - - smoother.update( - np.array([0.0, 0.0, -g]), - np.zeros(3), - degrees=True, - pos=np.zeros(3), - pos_var=np.ones(3), - vel=np.zeros(3), - vel_var=np.ones(3), - head=0.0, - head_var=1.0, - head_degrees=True, - g_ref=True, - g_var=np.ones(3), - ) - assert smoother.x.shape == (2, 16) - - smoother.update( - np.array([0.0, 0.0, -g]), - np.zeros(3), - degrees=True, - head=0.0, - head_var=1.0, - head_degrees=True, - ) - assert smoother.x.shape == (3, 16) - - def test_benchmark_ains(self): - # Reference signal - fs = 10.24 # sampling rate in Hz - _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs) - euler = np.degrees(euler) - gyro = np.degrees(gyro) - head = euler[:, 2] - - # IMU measurements - err_acc = sf.constants.ERR_ACC_MOTION2 # m/s^2 - err_gyro = sf.constants.ERR_GYRO_MOTION2 # rad/s - imu_noise = sf.noise.IMUNoise(err_acc, err_gyro)(fs, len(acc)) - acc_imu = acc + imu_noise[:, :3] - gyro_imu = gyro + np.degrees(imu_noise[:, 3:]) - - # Aiding measurements - pos_noise_std = 0.1 # m - head_noise_std = 1.0 # deg - rng = np.random.default_rng(0) - pos_aid = pos + pos_noise_std * rng.standard_normal(pos.shape) - head_aid = head + head_noise_std * rng.standard_normal(head.shape) - - # AINS - p0 = pos[0] # position [m] - v0 = vel[0] # velocity [m/s] - q0 = sf.quaternion_from_euler(euler[0], degrees=True) # unit quaternion - ba0 = np.zeros(3) # accelerometer bias [m/s^2] - bg0 = np.zeros(3) # gyroscope bias [rad/s] - x0 = np.concatenate((p0, v0, q0, ba0, bg0)) - P0 = np.eye(12) * 1e-3 - ains = sf.AidedINS(fs, x0, P0, err_acc, err_gyro, cold_start=True) - - smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True) - - pos_ains, vel_ains, euler_ains, err_ains = [], [], [], [] - for f_i, w_i, p_i, h_i in zip(acc_imu, gyro_imu, pos_aid, head_aid): - smoother.update( - f_i, - w_i, - degrees=True, - pos=p_i, - pos_var=pos_noise_std**2 * np.ones(3), - head=h_i, - head_var=head_noise_std**2, - head_degrees=True, - ) - - pos_ains.append(smoother.ains.position()) - vel_ains.append(smoother.ains.velocity()) - euler_ains.append(smoother.ains.euler(degrees=True)) - err_ains.append(smoother.ains.P.diagonal()) - - # Forward filter state estimates - pos_ains = np.array(pos_ains) - vel_ains = np.array(vel_ains) - euler_ains = np.array(euler_ains) - err_ains = np.array(err_ains) - - # Smoothed state estimates - pos_smth = smoother.position() - vel_smth = smoother.velocity() - euler_smth = smoother.euler(degrees=True) - err_smth = np.array([P_i.diagonal() for P_i in smoother.P]) - - # Half-sample shift - # # (compensates for the delay introduced by Euler integration) - pos_ains = resample_poly(pos_ains, 2, 1)[1:-1:2] - pos_smth = resample_poly(pos_smth, 2, 1)[1:-1:2] - pos_ref = pos[:-1, :] - vel_ains = resample_poly(vel_ains, 2, 1)[1:-1:2] - vel_smth = resample_poly(vel_smth, 2, 1)[1:-1:2] - vel_ref = vel[:-1, :] - euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2] - euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] - euler_ref = euler[:-1, :] - - warmup = int(fs * 600.0) # truncate 600 seconds from the beginning - - pos_err_smth = np.std((pos_smth - pos_ref)[warmup:], axis=0) - pos_err_ains = np.std((pos_ains - pos_ref)[warmup:], axis=0) - np.testing.assert_array_less(pos_err_smth, pos_err_ains) - - vel_err_smth = np.std((vel_smth - vel_ref)[warmup:], axis=0) - vel_err_ains = np.std((vel_ains - vel_ref)[warmup:], axis=0) - np.testing.assert_array_less(vel_err_smth, vel_err_ains) - - euler_err_smth = np.std((euler_smth - euler_ref)[warmup:], axis=0) - euler_err_ains = np.std((euler_ains - euler_ref)[warmup:], axis=0) - np.testing.assert_array_less(euler_err_smth, euler_err_ains) - - smoother.P.shape == (len(acc_imu), 12, 12) - np.testing.assert_array_less(err_smth[warmup:], err_ains[warmup:] + 1e-12) - - def test_benchmark_ahrs(self): - # Reference signal - fs = 10.24 # sampling rate in Hz - _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs) - euler = np.degrees(euler) - gyro = np.degrees(gyro) - head = euler[:, 2] - - # IMU measurements - err_acc = sf.constants.ERR_ACC_MOTION2 # m/s^2 - err_gyro = sf.constants.ERR_GYRO_MOTION2 # rad/s - imu_noise = sf.noise.IMUNoise(err_acc, err_gyro)(fs, len(acc)) - acc_imu = acc + imu_noise[:, :3] - gyro_imu = gyro + np.degrees(imu_noise[:, 3:]) - - # Aiding measurements - head_noise_std = 1.0 # deg - rng = np.random.default_rng(0) - head_aid = head + head_noise_std * rng.standard_normal(head.shape) - - # AINS - p0 = pos[0] # position [m] - v0 = vel[0] # velocity [m/s] - q0 = sf.quaternion_from_euler(euler[0], degrees=True) # unit quaternion - ba0 = np.zeros(3) # accelerometer bias [m/s^2] - bg0 = np.zeros(3) # gyroscope bias [rad/s] - x0 = np.concatenate((p0, v0, q0, ba0, bg0)) - P0 = np.eye(12) * 1e-3 - ains = sf.AHRS(fs, x0, P0, err_acc, err_gyro, cold_start=True) - - smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True) - - euler_ains, err_ains = [], [] - for f_i, w_i, h_i in zip(acc_imu, gyro_imu, head_aid): - smoother.update( - f_i, - w_i, - degrees=True, - head=h_i, - head_var=head_noise_std**2, - head_degrees=True, - ) - - euler_ains.append(smoother.ains.euler(degrees=True)) - err_ains.append(smoother.ains.P.diagonal()) - - # Forward filter state estimates - euler_ains = np.array(euler_ains) - err_ains = np.array(err_ains) - - # Smoothed state estimates - euler_smth = smoother.euler(degrees=True) - err_smth = np.array([P_i.diagonal() for P_i in smoother.P]) - - # Half-sample shift - # # (compensates for the delay introduced by Euler integration) - euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2] - euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] - euler = euler[:-1, :] - - warmup = int(fs * 600.0) # truncate 600 seconds from the beginning - - euler_err_smth = np.std((euler_smth - euler)[warmup:], axis=0) - euler_err_ains = np.std((euler_ains - euler)[warmup:], axis=0) - np.testing.assert_array_less(euler_err_smth, euler_err_ains) - - smoother.P.shape == (len(acc_imu), 12, 12) - np.testing.assert_array_less(err_smth[warmup:], err_ains[warmup:] + 1e-12) - - def test_benchmark_vru(self): - # Reference signal - fs = 10.24 # sampling rate in Hz - _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs) - euler = np.degrees(euler) - gyro = np.degrees(gyro) - - # IMU measurements - err_acc = sf.constants.ERR_ACC_MOTION2 # m/s^2 - err_gyro = sf.constants.ERR_GYRO_MOTION2 # rad/s - imu_noise = sf.noise.IMUNoise(err_acc, err_gyro)(fs, len(acc)) - acc_imu = acc + imu_noise[:, :3] - gyro_imu = gyro + np.degrees(imu_noise[:, 3:]) - - # AINS - p0 = pos[0] # position [m] - v0 = vel[0] # velocity [m/s] - q0 = sf.quaternion_from_euler(euler[0], degrees=True) # unit quaternion - ba0 = np.zeros(3) # accelerometer bias [m/s^2] - bg0 = np.zeros(3) # gyroscope bias [rad/s] - x0 = np.concatenate((p0, v0, q0, ba0, bg0)) - P0 = np.eye(12) * 1e-3 - ains = sf.VRU(fs, x0, P0, err_acc, err_gyro, cold_start=True) - - smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True) - - euler_ains, err_ains = [], [] - for f_i, w_i in zip(acc_imu, gyro_imu): - smoother.update( - f_i, - w_i, - degrees=True, - ) - - euler_ains.append(smoother.ains.euler(degrees=True)) - err_ains.append(smoother.ains.P.diagonal()) - - # Forward filter state estimates - euler_ains = np.array(euler_ains) - err_ains = np.array(err_ains) - - # Smoothed state estimates - euler_smth = smoother.euler(degrees=True) - err_smth = np.array([P_i.diagonal() for P_i in smoother.P]) - - # Half-sample shift - # # (compensates for the delay introduced by Euler integration) - euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2] - euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] - euler = euler[:-1, :] - - # Drop yaw - euler = euler[:, :2] - euler_ains = euler_ains[:, :2] - euler_smth = euler_smth[:, :2] - - warmup = int(fs * 600.0) # truncate 600 seconds from the beginning - - euler_err_smth = np.std((euler_smth - euler)[warmup:], axis=0) - euler_err_ains = np.std((euler_ains - euler)[warmup:], axis=0) - np.testing.assert_array_less(euler_err_smth, euler_err_ains) - - smoother.P.shape == (len(acc_imu), 12, 12) - np.testing.assert_array_less(err_smth[warmup:], err_ains[warmup:] + 1e-12) - - def test_cov_smoothing(self): - # Reference signal - fs = 10.24 # sampling rate in Hz - _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs) - euler = np.degrees(euler) - gyro = np.degrees(gyro) - - # IMU measurements - err_acc = sf.constants.ERR_ACC_MOTION2 # m/s^2 - err_gyro = sf.constants.ERR_GYRO_MOTION2 # rad/s - imu_noise = sf.noise.IMUNoise(err_acc, err_gyro)(fs, len(acc)) - acc_imu = acc + imu_noise[:, :3] - gyro_imu = gyro + np.degrees(imu_noise[:, 3:]) - - # AINS - p0 = pos[0] # position [m] - v0 = vel[0] # velocity [m/s] - q0 = sf.quaternion_from_euler(euler[0], degrees=True) # unit quaternion - ba0 = np.zeros(3) # accelerometer bias [m/s^2] - bg0 = np.zeros(3) # gyroscope bias [rad/s] - x0 = np.concatenate((p0, v0, q0, ba0, bg0)) - P0 = np.eye(12) * 1e-3 - - ains_a = sf.VRU(fs, x0, P0, err_acc, err_gyro) - smoother_a = sf.FixedIntervalSmoother(ains_a, cov_smoothing=True) - ains_b = sf.VRU(fs, x0, P0, err_acc, err_gyro) - smoother_b = sf.FixedIntervalSmoother(ains_b, cov_smoothing=False) - - err_ains = [] - for f_i, w_i in zip(acc_imu, gyro_imu): - smoother_a.update( - f_i, - w_i, - degrees=True, - ) - smoother_b.update( - f_i, - w_i, - degrees=True, - ) - - err_ains.append(smoother_a.ains.P.diagonal()) - - # Forward filter state estimates - err_ains = np.array(err_ains) - - # Smoothed state estimates - err_smth_a = np.array([P_i.diagonal() for P_i in smoother_a.P]) - err_smth_b = np.array([P_i.diagonal() for P_i in smoother_b.P]) - - smoother_a.P.shape == (len(acc_imu), 12, 12) - smoother_b.P.shape == (len(acc_imu), 12, 12) - np.testing.assert_array_less(err_smth_a, err_ains + 1e-12) - np.testing.assert_array_less(err_smth_a, err_smth_b + 1e-12) - np.testing.assert_allclose(err_smth_b, err_ains)