From c12b10f3f14b92339de227db984650a7b9d10747 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 12:56:39 +0100 Subject: [PATCH 01/86] init method --- src/smsfusion/_v2.py | 246 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 246 insertions(+) create mode 100644 src/smsfusion/_v2.py diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py new file mode 100644 index 00000000..7429ce28 --- /dev/null +++ b/src/smsfusion/_v2.py @@ -0,0 +1,246 @@ +from numba import njit +import numpy as np +from numpy.typing import ArrayLike, NDArray + +from ._ins import dhda_head +from ._vectorops import _normalize, _quaternion_product, _skew_symmetric + + +def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]: + """ + Gravity vector expressed in the navigation frame ('NED' or 'ENU'). + + Parameters + ---------- + g : float + Gravitational acceleration in m/s^2. + nav_frame : {'NED', 'ENU'} + Navigation frame in which the gravity vector is expressed. + + Returns + ------- + ndarray, shape (3,) + Gravity vector expressed in the navigation frame. + """ + if nav_frame.lower() == "ned": + g_n = np.array([0.0, 0.0, g]) + elif nav_frame.lower() == "enu": + g_n = np.array([0.0, 0.0, -g]) + else: + raise ValueError(f"Unknown navigation frame: {nav_frame}.") + return g_n + + +def _nz2vg(nav_frame: str) -> float: + """ + Gravity direction along the navigation frame's z-axis. + """ + if nav_frame == "ned": + return 1.0 + elif nav_frame == "enu": + return -1.0 + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + + +@njit # type: ignore[misc] +def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: + """ + Gravity reference vector expressed in the body frame, computed from the attitude + quaternion, q_nb. + + Parameters + ---------- + q_nb : numpy.ndarray, shape (4,) + Unit quaternion. + nz2vg : float + Gravity direction along the navigation frame's z-axis. Should be +1 for + NED and -1 for ENU. + """ + qw, qx, qy, qz = q_nb + + x = 2.0 * (qx * qz - qw * qy) + y = 2.0 * (qy * qz + qw * qx) + z = 1.0 - 2.0 * (qx**2 + qy**2) + + return nz2vg * np.array([x, y, z]) + + +def _state_transition( + dt: float, w_b: NDArray[np.float64], gbc: float +) -> NDArray[np.float64]: + """ + State transition matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + w_b : ndarray, shape (3,) + Angular rate measurement (bias corrected) in body frame. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + ndarray, shape (6, 6) + State transition matrix. + """ + phi = np.eye(6) + phi[0:3, 0:3] -= dt * _skew_symmetric(w_b) # NB! update each time step + phi[0:3, 3:6] -= dt * np.eye(3) + phi[3:6, 3:6] -= dt * np.eye(3) / gbc + return phi + + +@njit # type: ignore[misc] +def _update_state_transition( + phi: NDArray[np.float64], + dt: float, + w_b: NDArray[np.float64], +) -> None: + """ + Update the state transition matrix in place. + + Parameters + ---------- + phi : ndarray, shape (6, 6) + State transition matrix to be updated in place. + dt : float + Time step. + w_b : ndarray, shape (3,) + Angular rate measurement (bias corrected) in body frame. + """ + wx, wy, wz = w_b + phi[0, 1] = dt * wz + phi[0, 2] = -dt * wy + phi[1, 0] = -dt * wz + phi[1, 2] = dt * wx + phi[2, 0] = dt * wy + phi[2, 1] = -dt * wx + + +def _process_noise_cov( + dt: float, arw: float, gbs: float, gbc: float +) -> NDArray[np.float64]: + """ + Process noise covariance matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + arw : float + Angular random walk (gyroscope noise density) in rad/√Hz. + gbs : float + Gyro bias stability (bias instability) in rad/s. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + Q : ndarray, shape (6, 6) + Process noise covariance matrix. + """ + Q = np.zeros((6, 6)) + Q[0:3, 0:3] = dt * arw**2 * np.eye(3) + Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + return Q + + +def _measurement_matrix( + q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] +) -> NDArray[np.float64]: + """ + Measurement matrix. + + Parameters + ---------- + q_nb : ndarray, shape (4,) + Unit quaternion. + vg_b : ndarray, shape (3,) + Gravity reference unit vector expressed in the body frame. + + Returns + ------- + ndarray, shape (4, 6) + Linearized measurement matrix. + """ + dhdx = np.zeros((4, 6)) + dhdx[0:3, 0:3] = _skew_symmetric(vg_b) # gravity ref vector + dhdx[3:4, 0:3] = dhda_head(q_nb) # heading + return dhdx + + +class VRU: + """ + Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF). + + Parameters + ---------- + fs : float + Sampling rate in Hz. + q_nb : Attitude or array_like, shape (4,), optional + Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults + to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). + bg_b : array_like, shape (3,), optional + Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. + w_b : array_like, shape (3,), optional + Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame. + Defaults to zero angular rate (stationary). + P : array_like, shape (6, 6), optional + Initial (a priori) estimate of the error covariance matrix, **P**. If not + given, a small diagonal matrix will be used. + gyro_noise_density : float, optional + Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to + 0.00005 (SMS Motion 2 noise level). + gyro_bias_stability : float, optional + Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). + gyro_bias_corr_time : float, optional + Gyroscope bias correlation time in seconds. Defaults to 50.0 s. + nav_frame : {'NED', 'ENU'}, default 'NED' + Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) + (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom + will be expressed relative to this frame. + + """ + _I: NDArray[np.float64] = np.eye(6) + + def __init__( + self, + fs: float, + q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), + bg_b: ArrayLike = (0.0, 0.0, 0.0), + w_b: ArrayLike = (0.0, 0.0, 0.0), + P: ArrayLike = 1e-6 * np.eye(6), + gyro_noise_density: float = 0.0001, + gyro_bias_stability: float = 0.00005, + gyro_bias_corr_time: float = 50.0, + nav_frame: str = "NED", + ) -> None: + self._fs = fs + self._dt = 1.0 / fs + self._nav_frame = nav_frame.lower() + self._nz2vg = _nz2vg(self._nav_frame) + + # IMU noise parameters + self._arw = gyro_noise_density # angular random walk + self._gbs = gyro_bias_stability # gyro bias stability + self._gbc = gyro_bias_corr_time # gyro bias correlation time + + # State and covariance estimates + self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() + self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() + self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy() + self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() + self._dx = np.zeros(6) + + # Discrete state-space model + self._phi = _state_transition(self._dt, self._w_b, self._gbc) + self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) + self._dhdx = _measurement_matrix(self._q_nb, self._vg_b) + + @property + def _vg_b(self): + """Gravity reference vector (unit vector) expressed in the body frame.""" + return _vg_b(self._q_nb, self._nz2vg) From cc21eae8e05f171c7f5f8b07dbe5f6df9c7d6590 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 12:59:58 +0100 Subject: [PATCH 02/86] properties --- src/smsfusion/_v2.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 7429ce28..f543915d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -244,3 +244,28 @@ def __init__( def _vg_b(self): """Gravity reference vector (unit vector) expressed in the body frame.""" return _vg_b(self._q_nb, self._nz2vg) + + def quaternion(self) -> NDArray[np.float64]: + """ + Return a copy of the attitude quaternion. + """ + return self._q_nb.copy() + + def bias_gyro(self) -> NDArray[np.float64]: + """ + Return a copy of the gyroscope bias estimate (rad/s) expressed in the body frame. + """ + return self._bg_b.copy() + + def angular_rate(self) -> NDArray[np.float64]: + """ + Return a copy of the bias corrected angular rate measurement (rad/s). + """ + return self._w_b.copy() + + @property + def P(self) -> NDArray[np.float64]: + """ + Copy of the error covariance matrix estimate. + """ + return self._P.copy() From f57c6efc4fc2f75fc9c7d02150136effddf03d1c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:07:09 +0100 Subject: [PATCH 03/86] more methods --- src/smsfusion/_v2.py | 63 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index f543915d..efa695f1 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -172,6 +172,43 @@ def _measurement_matrix( return dhdx +@njit # type: ignore[misc] +def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: + """ + Corrects a unit quaternion, q, with a small attitude error, da, parameterized + as a scaled (2x) Gibbs vector: + + q = q ⊗ dq(da) + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion [qw, qx, qy, qz] (modified in place). + da : ndarray, shape (3,) + Small attitude error parameterized as a scaled (2x) Gibbs vector. + + Notes + ----- + As described in ref [1]_, this correction can be simplified by doing it in two + steps: first a correction, followed by renormalization. The scaling factor becomes + obsolete due to the renormalization step. + + References + ---------- + Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination + and Control, Eq. (6.27)-(6.28). + """ + + qw, qx, qy, qz = q + dax, day, daz = da + + q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) + q[1] += 0.5 * (qw * dax + qy * daz - qz * day) + q[2] += 0.5 * (qw * day - qx * daz + qz * dax) + q[3] += 0.5 * (qw * daz + qx * day - qy * dax) + q[:] = _normalize(q) + + class VRU: """ Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF). @@ -269,3 +306,29 @@ def P(self) -> NDArray[np.float64]: Copy of the error covariance matrix estimate. """ return self._P.copy() + + def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Gravity reference vector part of the measurement matrix, shape (3, 6). + """ + self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) + return self._dhdx[0:3] + + def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Heading (yaw angle) part of the measurement matrix, shape (6,). + """ + self._dhdx[3:4, 0:3] = dhda_head(q_nb) + return self._dhdx[3] + + def _reset(self) -> None: + """ + Reset state. + """ + + if not self._dx.any(): + return + + _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3]) + self._bg_b[:] += self._dx[3:6] + self._dx[:] = 0.0 From 22271e3ce0e6f169e898c6717b25b4ef128491e8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:14:05 +0100 Subject: [PATCH 04/86] more code --- src/smsfusion/_v2.py | 179 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 172 insertions(+), 7 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index efa695f1..89ac2619 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -2,7 +2,7 @@ import numpy as np from numpy.typing import ArrayLike, NDArray -from ._ins import dhda_head +from ._ins import dhda_head, _signed_smallest_angle, h_head from ._vectorops import _normalize, _quaternion_product, _skew_symmetric @@ -209,6 +209,137 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - q[:] = _normalize(q) +@njit # type: ignore[misc] +def _kalman_gain( + P: NDArray[np.float64], h: NDArray[np.float64], r: float +) -> NDArray[np.float64]: + """ + Compute the Kalman gain for a scalar measurement. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + + Returns + ------- + ndarray, shape (n,) + Kalman gain vector. + """ + + # Innovation covariance (inverse) + Ph = np.dot(P, h) + s_inv = 1.0 / (np.dot(h, Ph) + r) + + # Kalman gain + k = Ph * s_inv + + return k + + +@njit # type: ignore[misc] +def _covariance_update( + P: NDArray[np.float64], + k: NDArray[np.float64], + h: NDArray[np.float64], + r: float, + I_: NDArray[np.float64], +) -> None: + """ + Compute the updated state error covariance matrix estimate (Joseph form). + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + k : ndarray, shape (n,) + Kalman gain vector. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + A = I_ - np.outer(k, h) + P = A @ P @ A.T + r * np.outer(k, k) + return P + + +@njit # type: ignore[misc] +def _kalman_update_scalar( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: float, + r: float, + h: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Scalar Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : float + Scalar measurement. + r : float + Scalar measurement noise variance. + h : ndarray, shape (n,) + Measurement matrix (row vector). + I_ : ndarray, shape (n, n) + Identity matrix. + """ + + # Kalman gain + k = _kalman_gain(P, h, r) + + # Updated (a posteriori) state estimate + x[:] += k * (z - np.dot(h, x)) + + # Updated (a posteriori) covariance estimate (Joseph form) + P[:, :] = _covariance_update(P, k, h, r, I_) + + +@njit # type: ignore[misc] +def _kalman_update_sequential( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: NDArray[np.float64], + var: NDArray[np.float64], + H: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Sequential (one-at-a-time) Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : ndarray, shape (m,) + Measurement vector. + var : ndarray, shape (m,) + Measurement noise variances corresponding to each scalar measurement. + H : ndarray, shape (m, n) + Measurement matrix where each row corresponds to a scalar measurement model. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + m = z.shape[0] + for i in range(m): + _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) + + class VRU: """ Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF). @@ -275,12 +406,7 @@ def __init__( # Discrete state-space model self._phi = _state_transition(self._dt, self._w_b, self._gbc) self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) - self._dhdx = _measurement_matrix(self._q_nb, self._vg_b) - - @property - def _vg_b(self): - """Gravity reference vector (unit vector) expressed in the body frame.""" - return _vg_b(self._q_nb, self._nz2vg) + self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg)) def quaternion(self) -> NDArray[np.float64]: """ @@ -332,3 +458,42 @@ def _reset(self) -> None: _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3]) self._bg_b[:] += self._dx[3:6] self._dx[:] = 0.0 + + def _aiding_update_gref( + self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None + ) -> None: + """ + Update with gravity reference vector aiding measurement. + """ + + if vg_meas is None: + return None + + if vg_var is None: + raise ValueError("'vg_var' not provided.") + + vg_b = _vg_b(self._q_nb, self._nz2vg) + dz = vg_meas - vg_b + dhdx = self._dhdx_gref(vg_b) + _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) + + def _aiding_update_yaw( + self, yaw_meas: float | None, yaw_var: float | None, yaw_degrees: bool + ) -> None: + """ + Update with heading aiding measurement. + """ + + if yaw_meas is None: + return None + + if yaw_var is None: + raise ValueError("'yaw_var' not provided.") + + if yaw_degrees: + yaw_meas = (np.pi / 180.0) * yaw_meas + yaw_var = (np.pi / 180.0) ** 2 * yaw_var + + dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb)) + dhdx = self._dhdx_yaw(self._q_nb) + _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I) From a70983c93e67ce37bb08c63c394e275586b6d28e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:21:12 +0100 Subject: [PATCH 05/86] project ahead --- src/smsfusion/_v2.py | 40 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 89ac2619..9182de74 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -3,6 +3,7 @@ from numpy.typing import ArrayLike, NDArray from ._ins import dhda_head, _signed_smallest_angle, h_head +from ._transforms import _angular_matrix_from_quaternion from ._vectorops import _normalize, _quaternion_product, _skew_symmetric @@ -340,7 +341,32 @@ def _kalman_update_sequential( _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) -class VRU: +@njit # type: ignore[misc] +def _project_cov_ahead( + P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] +) -> None: + """ + Project the error covariance matrix estimate ahead. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be projected ahead. + phi : ndarray, shape (n, n) + State transition matrix. + Q : ndarray, shape (n, n) + Process noise covariance matrix. + + Returns + ------- + ndarray, shape (n, n) + Projected error covariance matrix estimate. + """ + P = phi @ P @ phi.T + Q + return P + + +class VRUv2: """ Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF). @@ -497,3 +523,15 @@ def _aiding_update_yaw( dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb)) dhdx = self._dhdx_yaw(self._q_nb) _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I) + + def _project_ahead(self) -> None: + """ + Project state and covariance estimates ahead. + """ + + # Attitude (dead reckoning) + self._q_nb[:] += self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b + self._q_nb[:] = _normalize(self._q_nb) + + # Covariance + self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From 54a828aebcdd675eeaa5bc9d3982678a8dbf46b1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:22:35 +0100 Subject: [PATCH 06/86] rename to head --- src/smsfusion/_v2.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 9182de74..e83528f5 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -503,26 +503,26 @@ def _aiding_update_gref( dhdx = self._dhdx_gref(vg_b) _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) - def _aiding_update_yaw( - self, yaw_meas: float | None, yaw_var: float | None, yaw_degrees: bool + def _aiding_update_head( + self, head_meas: float | None, head_var: float | None, head_degrees: bool ) -> None: """ Update with heading aiding measurement. """ - if yaw_meas is None: + if head_meas is None: return None - if yaw_var is None: - raise ValueError("'yaw_var' not provided.") + if head_var is None: + raise ValueError("'head_var' not provided.") - if yaw_degrees: - yaw_meas = (np.pi / 180.0) * yaw_meas - yaw_var = (np.pi / 180.0) ** 2 * yaw_var + if head_degrees: + head_meas = (np.pi / 180.0) * head_meas + head_var = (np.pi / 180.0) ** 2 * head_var - dz = _signed_smallest_angle(yaw_meas - h_head(self._q_nb)) + dz = _signed_smallest_angle(head_meas - h_head(self._q_nb)) dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, yaw_var, dhdx, self._I) + _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) def _project_ahead(self) -> None: """ From fa2a84a539e6f42415407e17e37b56be0b6f24ac Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:37:22 +0100 Subject: [PATCH 07/86] more code --- src/smsfusion/_v2.py | 68 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index e83528f5..0629c24b 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -1,3 +1,5 @@ +from typing import Self + from numba import njit import numpy as np from numpy.typing import ArrayLike, NDArray @@ -535,3 +537,69 @@ def _project_ahead(self) -> None: # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) + + def update( + self, + f: ArrayLike, + w: ArrayLike, + degrees: bool = False, + head: float | None = None, + head_var: float | None = None, + head_degrees: bool = False, + g_ref: bool = True, + g_var: ArrayLike | None = (0.001, 0.001, 0.001), + ) -> Self: + """ + Update state estimates with IMU and aiding measurements. + + Parameters + ---------- + f : array_like, shape (3,) + Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) + in m/s^2. + w : array_like, shape (3,) + Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See + ``degrees`` parameter for units. + degrees : bool, optional + Specifies whether the unit of the rotation rate, ``w``, is deg/s or + rad/s. Defaults to rad/s. + head : float, optional + Heading measurement. I.e., the yaw angle of the 'body' frame relative to the + assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. + If ``None``, compass aiding is not used. See ``head_degrees`` for units. + head_var : float, optional + Variance of heading measurement noise. Units must be compatible with ``head``. + See ``head_degrees`` for units. Required for ``head``. + head_degrees : bool, default False + Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, + or radians and radians^2. Default is in radians and radians^2. + g_ref : bool, optional + Specifies whether the gravity reference vector is used as an aiding measurement. + g_var : array-like, optional + Variance of gravitational reference vector measurement noise. Required for + ``g_ref``. + + Returns + ------- + MEKF + A reference to the instance itself after the update. + """ + + if degrees: + w = np.radians(w) + + # Project (a priori) state and covariance estimates ahead + self._project_ahead() + + # Update (a posteriori) state and covariance estimates with aiding measurements + self._aiding_update_gref(-_normalize(f) if g_ref else None, g_var) + self._aiding_update_head(head, head_var, head_degrees) + + # Reset state + self._reset() + + # Update model + self._w_b[:] = w - self._bg_b + _update_state_transition(self._phi, self._dt, self._w_b) + + return self From 8a528a41958ba7f870c1b1de42b6da1c83d86409 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 13:40:44 +0100 Subject: [PATCH 08/86] init and style --- src/smsfusion/__init__.py | 2 ++ src/smsfusion/_v2.py | 15 +++++++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 48ec7ea4..58a51310 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,6 +3,7 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler +from ._v2 import VRUv2 __all__ = [ "AHRS", @@ -16,6 +17,7 @@ "noise", "StrapdownINS", "VRU", + "VRUv2", "quaternion_from_euler", "ConingScullingAlg", ] diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 0629c24b..17a9e58f 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -1,10 +1,10 @@ from typing import Self -from numba import njit import numpy as np +from numba import njit from numpy.typing import ArrayLike, NDArray -from ._ins import dhda_head, _signed_smallest_angle, h_head +from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion from ._vectorops import _normalize, _quaternion_product, _skew_symmetric @@ -171,7 +171,7 @@ def _measurement_matrix( """ dhdx = np.zeros((4, 6)) dhdx[0:3, 0:3] = _skew_symmetric(vg_b) # gravity ref vector - dhdx[3:4, 0:3] = dhda_head(q_nb) # heading + dhdx[3:4, 0:3] = _dhda_head(q_nb) # heading return dhdx @@ -400,6 +400,7 @@ class VRUv2: will be expressed relative to this frame. """ + _I: NDArray[np.float64] = np.eye(6) def __init__( @@ -472,7 +473,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: """ Heading (yaw angle) part of the measurement matrix, shape (6,). """ - self._dhdx[3:4, 0:3] = dhda_head(q_nb) + self._dhdx[3:4, 0:3] = _dhda_head(q_nb) return self._dhdx[3] def _reset(self) -> None: @@ -522,7 +523,7 @@ def _aiding_update_head( head_meas = (np.pi / 180.0) * head_meas head_var = (np.pi / 180.0) ** 2 * head_var - dz = _signed_smallest_angle(head_meas - h_head(self._q_nb)) + dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) dhdx = self._dhdx_yaw(self._q_nb) _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) @@ -532,7 +533,9 @@ def _project_ahead(self) -> None: """ # Attitude (dead reckoning) - self._q_nb[:] += self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b + self._q_nb[:] += ( + self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b + ) self._q_nb[:] = _normalize(self._q_nb) # Covariance From 8d39d4ee1c5bc2d0fa4ce5b974355d4d26bc70fd Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:07:12 +0100 Subject: [PATCH 09/86] remove obsolete import --- src/smsfusion/_v2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 17a9e58f..9cdd6c49 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -6,7 +6,7 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion -from ._vectorops import _normalize, _quaternion_product, _skew_symmetric +from ._vectorops import _normalize, _skew_symmetric def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]: From a2962bde7a6bada01ca6e0df23e787242a7a37b5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:14:59 +0100 Subject: [PATCH 10/86] delete obsolete function --- src/smsfusion/_v2.py | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 9cdd6c49..bf39ff06 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -9,31 +9,6 @@ from ._vectorops import _normalize, _skew_symmetric -def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]: - """ - Gravity vector expressed in the navigation frame ('NED' or 'ENU'). - - Parameters - ---------- - g : float - Gravitational acceleration in m/s^2. - nav_frame : {'NED', 'ENU'} - Navigation frame in which the gravity vector is expressed. - - Returns - ------- - ndarray, shape (3,) - Gravity vector expressed in the navigation frame. - """ - if nav_frame.lower() == "ned": - g_n = np.array([0.0, 0.0, g]) - elif nav_frame.lower() == "enu": - g_n = np.array([0.0, 0.0, -g]) - else: - raise ValueError(f"Unknown navigation frame: {nav_frame}.") - return g_n - - def _nz2vg(nav_frame: str) -> float: """ Gravity direction along the navigation frame's z-axis. From 553b2a211c2b4f1b594f4f78356344d3e70a01e2 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:27:16 +0100 Subject: [PATCH 11/86] euler method --- src/smsfusion/_v2.py | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index bf39ff06..5c40299e 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -5,7 +5,7 @@ from numpy.typing import ArrayLike, NDArray from ._ins import _dhda_head, _h_head, _signed_smallest_angle -from ._transforms import _angular_matrix_from_quaternion +from ._transforms import _angular_matrix_from_quaternion, _euler_from_quaternion from ._vectorops import _normalize, _skew_symmetric @@ -414,19 +414,41 @@ def __init__( def quaternion(self) -> NDArray[np.float64]: """ - Return a copy of the attitude quaternion. + Attitude expressed as a unit quaternion. """ return self._q_nb.copy() + + def euler(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Attitude expressed as Euler angles (roll, pitch, yaw). + + Parameters + ---------- + degrees : bool, default False + Whether to return the Euler angles in degrees or radians. + + Returns + ------- + numpy.ndarray, shape (3,) + Euler angles (roll, pitch, yaw). + """ + + theta = _euler_from_quaternion(self._q_nb) + + if degrees: + theta = (180.0 / np.pi) * theta + + return theta def bias_gyro(self) -> NDArray[np.float64]: """ - Return a copy of the gyroscope bias estimate (rad/s) expressed in the body frame. + Gyroscope bias estimate (rad/s) expressed in the body frame. """ return self._bg_b.copy() def angular_rate(self) -> NDArray[np.float64]: """ - Return a copy of the bias corrected angular rate measurement (rad/s). + Bias corrected angular rate measurement (rad/s) expressed in the body frame. """ return self._w_b.copy() From 45275d14c4e9d1d41fc0352dc15ce2dedf0b236e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:37:52 +0100 Subject: [PATCH 12/86] rename to AHRS --- src/smsfusion/__init__.py | 4 ++-- src/smsfusion/_v2.py | 29 +++++++++++++---------------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 58a51310..6dd89429 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,10 +3,11 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import VRUv2 +from ._v2 import AHRSv2 __all__ = [ "AHRS", + "AHRSv2", "AidedINS", "benchmark", "constants", @@ -17,7 +18,6 @@ "noise", "StrapdownINS", "VRU", - "VRUv2", "quaternion_from_euler", "ConingScullingAlg", ] diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 5c40299e..6d8ae286 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -5,7 +5,8 @@ from numpy.typing import ArrayLike, NDArray from ._ins import _dhda_head, _h_head, _signed_smallest_angle -from ._transforms import _angular_matrix_from_quaternion, _euler_from_quaternion +from ._transforms import _angular_matrix_from_quaternion as T +from ._transforms import _euler_from_quaternion from ._vectorops import _normalize, _skew_symmetric @@ -24,8 +25,7 @@ def _nz2vg(nav_frame: str) -> float: @njit # type: ignore[misc] def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: """ - Gravity reference vector expressed in the body frame, computed from the attitude - quaternion, q_nb. + Gravity reference vector expressed in the body frame, computed from a unit quaternion. Parameters ---------- @@ -158,6 +158,10 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - q = q ⊗ dq(da) + As described in ref [1]_, this correction can be simplified by doing it in two + steps: first a correction, followed by renormalization. The scaling factor becomes + obsolete due to the renormalization step. + Parameters ---------- q : ndarray, shape (4,) @@ -165,12 +169,6 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - da : ndarray, shape (3,) Small attitude error parameterized as a scaled (2x) Gibbs vector. - Notes - ----- - As described in ref [1]_, this correction can be simplified by doing it in two - steps: first a correction, followed by renormalization. The scaling factor becomes - obsolete due to the renormalization step. - References ---------- Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination @@ -343,9 +341,10 @@ def _project_cov_ahead( return P -class VRUv2: +class AHRSv2: """ - Vertical Reference Unit (VRU) using a multiplicative extended Kalman filter (MEKF). + Attitude and Heading Reference System (AHRS) using a multiplicative extended + Kalman filter (MEKF). Parameters ---------- @@ -417,7 +416,7 @@ def quaternion(self) -> NDArray[np.float64]: Attitude expressed as a unit quaternion. """ return self._q_nb.copy() - + def euler(self, degrees: bool = False) -> NDArray[np.float64]: """ Attitude expressed as Euler angles (roll, pitch, yaw). @@ -530,9 +529,7 @@ def _project_ahead(self) -> None: """ # Attitude (dead reckoning) - self._q_nb[:] += ( - self._dt * _angular_matrix_from_quaternion(self._q_nb) @ self._w_b - ) + self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b self._q_nb[:] = _normalize(self._q_nb) # Covariance @@ -581,7 +578,7 @@ def update( Returns ------- - MEKF + AHRS A reference to the instance itself after the update. """ From ffcbb233db6cf92a9a51c5ca66c7795adaf6506f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:41:34 +0100 Subject: [PATCH 13/86] gyro bias degrees flag --- src/smsfusion/_v2.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 6d8ae286..d011fc19 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -439,11 +439,19 @@ def euler(self, degrees: bool = False) -> NDArray[np.float64]: return theta - def bias_gyro(self) -> NDArray[np.float64]: + def bias_gyro(self, degrees=False) -> NDArray[np.float64]: """ Gyroscope bias estimate (rad/s) expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the bias in deg/s or rad/s. Defaults to rad/s. """ - return self._bg_b.copy() + bg_b = self._bg_b.copy() + if degrees: + bg_b = (180.0 / np.pi) * bg_b + return bg_b def angular_rate(self) -> NDArray[np.float64]: """ From 45f667b4adee09fb68ebb08c902e2455a525782e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 12 Mar 2026 14:45:26 +0100 Subject: [PATCH 14/86] degrees flag in angular rate --- src/smsfusion/_v2.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index d011fc19..bc01b4c7 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -453,11 +453,19 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]: bg_b = (180.0 / np.pi) * bg_b return bg_b - def angular_rate(self) -> NDArray[np.float64]: + def angular_rate(self, degrees=False) -> NDArray[np.float64]: """ - Bias corrected angular rate measurement (rad/s) expressed in the body frame. + Bias corrected angular rate measurement expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. """ - return self._w_b.copy() + w_b = self._w_b.copy() + if degrees: + w_b = (180.0 / np.pi) * w_b + return w_b @property def P(self) -> NDArray[np.float64]: From 4cc61ff3e0b7dba4e07e299a3df9af59d2fb2090 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 08:33:30 +0100 Subject: [PATCH 15/86] v2b --- src/smsfusion/__init__.py | 4 +- src/smsfusion/_v2.py | 2 +- src/smsfusion/_v2b.py | 146 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 149 insertions(+), 3 deletions(-) create mode 100644 src/smsfusion/_v2b.py diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 6dd89429..0b202de0 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,11 +3,11 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import AHRSv2 +from ._v2 import AHRSv2a __all__ = [ "AHRS", - "AHRSv2", + "AHRSv2a", "AidedINS", "benchmark", "constants", diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index bc01b4c7..f1db0be9 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -341,7 +341,7 @@ def _project_cov_ahead( return P -class AHRSv2: +class AHRSv2a: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended Kalman filter (MEKF). diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py new file mode 100644 index 00000000..dbce5fe6 --- /dev/null +++ b/src/smsfusion/_v2b.py @@ -0,0 +1,146 @@ +from typing import Self + +import numpy as np +from numba import njit +from numpy.typing import ArrayLike, NDArray + +from ._ins import _dhda_head, _h_head, _signed_smallest_angle +from ._transforms import _angular_matrix_from_quaternion as T +from ._transforms import _euler_from_quaternion +from ._vectorops import _normalize, _skew_symmetric + + +def _state_transition( + dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float +) -> NDArray[np.float64]: + """ + State transition matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + f_b : ndarray, shape (3,) + Specific force measurement in body frame. + w_b : ndarray, shape (3,) + Angular rate measurement (bias corrected) in body frame. + R_nb : ndarray, shape (3, 3) + Rotation matrix from body to navigation frame. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + ndarray, shape (9, 9) + State transition matrix. + """ + phi = np.eye(9) + phi[0:3, 0:3] -= dt * _skew_symmetric(w_b) # NB! update each time step + phi[0:3, 3:6] -= dt * np.eye(3) + phi[3:6, 3:6] -= dt * np.eye(3) / gbc + phi[6:9, 0:3] -= dt * R_nb @ _skew_symmetric(f_b) # NB! update each time step + return phi + + +@njit # type: ignore[misc] +def _update_state_transition( + phi: NDArray[np.float64], + dt: float, + f_b: NDArray[np.float64], + w_b: NDArray[np.float64], + R_nb: NDArray[np.float64], +) -> None: + """ + Update the state transition matrix in place. + + Parameters + ---------- + phi : ndarray, shape (9, 9) + State transition matrix to be updated in place. + dt : float + Time step. + f_b : ndarray, shape (3,) + Specific force measurement in body frame. + w_b : ndarray, shape (3,) + Angular rate measurement (bias corrected) in body frame. + R_nb : ndarray, shape (3, 3) + Rotation matrix from body to navigation frame. + """ + wx, wy, wz = w_b + fx, fy, fz = f_b + + r00, r01, r02 = R_nb[0] + r10, r11, r12 = R_nb[1] + r20, r21, r22 = R_nb[2] + + # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b) + phi[0, 1] = dt * wz + phi[0, 2] = -dt * wy + phi[1, 0] = -dt * wz + phi[1, 2] = dt * wx + phi[2, 0] = dt * wy + phi[2, 1] = -dt * wx + + # phi[6:9, 0:3] = -dt * R_nb @ S(f_b) + phi[6, 0] = -dt * (fz * r01 - fy * r02) + phi[7, 0] = -dt * (fz * r11 - fy * r12) + phi[8, 0] = -dt * (fz * r21 - fy * r22) + phi[6, 1] = -dt * (-fz * r00 + fx * r02) + phi[7, 1] = -dt * (-fz * r10 + fx * r12) + phi[8, 1] = -dt * (-fz * r20 + fx * r22) + phi[6, 2] = -dt * (fy * r00 - fx * r01) + phi[7, 2] = -dt * (fy * r10 - fx * r11) + phi[8, 2] = -dt * (fy * r20 - fx * r21) + + +def _process_noise_cov( + dt: float, arw: float, gbs: float, gbc: float +) -> NDArray[np.float64]: + """ + Process noise covariance matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + arw : float + Angular random walk (gyroscope noise density) in rad/√Hz. + gbs : float + Gyro bias stability (bias instability) in rad/s. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + Q : ndarray, shape (6, 6) + Process noise covariance matrix. + """ + Q = np.zeros((6, 6)) + Q[0:3, 0:3] = dt * arw**2 * np.eye(3) + Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + return Q + + +def _measurement_matrix( + q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] +) -> NDArray[np.float64]: + """ + Measurement matrix. + + Parameters + ---------- + q_nb : ndarray, shape (4,) + Unit quaternion. + vg_b : ndarray, shape (3,) + Gravity reference unit vector expressed in the body frame. + + Returns + ------- + ndarray, shape (4, 6) + Linearized measurement matrix. + """ + dhdx = np.zeros((4, 6)) + dhdx[0:3, 0:3] = _skew_symmetric(vg_b) # gravity ref vector + dhdx[3:4, 0:3] = _dhda_head(q_nb) # heading + return dhdx + From 49d98f715a85b88b9913372a91a18e5bdbeca394 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 08:54:57 +0100 Subject: [PATCH 16/86] reorder vel first --- src/smsfusion/_v2b.py | 69 ++++++++++++++++++++++++------------------- 1 file changed, 39 insertions(+), 30 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index dbce5fe6..09e527b7 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -10,6 +10,12 @@ from ._vectorops import _normalize, _skew_symmetric +ATT_IDX = slice(0, 3) +BG_IDX = slice(3, 6) +VEL_IDX = slice(6, 9) + + + def _state_transition( dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float ) -> NDArray[np.float64]: @@ -35,10 +41,10 @@ def _state_transition( State transition matrix. """ phi = np.eye(9) - phi[0:3, 0:3] -= dt * _skew_symmetric(w_b) # NB! update each time step - phi[0:3, 3:6] -= dt * np.eye(3) - phi[3:6, 3:6] -= dt * np.eye(3) / gbc - phi[6:9, 0:3] -= dt * R_nb @ _skew_symmetric(f_b) # NB! update each time step + phi[VEL_IDX, ATT_IDX] -= dt * R_nb @ _skew_symmetric(f_b) # NB! update each time step + phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b) # NB! update each time step + phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) + phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc return phi @@ -73,28 +79,28 @@ def _update_state_transition( r10, r11, r12 = R_nb[1] r20, r21, r22 = R_nb[2] - # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b) - phi[0, 1] = dt * wz - phi[0, 2] = -dt * wy - phi[1, 0] = -dt * wz - phi[1, 2] = dt * wx - phi[2, 0] = dt * wy - phi[2, 1] = -dt * wx - - # phi[6:9, 0:3] = -dt * R_nb @ S(f_b) - phi[6, 0] = -dt * (fz * r01 - fy * r02) - phi[7, 0] = -dt * (fz * r11 - fy * r12) - phi[8, 0] = -dt * (fz * r21 - fy * r22) - phi[6, 1] = -dt * (-fz * r00 + fx * r02) - phi[7, 1] = -dt * (-fz * r10 + fx * r12) - phi[8, 1] = -dt * (-fz * r20 + fx * r22) - phi[6, 2] = -dt * (fy * r00 - fx * r01) - phi[7, 2] = -dt * (fy * r10 - fx * r11) - phi[8, 2] = -dt * (fy * r20 - fx * r21) + # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b) + phi[3, 4] = dt * wz + phi[3, 5] = -dt * wy + phi[4, 3] = -dt * wz + phi[4, 5] = dt * wx + phi[5, 3] = dt * wy + phi[5, 4] = -dt * wx + + # phi[0:3, 3:6] = -dt * R_nb @ S(f_b) + phi[0, 3] = -dt * (fz * r01 - fy * r02) + phi[1, 3] = -dt * (fz * r11 - fy * r12) + phi[2, 3] = -dt * (fz * r21 - fy * r22) + phi[0, 4] = -dt * (-fz * r00 + fx * r02) + phi[1, 4] = -dt * (-fz * r10 + fx * r12) + phi[2, 4] = -dt * (-fz * r20 + fx * r22) + phi[0, 5] = -dt * (fy * r00 - fx * r01) + phi[1, 5] = -dt * (fy * r10 - fx * r11) + phi[2, 5] = -dt * (fy * r20 - fx * r21) def _process_noise_cov( - dt: float, arw: float, gbs: float, gbc: float + dt: float, vrw: float, arw: float, gbs: float, gbc: float ) -> NDArray[np.float64]: """ Process noise covariance matrix. @@ -103,6 +109,8 @@ def _process_noise_cov( ---------- dt : float Time step in seconds. + vrw : float + Velocity random walk (accelerometer noise density) in m/s/√Hz. arw : float Angular random walk (gyroscope noise density) in rad/√Hz. gbs : float @@ -112,12 +120,13 @@ def _process_noise_cov( Returns ------- - Q : ndarray, shape (6, 6) + Q : ndarray, shape (9, 9) Process noise covariance matrix. """ - Q = np.zeros((6, 6)) - Q[0:3, 0:3] = dt * arw**2 * np.eye(3) - Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + Q = np.zeros((9, 9)) + Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3) + Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) + Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) return Q @@ -139,8 +148,8 @@ def _measurement_matrix( ndarray, shape (4, 6) Linearized measurement matrix. """ - dhdx = np.zeros((4, 6)) - dhdx[0:3, 0:3] = _skew_symmetric(vg_b) # gravity ref vector - dhdx[3:4, 0:3] = _dhda_head(q_nb) # heading + dhdx = np.zeros((4, 9)) + dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b) # gravity ref vector + dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading return dhdx From 77e1cdd844a28e6ffb9b94a283bc1e1d714cdd40 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:41:35 +0100 Subject: [PATCH 17/86] some changes --- src/smsfusion/_v2b.py | 337 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 330 insertions(+), 7 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 09e527b7..fb7303a5 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -6,9 +6,10 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion as T -from ._transforms import _euler_from_quaternion +from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion from ._vectorops import _normalize, _skew_symmetric +from ._v2 import _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, _correct_quat_with_gibbs2 ATT_IDX = slice(0, 3) BG_IDX = slice(3, 6) @@ -130,9 +131,7 @@ def _process_noise_cov( return Q -def _measurement_matrix( - q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] -) -> NDArray[np.float64]: +def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: """ Measurement matrix. @@ -140,8 +139,6 @@ def _measurement_matrix( ---------- q_nb : ndarray, shape (4,) Unit quaternion. - vg_b : ndarray, shape (3,) - Gravity reference unit vector expressed in the body frame. Returns ------- @@ -149,7 +146,333 @@ def _measurement_matrix( Linearized measurement matrix. """ dhdx = np.zeros((4, 9)) - dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b) # gravity ref vector + dhdx[0:3, VEL_IDX] = np.eye(3) # velocity dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading return dhdx + +class AHRSv2b: + """ + Attitude and Heading Reference System (AHRS) using a multiplicative extended + Kalman filter (MEKF). + + Parameters + ---------- + fs : float + Sampling rate in Hz. + v_n : array_like, shape (3,), optional + Initial velocity estimate in m/s. + q_nb : Attitude or array_like, shape (4,), optional + Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults + to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). + bg_b : array_like, shape (3,), optional + Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. + w_b : array_like, shape (3,), optional + Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame. + Defaults to zero angular rate (stationary). + P : array_like, shape (6, 6), optional + Initial (a priori) estimate of the error covariance matrix, **P**. If not + given, a small diagonal matrix will be used. + acc_noise_density : float, optional + Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to + 0.0007 (SMS Motion 2 noise level). + gyro_noise_density : float, optional + Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to + 0.00005 (SMS Motion 2 noise level). + gyro_bias_stability : float, optional + Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). + gyro_bias_corr_time : float, optional + Gyroscope bias correlation time in seconds. Defaults to 50.0 s. + g : float, default 9.80665 + The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665 + m/s^2. + nav_frame : {'NED', 'ENU'}, default 'NED' + Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) + (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom + will be expressed relative to this frame. + + """ + + _I: NDArray[np.float64] = np.eye(9) + + def __init__( + self, + fs: float, + v_n: ArrayLike = (0.0, 0.0, 0.0), + a_n: ArrayLike = (0.0, 0.0, 0.0), + q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), + bg_b: ArrayLike = (0.0, 0.0, 0.0), + w_b: ArrayLike = (0.0, 0.0, 0.0), + P: ArrayLike = 1e-6 * np.eye(6), + acc_noise_density: float = 0.0007, + gyro_noise_density: float = 0.0001, + gyro_bias_stability: float = 0.00005, + gyro_bias_corr_time: float = 50.0, + g: float = 9.80665, + nav_frame: str = "NED", + ) -> None: + self._fs = fs + self._dt = 1.0 / fs + self._g = g + self._nav_frame = nav_frame.lower() + + if self._nav_frame == "ned": + self._g_n = np.array([0.0, 0.0, g]) + elif self._nav_frame == "enu": + self._g_n = np.array([0.0, 0.0, -g]) + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + + # IMU noise parameters + self._vrw = acc_noise_density # velocity random walk + self._arw = gyro_noise_density # angular random walk + self._gbs = gyro_bias_stability # gyro bias stability + self._gbc = gyro_bias_corr_time # gyro bias correlation time + + # State and covariance estimates + self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() + self._a_n = np.asarray_chkfinite(a_n).reshape(3).copy() + self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() + self._R_nb = _rot_matrix_from_quaternion(self._q_nb) + self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() + self._f_b = self._R_nb.T @ (self._a_n - self._g_n) + self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy() + self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() + self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() + self._dx = np.zeros(6) + + # Discrete state-space model + self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc) + self._Q = _process_noise_cov(self._dt, self._vrw, self._arw, self._gbs, self._gbc) + self._dhdx = _measurement_matrix(self._q_nb) + + def quaternion(self) -> NDArray[np.float64]: + """ + Attitude expressed as a unit quaternion. + """ + return self._q_nb.copy() + + def euler(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Attitude expressed as Euler angles (roll, pitch, yaw). + + Parameters + ---------- + degrees : bool, default False + Whether to return the Euler angles in degrees or radians. + + Returns + ------- + numpy.ndarray, shape (3,) + Euler angles (roll, pitch, yaw). + """ + + theta = _euler_from_quaternion(self._q_nb) + + if degrees: + theta = (180.0 / np.pi) * theta + + return theta + + def bias_gyro(self, degrees=False) -> NDArray[np.float64]: + """ + Gyroscope bias estimate (rad/s) expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the bias in deg/s or rad/s. Defaults to rad/s. + """ + bg_b = self._bg_b.copy() + if degrees: + bg_b = (180.0 / np.pi) * bg_b + return bg_b + + def angular_rate(self, degrees=False) -> NDArray[np.float64]: + """ + Bias corrected angular rate measurement expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. + """ + w_b = self._w_b.copy() + if degrees: + w_b = (180.0 / np.pi) * w_b + return w_b + + @property + def P(self) -> NDArray[np.float64]: + """ + Copy of the error covariance matrix estimate. + """ + return self._P.copy() + + # def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: + # """ + # Gravity reference vector part of the measurement matrix, shape (3, 6). + # """ + # self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) + # return self._dhdx[0:3] + + def _dhdx_vel(self) -> NDArray[np.float64]: + """ + Velocity part of the measurement matrix, shape (3, 6). + """ + return self._dhdx[0:3] + + def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Heading (yaw angle) part of the measurement matrix, shape (6,). + """ + self._dhdx[3:4, 0:3] = _dhda_head(q_nb) + return self._dhdx[3] + + def _reset(self) -> None: + """ + Reset state. + """ + + if not self._dx.any(): + return + + _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) + self._v_n[:] += self._dx[VEL_IDX] + self._bg_b[:] += self._dx[BG_IDX] + self._dx[:] = 0.0 + + # def _aiding_update_gref( + # self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None + # ) -> None: + # """ + # Update with gravity reference vector aiding measurement. + # """ + + # if vg_meas is None: + # return None + + # if vg_var is None: + # raise ValueError("'vg_var' not provided.") + + # vg_b = _vg_b(self._q_nb, self._nz2vg) + # dz = vg_meas - vg_b + # dhdx = self._dhdx_gref(vg_b) + # _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) + + def _aiding_update_vel( + self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None + ) -> None: + """ + Update with velocity aiding measurement. + """ + + if vel_meas is None: + return None + + if vel_var is None: + raise ValueError("'vg_var' not provided.") + + dz = vel_meas - self._v_n + dhdx = self._dhdx_vel() + _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) + + def _aiding_update_head( + self, head_meas: float | None, head_var: float | None, head_degrees: bool + ) -> None: + """ + Update with heading aiding measurement. + """ + + if head_meas is None: + return None + + if head_var is None: + raise ValueError("'head_var' not provided.") + + if head_degrees: + head_meas = (np.pi / 180.0) * head_meas + head_var = (np.pi / 180.0) ** 2 * head_var + + dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) + dhdx = self._dhdx_yaw(self._q_nb) + _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) + + def _project_ahead(self) -> None: + """ + Project state and covariance estimates ahead. + """ + + # Velocity (dead reckoning) + self._v_n[:] += self._dt * self._a_n + + # Attitude (dead reckoning) + self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b + self._q_nb[:] = _normalize(self._q_nb) + + # Covariance + self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) + + def update( + self, + f: ArrayLike, + w: ArrayLike, + degrees: bool = False, + head: float | None = None, + head_var: float | None = None, + head_degrees: bool = False, + vel: ArrayLike | None = (0.0, 0.0, 0.0), + vel_var: ArrayLike | None = (100.0, 100.0, 100.0), + ) -> Self: + """ + Update state estimates with IMU and aiding measurements. + + Parameters + ---------- + f : array_like, shape (3,) + Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) + in m/s^2. + w : array_like, shape (3,) + Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See + ``degrees`` parameter for units. + degrees : bool, optional + Specifies whether the unit of the rotation rate, ``w``, is deg/s or + rad/s. Defaults to rad/s. + head : float, optional + Heading measurement. I.e., the yaw angle of the 'body' frame relative to the + assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. + If ``None``, compass aiding is not used. See ``head_degrees`` for units. + head_var : float, optional + Variance of heading measurement noise. Units must be compatible with ``head``. + See ``head_degrees`` for units. Required for ``head``. + head_degrees : bool, default False + Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, + or radians and radians^2. Default is in radians and radians^2. + + Returns + ------- + AHRS + A reference to the instance itself after the update. + """ + + if degrees: + w = np.radians(w) + + # Project (a priori) state and covariance estimates ahead + self._project_ahead() + + # Update (a posteriori) state and covariance estimates with aiding measurements + self._aiding_update_vel(vel, vel_var) + self._aiding_update_head(head, head_var, head_degrees) + + # Reset state + self._reset() + + # Update model + self._f_b[:] = f + self._w_b[:] = w - self._bg_b + self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) + self._a_n[:] = self._R_nb @ self._f_b + self._g_n + _update_state_transition(self._phi, self._f_b, self._w_b, self._R_nb) + + return self From ca6b57c05f49121194492b627f1292cb8867d110 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:43:06 +0100 Subject: [PATCH 18/86] style --- src/smsfusion/_v2b.py | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index fb7303a5..0924aa34 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -7,18 +7,25 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion as T from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from ._v2 import ( + _correct_quat_with_gibbs2, + _kalman_update_scalar, + _kalman_update_sequential, + _project_cov_ahead, +) from ._vectorops import _normalize, _skew_symmetric -from ._v2 import _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, _correct_quat_with_gibbs2 - ATT_IDX = slice(0, 3) BG_IDX = slice(3, 6) VEL_IDX = slice(6, 9) - def _state_transition( - dt: float, f_b: NDArray[np.float64], w_b: NDArray[np.float64], R_nb: NDArray[np.float64], gbc: float + dt: float, + f_b: NDArray[np.float64], + w_b: NDArray[np.float64], + R_nb: NDArray[np.float64], + gbc: float, ) -> NDArray[np.float64]: """ State transition matrix. @@ -42,7 +49,9 @@ def _state_transition( State transition matrix. """ phi = np.eye(9) - phi[VEL_IDX, ATT_IDX] -= dt * R_nb @ _skew_symmetric(f_b) # NB! update each time step + phi[VEL_IDX, ATT_IDX] -= ( + dt * R_nb @ _skew_symmetric(f_b) + ) # NB! update each time step phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b) # NB! update each time step phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc @@ -243,7 +252,9 @@ def __init__( # Discrete state-space model self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc) - self._Q = _process_noise_cov(self._dt, self._vrw, self._arw, self._gbs, self._gbc) + self._Q = _process_noise_cov( + self._dt, self._vrw, self._arw, self._gbs, self._gbc + ) self._dhdx = _measurement_matrix(self._q_nb) def quaternion(self) -> NDArray[np.float64]: From a23dd45946cc23ab8dacd59a1095ca924debaf46 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:45:44 +0100 Subject: [PATCH 19/86] some fixces --- src/smsfusion/__init__.py | 2 ++ src/smsfusion/_v2b.py | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 0b202de0..978dfdc1 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -4,10 +4,12 @@ from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler from ._v2 import AHRSv2a +from ._v2b import AHRSv2b __all__ = [ "AHRS", "AHRSv2a", + "AHRSv2b", "AidedINS", "benchmark", "constants", diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 0924aa34..8f7bcc3c 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -251,7 +251,9 @@ def __init__( self._dx = np.zeros(6) # Discrete state-space model - self._phi = _state_transition(self._dt, self._f_b, self._w_b, self._gbc) + self._phi = _state_transition( + self._dt, self._f_b, self._w_b, self._R_nb, self._gbc + ) self._Q = _process_noise_cov( self._dt, self._vrw, self._arw, self._gbs, self._gbc ) From b9f1fee315f1c93a3286a4800ba47dad3ddbac15 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:46:57 +0100 Subject: [PATCH 20/86] more fixes --- src/smsfusion/_v2b.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 8f7bcc3c..10cb9c8f 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -212,7 +212,7 @@ def __init__( q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), bg_b: ArrayLike = (0.0, 0.0, 0.0), w_b: ArrayLike = (0.0, 0.0, 0.0), - P: ArrayLike = 1e-6 * np.eye(6), + P: ArrayLike = 1e-6 * np.eye(9), acc_noise_density: float = 0.0007, gyro_noise_density: float = 0.0001, gyro_bias_stability: float = 0.00005, @@ -247,8 +247,8 @@ def __init__( self._f_b = self._R_nb.T @ (self._a_n - self._g_n) self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy() self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() - self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() - self._dx = np.zeros(6) + self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() + self._dx = np.zeros(9) # Discrete state-space model self._phi = _state_transition( From 784c23e31bf4a90a448ec36cd8dd19f16ebc4c9b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:47:43 +0100 Subject: [PATCH 21/86] another fix --- src/smsfusion/_v2b.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 10cb9c8f..880561bf 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -388,7 +388,7 @@ def _aiding_update_vel( dz = vel_meas - self._v_n dhdx = self._dhdx_vel() - _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) + _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I) def _aiding_update_head( self, head_meas: float | None, head_var: float | None, head_degrees: bool From 9adf6dfaf1f8e74dfe40126fba32a58f87804a03 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 09:48:56 +0100 Subject: [PATCH 22/86] small fix --- src/smsfusion/_v2b.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 880561bf..40e53811 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -486,6 +486,6 @@ def update( self._w_b[:] = w - self._bg_b self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) self._a_n[:] = self._R_nb @ self._f_b + self._g_n - _update_state_transition(self._phi, self._f_b, self._w_b, self._R_nb) + _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb) return self From 36df90a375dcd253cc490ce18544767373981772 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:02:44 +0100 Subject: [PATCH 23/86] bugfix --- src/smsfusion/_v2b.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 40e53811..59050b1d 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -15,9 +15,10 @@ ) from ._vectorops import _normalize, _skew_symmetric -ATT_IDX = slice(0, 3) -BG_IDX = slice(3, 6) -VEL_IDX = slice(6, 9) + +VEL_IDX = slice(0, 3) +ATT_IDX = slice(3, 6) +BG_IDX = slice(6, 9) def _state_transition( @@ -339,7 +340,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: """ Heading (yaw angle) part of the measurement matrix, shape (6,). """ - self._dhdx[3:4, 0:3] = _dhda_head(q_nb) + self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) return self._dhdx[3] def _reset(self) -> None: From 0bee53b4467f6cb19c6c4af3979f419daa313120 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:03:41 +0100 Subject: [PATCH 24/86] delete commented out code --- src/smsfusion/_v2b.py | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 59050b1d..8bb43991 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -323,13 +323,6 @@ def P(self) -> NDArray[np.float64]: """ return self._P.copy() - # def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: - # """ - # Gravity reference vector part of the measurement matrix, shape (3, 6). - # """ - # self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) - # return self._dhdx[0:3] - def _dhdx_vel(self) -> NDArray[np.float64]: """ Velocity part of the measurement matrix, shape (3, 6). @@ -356,24 +349,6 @@ def _reset(self) -> None: self._bg_b[:] += self._dx[BG_IDX] self._dx[:] = 0.0 - # def _aiding_update_gref( - # self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None - # ) -> None: - # """ - # Update with gravity reference vector aiding measurement. - # """ - - # if vg_meas is None: - # return None - - # if vg_var is None: - # raise ValueError("'vg_var' not provided.") - - # vg_b = _vg_b(self._q_nb, self._nz2vg) - # dz = vg_meas - vg_b - # dhdx = self._dhdx_gref(vg_b) - # _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) - def _aiding_update_vel( self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None ) -> None: From 347a3aeb7f4866d5f8e897659b9cc62a312100e6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:23:00 +0100 Subject: [PATCH 25/86] common module --- src/smsfusion/_v2.py | 226 +----------------------------------- src/smsfusion/_v2b.py | 2 +- src/smsfusion/_v2common.py | 230 +++++++++++++++++++++++++++++++++++++ 3 files changed, 232 insertions(+), 226 deletions(-) create mode 100644 src/smsfusion/_v2common.py diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index f1db0be9..427999fe 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -8,40 +8,7 @@ from ._transforms import _angular_matrix_from_quaternion as T from ._transforms import _euler_from_quaternion from ._vectorops import _normalize, _skew_symmetric - - -def _nz2vg(nav_frame: str) -> float: - """ - Gravity direction along the navigation frame's z-axis. - """ - if nav_frame == "ned": - return 1.0 - elif nav_frame == "enu": - return -1.0 - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - - -@njit # type: ignore[misc] -def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: - """ - Gravity reference vector expressed in the body frame, computed from a unit quaternion. - - Parameters - ---------- - q_nb : numpy.ndarray, shape (4,) - Unit quaternion. - nz2vg : float - Gravity direction along the navigation frame's z-axis. Should be +1 for - NED and -1 for ENU. - """ - qw, qx, qy, qz = q_nb - - x = 2.0 * (qx * qz - qw * qy) - y = 2.0 * (qy * qz + qw * qx) - z = 1.0 - 2.0 * (qx**2 + qy**2) - - return nz2vg * np.array([x, y, z]) +from ._v2common import _nz2vg, _vg_b, _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead def _state_transition( @@ -150,197 +117,6 @@ def _measurement_matrix( return dhdx -@njit # type: ignore[misc] -def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: - """ - Corrects a unit quaternion, q, with a small attitude error, da, parameterized - as a scaled (2x) Gibbs vector: - - q = q ⊗ dq(da) - - As described in ref [1]_, this correction can be simplified by doing it in two - steps: first a correction, followed by renormalization. The scaling factor becomes - obsolete due to the renormalization step. - - Parameters - ---------- - q : ndarray, shape (4,) - Unit quaternion [qw, qx, qy, qz] (modified in place). - da : ndarray, shape (3,) - Small attitude error parameterized as a scaled (2x) Gibbs vector. - - References - ---------- - Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination - and Control, Eq. (6.27)-(6.28). - """ - - qw, qx, qy, qz = q - dax, day, daz = da - - q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) - q[1] += 0.5 * (qw * dax + qy * daz - qz * day) - q[2] += 0.5 * (qw * day - qx * daz + qz * dax) - q[3] += 0.5 * (qw * daz + qx * day - qy * dax) - q[:] = _normalize(q) - - -@njit # type: ignore[misc] -def _kalman_gain( - P: NDArray[np.float64], h: NDArray[np.float64], r: float -) -> NDArray[np.float64]: - """ - Compute the Kalman gain for a scalar measurement. - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix. - h : ndarray, shape (n,) - Measurement matrix (row vector). - r : float - Scalar measurement noise variance. - - Returns - ------- - ndarray, shape (n,) - Kalman gain vector. - """ - - # Innovation covariance (inverse) - Ph = np.dot(P, h) - s_inv = 1.0 / (np.dot(h, Ph) + r) - - # Kalman gain - k = Ph * s_inv - - return k - - -@njit # type: ignore[misc] -def _covariance_update( - P: NDArray[np.float64], - k: NDArray[np.float64], - h: NDArray[np.float64], - r: float, - I_: NDArray[np.float64], -) -> None: - """ - Compute the updated state error covariance matrix estimate (Joseph form). - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - k : ndarray, shape (n,) - Kalman gain vector. - h : ndarray, shape (n,) - Measurement matrix (row vector). - r : float - Scalar measurement noise variance. - I_ : ndarray, shape (n, n) - Identity matrix. - """ - A = I_ - np.outer(k, h) - P = A @ P @ A.T + r * np.outer(k, k) - return P - - -@njit # type: ignore[misc] -def _kalman_update_scalar( - x: NDArray[np.float64], - P: NDArray[np.float64], - z: float, - r: float, - h: NDArray[np.float64], - I_: NDArray[np.float64], -) -> None: - """ - Scalar Kalman filter measurement update. - - Parameters - ---------- - x : ndarray, shape (n,) - State estimate to be updated in place. - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - z : float - Scalar measurement. - r : float - Scalar measurement noise variance. - h : ndarray, shape (n,) - Measurement matrix (row vector). - I_ : ndarray, shape (n, n) - Identity matrix. - """ - - # Kalman gain - k = _kalman_gain(P, h, r) - - # Updated (a posteriori) state estimate - x[:] += k * (z - np.dot(h, x)) - - # Updated (a posteriori) covariance estimate (Joseph form) - P[:, :] = _covariance_update(P, k, h, r, I_) - - -@njit # type: ignore[misc] -def _kalman_update_sequential( - x: NDArray[np.float64], - P: NDArray[np.float64], - z: NDArray[np.float64], - var: NDArray[np.float64], - H: NDArray[np.float64], - I_: NDArray[np.float64], -) -> None: - """ - Sequential (one-at-a-time) Kalman filter measurement update. - - Parameters - ---------- - x : ndarray, shape (n,) - State estimate to be updated in place. - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - z : ndarray, shape (m,) - Measurement vector. - var : ndarray, shape (m,) - Measurement noise variances corresponding to each scalar measurement. - H : ndarray, shape (m, n) - Measurement matrix where each row corresponds to a scalar measurement model. - I_ : ndarray, shape (n, n) - Identity matrix. - """ - m = z.shape[0] - for i in range(m): - _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) - - -@njit # type: ignore[misc] -def _project_cov_ahead( - P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] -) -> None: - """ - Project the error covariance matrix estimate ahead. - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix to be projected ahead. - phi : ndarray, shape (n, n) - State transition matrix. - Q : ndarray, shape (n, n) - Process noise covariance matrix. - - Returns - ------- - ndarray, shape (n, n) - Projected error covariance matrix estimate. - """ - P = phi @ P @ phi.T + Q - return P - - class AHRSv2a: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index 8bb43991..b7e9c7e8 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -7,7 +7,7 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion as T from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion -from ._v2 import ( +from ._v2common import ( _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py new file mode 100644 index 00000000..ebd01a02 --- /dev/null +++ b/src/smsfusion/_v2common.py @@ -0,0 +1,230 @@ +import numpy as np +from numba import njit +from numpy.typing import NDArray + +from ._vectorops import _normalize + + +def _nz2vg(nav_frame: str) -> float: + """ + Gravity direction along the navigation frame's z-axis. + """ + if nav_frame == "ned": + return 1.0 + elif nav_frame == "enu": + return -1.0 + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + + +@njit # type: ignore[misc] +def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: + """ + Gravity reference vector expressed in the body frame, computed from a unit quaternion. + + Parameters + ---------- + q_nb : numpy.ndarray, shape (4,) + Unit quaternion. + nz2vg : float + Gravity direction along the navigation frame's z-axis. Should be +1 for + NED and -1 for ENU. + """ + qw, qx, qy, qz = q_nb + + x = 2.0 * (qx * qz - qw * qy) + y = 2.0 * (qy * qz + qw * qx) + z = 1.0 - 2.0 * (qx**2 + qy**2) + + return nz2vg * np.array([x, y, z]) + + +@njit # type: ignore[misc] +def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: + """ + Corrects a unit quaternion, q, with a small attitude error, da, parameterized + as a scaled (2x) Gibbs vector: + + q = q ⊗ dq(da) + + As described in ref [1]_, this correction can be simplified by doing it in two + steps: first a correction, followed by renormalization. The scaling factor becomes + obsolete due to the renormalization step. + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion [qw, qx, qy, qz] (modified in place). + da : ndarray, shape (3,) + Small attitude error parameterized as a scaled (2x) Gibbs vector. + + References + ---------- + Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination + and Control, Eq. (6.27)-(6.28). + """ + + qw, qx, qy, qz = q + dax, day, daz = da + + q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) + q[1] += 0.5 * (qw * dax + qy * daz - qz * day) + q[2] += 0.5 * (qw * day - qx * daz + qz * dax) + q[3] += 0.5 * (qw * daz + qx * day - qy * dax) + q[:] = _normalize(q) + + +@njit # type: ignore[misc] +def _kalman_gain( + P: NDArray[np.float64], h: NDArray[np.float64], r: float +) -> NDArray[np.float64]: + """ + Compute the Kalman gain for a scalar measurement. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + + Returns + ------- + ndarray, shape (n,) + Kalman gain vector. + """ + + # Innovation covariance (inverse) + Ph = np.dot(P, h) + s_inv = 1.0 / (np.dot(h, Ph) + r) + + # Kalman gain + k = Ph * s_inv + + return k + + +@njit # type: ignore[misc] +def _covariance_update( + P: NDArray[np.float64], + k: NDArray[np.float64], + h: NDArray[np.float64], + r: float, + I_: NDArray[np.float64], +) -> None: + """ + Compute the updated state error covariance matrix estimate (Joseph form). + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + k : ndarray, shape (n,) + Kalman gain vector. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + A = I_ - np.outer(k, h) + P = A @ P @ A.T + r * np.outer(k, k) + return P + + +@njit # type: ignore[misc] +def _kalman_update_scalar( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: float, + r: float, + h: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Scalar Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : float + Scalar measurement. + r : float + Scalar measurement noise variance. + h : ndarray, shape (n,) + Measurement matrix (row vector). + I_ : ndarray, shape (n, n) + Identity matrix. + """ + + # Kalman gain + k = _kalman_gain(P, h, r) + + # Updated (a posteriori) state estimate + x[:] += k * (z - np.dot(h, x)) + + # Updated (a posteriori) covariance estimate (Joseph form) + P[:, :] = _covariance_update(P, k, h, r, I_) + + +@njit # type: ignore[misc] +def _kalman_update_sequential( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: NDArray[np.float64], + var: NDArray[np.float64], + H: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Sequential (one-at-a-time) Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : ndarray, shape (m,) + Measurement vector. + var : ndarray, shape (m,) + Measurement noise variances corresponding to each scalar measurement. + H : ndarray, shape (m, n) + Measurement matrix where each row corresponds to a scalar measurement model. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + m = z.shape[0] + for i in range(m): + _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) + + +@njit # type: ignore[misc] +def _project_cov_ahead( + P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] +) -> None: + """ + Project the error covariance matrix estimate ahead. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be projected ahead. + phi : ndarray, shape (n, n) + State transition matrix. + Q : ndarray, shape (n, n) + Process noise covariance matrix. + + Returns + ------- + ndarray, shape (n, n) + Projected error covariance matrix estimate. + """ + P = phi @ P @ phi.T + Q + return P From cf5b82060044f3e8bed011b730f7d3310461820b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:33:48 +0100 Subject: [PATCH 26/86] v2c --- src/smsfusion/_v2c.py | 462 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 462 insertions(+) create mode 100644 src/smsfusion/_v2c.py diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py new file mode 100644 index 00000000..b5477532 --- /dev/null +++ b/src/smsfusion/_v2c.py @@ -0,0 +1,462 @@ +from typing import Self + +import numpy as np +from numba import njit +from numpy.typing import ArrayLike, NDArray + +from ._ins import _dhda_head, _h_head, _signed_smallest_angle +from ._transforms import _angular_matrix_from_quaternion as T +from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from ._v2common import ( + _correct_quat_with_gibbs2, + _kalman_update_scalar, + _kalman_update_sequential, + _project_cov_ahead, +) +from ._vectorops import _normalize, _skew_symmetric + + +VEL_IDX = slice(0, 3) +ATT_IDX = slice(3, 6) +BG_IDX = slice(6, 9) + + +def _state_transition( + dt: float, + dvel: NDArray[np.float64], + dtheta: NDArray[np.float64], + R_nb: NDArray[np.float64], + gbc: float, +) -> NDArray[np.float64]: + """ + State transition matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + dvel : ndarray, shape (3,) + Velocity change vector (sculling integral). + dtheta : ndarray, shape (3,) + Attitude change vector (coning integral). + R_nb : ndarray, shape (3, 3) + Rotation matrix from body to navigation frame. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + ndarray, shape (9, 9) + State transition matrix. + """ + phi = np.eye(9) + phi[VEL_IDX, ATT_IDX] -= R_nb @ _skew_symmetric(dvel) # NB! update each time step + phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta) # NB! update each time step + phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) + phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc + return phi + + +@njit # type: ignore[misc] +def _update_state_transition( + phi: NDArray[np.float64], + dvel: NDArray[np.float64], + dtheta: NDArray[np.float64], + R_nb: NDArray[np.float64], +) -> None: + """ + Update the state transition matrix in place. + + Parameters + ---------- + phi : ndarray, shape (9, 9) + State transition matrix to be updated in place. + dvel : ndarray, shape (3,) + Velocity change vector (sculling integral). + dtheta : ndarray, shape (3,) + Attitude change vector (coning integral). + R_nb : ndarray, shape (3, 3) + Rotation matrix from body to navigation frame. + """ + dtx, dty, dtz = dtheta + dvx, dvy, dvz = dvel + + r00, r01, r02 = R_nb[0] + r10, r11, r12 = R_nb[1] + r20, r21, r22 = R_nb[2] + + # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b) + phi[3, 4] = dtz + phi[3, 5] = -dty + phi[4, 3] = -dtz + phi[4, 5] = dtx + phi[5, 3] = dty + phi[5, 4] = -dtx + + # phi[0:3, 3:6] = -dt * R_nb @ S(f_b) + phi[0, 3] = -(dvz * r01 - dvy * r02) + phi[1, 3] = -(dvz * r11 - dvy * r12) + phi[2, 3] = -(dvz * r21 - dvy * r22) + phi[0, 4] = -(-dvz * r00 + dvx * r02) + phi[1, 4] = -(-dvz * r10 + dvx * r12) + phi[2, 4] = -(-dvz * r20 + dvx * r22) + phi[0, 5] = -(dvy * r00 - dvx * r01) + phi[1, 5] = -(dvy * r10 - dvx * r11) + phi[2, 5] = -(dvy * r20 - dvx * r21) + + +def _process_noise_cov( + dt: float, vrw: float, arw: float, gbs: float, gbc: float +) -> NDArray[np.float64]: + """ + Process noise covariance matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + vrw : float + Velocity random walk (accelerometer noise density) in m/s/√Hz. + arw : float + Angular random walk (gyroscope noise density) in rad/√Hz. + gbs : float + Gyro bias stability (bias instability) in rad/s. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + Q : ndarray, shape (9, 9) + Process noise covariance matrix. + """ + Q = np.zeros((9, 9)) + Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3) + Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) + Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + return Q + + +def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Measurement matrix. + + Parameters + ---------- + q_nb : ndarray, shape (4,) + Unit quaternion. + + Returns + ------- + ndarray, shape (4, 6) + Linearized measurement matrix. + """ + dhdx = np.zeros((4, 9)) + dhdx[0:3, VEL_IDX] = np.eye(3) # velocity + dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading + return dhdx + + +class AHRSv2c: + """ + Attitude and Heading Reference System (AHRS) using a multiplicative extended + Kalman filter (MEKF). + + Parameters + ---------- + fs : float + Sampling rate in Hz. + v_n : array_like, shape (3,), optional + Initial velocity estimate in m/s. + q_nb : Attitude or array_like, shape (4,), optional + Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults + to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). + bg_b : array_like, shape (3,), optional + Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. + dvel : array_like, shape (3,), optional + Initial velocity change vector (sculling integral). + dtheta : array_like, shape (3,), optional + Initial attitude change vector (coning integral). + P : array_like, shape (6, 6), optional + Initial (a priori) estimate of the error covariance matrix, **P**. If not + given, a small diagonal matrix will be used. + acc_noise_density : float, optional + Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to + 0.0007 (SMS Motion 2 noise level). + gyro_noise_density : float, optional + Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to + 0.00005 (SMS Motion 2 noise level). + gyro_bias_stability : float, optional + Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). + gyro_bias_corr_time : float, optional + Gyroscope bias correlation time in seconds. Defaults to 50.0 s. + g : float, default 9.80665 + The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665 + m/s^2. + nav_frame : {'NED', 'ENU'}, default 'NED' + Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) + (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom + will be expressed relative to this frame. + + """ + + _I: NDArray[np.float64] = np.eye(9) + + def __init__( + self, + fs: float, + v_n: ArrayLike = (0.0, 0.0, 0.0), + q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), + bg_b: ArrayLike = (0.0, 0.0, 0.0), + dvel: ArrayLike = (0.0, 0.0, 0.0), + dtheta: ArrayLike = (0.0, 0.0, 0.0), + P: ArrayLike = 1e-6 * np.eye(9), + acc_noise_density: float = 0.0007, + gyro_noise_density: float = 0.0001, + gyro_bias_stability: float = 0.00005, + gyro_bias_corr_time: float = 50.0, + g: float = 9.80665, + nav_frame: str = "NED", + ) -> None: + self._fs = fs + self._dt = 1.0 / fs + self._g = g + self._nav_frame = nav_frame.lower() + + if self._nav_frame == "ned": + self._g_n = np.array([0.0, 0.0, g]) + elif self._nav_frame == "enu": + self._g_n = np.array([0.0, 0.0, -g]) + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + + # IMU noise parameters + self._vrw = acc_noise_density # velocity random walk + self._arw = gyro_noise_density # angular random walk + self._gbs = gyro_bias_stability # gyro bias stability + self._gbc = gyro_bias_corr_time # gyro bias correlation time + + # State and covariance estimates + self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() + self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() + self._R_nb = _rot_matrix_from_quaternion(self._q_nb) + self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() + self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy() + self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() + self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() + self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() + self._dx = np.zeros(9) + + # Discrete state-space model + self._phi = _state_transition( + self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc + ) + self._Q = _process_noise_cov( + self._dt, self._vrw, self._arw, self._gbs, self._gbc + ) + self._dhdx = _measurement_matrix(self._q_nb) + + def quaternion(self) -> NDArray[np.float64]: + """ + Attitude expressed as a unit quaternion. + """ + return self._q_nb.copy() + + def euler(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Attitude expressed as Euler angles (roll, pitch, yaw). + + Parameters + ---------- + degrees : bool, default False + Whether to return the Euler angles in degrees or radians. + + Returns + ------- + numpy.ndarray, shape (3,) + Euler angles (roll, pitch, yaw). + """ + + theta = _euler_from_quaternion(self._q_nb) + + if degrees: + theta = (180.0 / np.pi) * theta + + return theta + + def bias_gyro(self, degrees=False) -> NDArray[np.float64]: + """ + Gyroscope bias estimate (rad/s) expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the bias in deg/s or rad/s. Defaults to rad/s. + """ + bg_b = self._bg_b.copy() + if degrees: + bg_b = (180.0 / np.pi) * bg_b + return bg_b + + # def angular_rate(self, degrees=False) -> NDArray[np.float64]: + # """ + # Bias corrected angular rate measurement expressed in the body frame. + + # Parameters + # ---------- + # degrees : bool, optional + # Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. + # """ + # w_b = self._w_b.copy() + # if degrees: + # w_b = (180.0 / np.pi) * w_b + # return w_b + + @property + def P(self) -> NDArray[np.float64]: + """ + Copy of the error covariance matrix estimate. + """ + return self._P.copy() + + def _dhdx_vel(self) -> NDArray[np.float64]: + """ + Velocity part of the measurement matrix, shape (3, 6). + """ + return self._dhdx[0:3] + + def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Heading (yaw angle) part of the measurement matrix, shape (6,). + """ + self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) + return self._dhdx[3] + + def _reset(self) -> None: + """ + Reset state. + """ + + if not self._dx.any(): + return + + _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) + self._v_n[:] += self._dx[VEL_IDX] + self._bg_b[:] += self._dx[BG_IDX] + self._dx[:] = 0.0 + + def _aiding_update_vel( + self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None + ) -> None: + """ + Update with velocity aiding measurement. + """ + + if vel_meas is None: + return None + + if vel_var is None: + raise ValueError("'vg_var' not provided.") + + dz = vel_meas - self._v_n + dhdx = self._dhdx_vel() + _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I) + + def _aiding_update_head( + self, head_meas: float | None, head_var: float | None, head_degrees: bool + ) -> None: + """ + Update with heading aiding measurement. + """ + + if head_meas is None: + return None + + if head_var is None: + raise ValueError("'head_var' not provided.") + + if head_degrees: + head_meas = (np.pi / 180.0) * head_meas + head_var = (np.pi / 180.0) ** 2 * head_var + + dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) + dhdx = self._dhdx_yaw(self._q_nb) + _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) + + def _project_ahead(self) -> None: + """ + Project state and covariance estimates ahead. + """ + + # Velocity (dead reckoning) + self._v_n[:] += self._dvel + + # Attitude (dead reckoning) + self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b + self._q_nb[:] = _normalize(self._q_nb) + + # Covariance + self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) + + def update( + self, + f: ArrayLike, + w: ArrayLike, + degrees: bool = False, + head: float | None = None, + head_var: float | None = None, + head_degrees: bool = False, + vel: ArrayLike | None = (0.0, 0.0, 0.0), + vel_var: ArrayLike | None = (100.0, 100.0, 100.0), + ) -> Self: + """ + Update state estimates with IMU and aiding measurements. + + Parameters + ---------- + f : array_like, shape (3,) + Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) + in m/s^2. + w : array_like, shape (3,) + Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See + ``degrees`` parameter for units. + degrees : bool, optional + Specifies whether the unit of the rotation rate, ``w``, is deg/s or + rad/s. Defaults to rad/s. + head : float, optional + Heading measurement. I.e., the yaw angle of the 'body' frame relative to the + assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. + If ``None``, compass aiding is not used. See ``head_degrees`` for units. + head_var : float, optional + Variance of heading measurement noise. Units must be compatible with ``head``. + See ``head_degrees`` for units. Required for ``head``. + head_degrees : bool, default False + Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, + or radians and radians^2. Default is in radians and radians^2. + + Returns + ------- + AHRS + A reference to the instance itself after the update. + """ + + if degrees: + w = np.radians(w) + + # Project (a priori) state and covariance estimates ahead + self._project_ahead() + + # Update (a posteriori) state and covariance estimates with aiding measurements + self._aiding_update_vel(vel, vel_var) + self._aiding_update_head(head, head_var, head_degrees) + + # Reset state + self._reset() + + # Update model + self._f_b[:] = f + self._w_b[:] = w - self._bg_b + self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) + self._a_n[:] = self._R_nb @ self._f_b + self._g_n + _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb) + + return self From 111e9addf1f984280c0032d6235cbbec8fc1ca8c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:39:07 +0100 Subject: [PATCH 27/86] more changes to vc --- src/smsfusion/_v2c.py | 31 ++++++++++----------- src/smsfusion/_v2common.py | 56 +++++++++++++++++++++++++++++++++++++- 2 files changed, 69 insertions(+), 18 deletions(-) diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py index b5477532..d96fed72 100644 --- a/src/smsfusion/_v2c.py +++ b/src/smsfusion/_v2c.py @@ -12,6 +12,7 @@ _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, + _correct_quat_with_rotvec ) from ._vectorops import _normalize, _skew_symmetric @@ -391,16 +392,15 @@ def _project_ahead(self) -> None: self._v_n[:] += self._dvel # Attitude (dead reckoning) - self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b - self._q_nb[:] = _normalize(self._q_nb) + _correct_quat_with_rotvec(self._q_nb, self._dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) def update( self, - f: ArrayLike, - w: ArrayLike, + dvel: ArrayLike, + dtheta: ArrayLike, degrees: bool = False, head: float | None = None, head_var: float | None = None, @@ -413,15 +413,13 @@ def update( Parameters ---------- - f : array_like, shape (3,) - Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) - in m/s^2. - w : array_like, shape (3,) - Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See - ``degrees`` parameter for units. + dvel : array_like, shape (3,), optional + Initial velocity change vector (sculling integral). + dtheta : array_like, shape (3,), optional + Initial attitude change vector (coning integral). degrees : bool, optional - Specifies whether the unit of the rotation rate, ``w``, is deg/s or - rad/s. Defaults to rad/s. + Specifies whether the unit of the attitude change vector, ``dtheta``, + is degrees or radians. Defaults to radians. head : float, optional Heading measurement. I.e., the yaw angle of the 'body' frame relative to the assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. @@ -440,7 +438,7 @@ def update( """ if degrees: - w = np.radians(w) + dtheta = np.radians(dtheta) # Project (a priori) state and covariance estimates ahead self._project_ahead() @@ -453,10 +451,9 @@ def update( self._reset() # Update model - self._f_b[:] = f - self._w_b[:] = w - self._bg_b + self._dvel[:] = dvel + self._dtheta[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - self._a_n[:] = self._R_nb @ self._f_b + self._g_n - _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb) + _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) return self diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py index ebd01a02..e7d6fea5 100644 --- a/src/smsfusion/_v2common.py +++ b/src/smsfusion/_v2common.py @@ -2,7 +2,7 @@ from numba import njit from numpy.typing import NDArray -from ._vectorops import _normalize +from ._vectorops import _normalize, _quaternion_product def _nz2vg(nav_frame: str) -> float: @@ -74,6 +74,60 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - q[:] = _normalize(q) +@njit # type: ignore[misc] +def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Compute the unit quaternion from a rotation vector. + + Parameters + ---------- + r : numpy.ndarray, shape (3,) + Rotation vector (rx, ry, rz). + + Returns + ------- + numpy.ndarray, shape (4,) + Unit quaternion (qw, qx, qy, qz). + """ + # TODO: add reference + + rx, ry, rz = r + + angle2 = rx**2 + ry**2 + rz**2 + + if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) + a = 0.25 * angle2 + c = 1.0 - a / 2.0 + s = 0.5 * (1.0 - a / 6.0) + else: + angle = np.sqrt(angle2) + half_angle = 0.5 * angle + c = np.cos(half_angle) + s = np.sin(half_angle) / angle + + q = np.array([c, s * rx, s * ry, s * rz]) + + return _normalize(q) + + +@njit # type: ignore[misc] +def _correct_quat_with_rotvec(q: NDArray[np.float64], dtheta: NDArray[np.float64]) -> None: + """ + Corrects a unit quaternion, q, with a small attitude change vector, dtheta, + parameterized as a rotation vector: + + q = q ⊗ dq(dtheta) + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion (modified in place). + dtheta : ndarray, shape (3,) + Small attitude change parameterized as a rotation vector. + """ + q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) + + @njit # type: ignore[misc] def _kalman_gain( P: NDArray[np.float64], h: NDArray[np.float64], r: float From b2104ced75395e956c211915275aa9675797a290 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:39:29 +0100 Subject: [PATCH 28/86] style --- src/smsfusion/_v2.py | 9 ++++++++- src/smsfusion/_v2b.py | 1 - src/smsfusion/_v2c.py | 3 +-- src/smsfusion/_v2common.py | 4 +++- 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 427999fe..c402417f 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -7,8 +7,15 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _angular_matrix_from_quaternion as T from ._transforms import _euler_from_quaternion +from ._v2common import ( + _correct_quat_with_gibbs2, + _kalman_update_scalar, + _kalman_update_sequential, + _nz2vg, + _project_cov_ahead, + _vg_b, +) from ._vectorops import _normalize, _skew_symmetric -from ._v2common import _nz2vg, _vg_b, _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead def _state_transition( diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2b.py index b7e9c7e8..6352a1e5 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2b.py @@ -15,7 +15,6 @@ ) from ._vectorops import _normalize, _skew_symmetric - VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) BG_IDX = slice(6, 9) diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2c.py index d96fed72..a338ec18 100644 --- a/src/smsfusion/_v2c.py +++ b/src/smsfusion/_v2c.py @@ -9,14 +9,13 @@ from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion from ._v2common import ( _correct_quat_with_gibbs2, + _correct_quat_with_rotvec, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, - _correct_quat_with_rotvec ) from ._vectorops import _normalize, _skew_symmetric - VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) BG_IDX = slice(6, 9) diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2common.py index e7d6fea5..7d1e3370 100644 --- a/src/smsfusion/_v2common.py +++ b/src/smsfusion/_v2common.py @@ -111,7 +111,9 @@ def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: @njit # type: ignore[misc] -def _correct_quat_with_rotvec(q: NDArray[np.float64], dtheta: NDArray[np.float64]) -> None: +def _correct_quat_with_rotvec( + q: NDArray[np.float64], dtheta: NDArray[np.float64] +) -> None: """ Corrects a unit quaternion, q, with a small attitude change vector, dtheta, parameterized as a rotation vector: From a55246743a5a21361f7c59743ffbc9ba690d94e6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:49:53 +0100 Subject: [PATCH 29/86] move to v2 module --- src/smsfusion/__init__.py | 4 ++-- src/smsfusion/_v2/__init__.py | 5 +++++ src/smsfusion/{_v2.py => _v2/_v2a.py} | 8 ++++---- src/smsfusion/{ => _v2}/_v2b.py | 8 ++++---- src/smsfusion/{ => _v2}/_v2c.py | 7 +++---- src/smsfusion/{ => _v2}/_v2common.py | 2 +- 6 files changed, 19 insertions(+), 15 deletions(-) create mode 100644 src/smsfusion/_v2/__init__.py rename src/smsfusion/{_v2.py => _v2/_v2a.py} (98%) rename src/smsfusion/{ => _v2}/_v2b.py (98%) rename src/smsfusion/{ => _v2}/_v2c.py (98%) rename src/smsfusion/{ => _v2}/_v2common.py (99%) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 978dfdc1..1c8a6eca 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,13 +3,13 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import AHRSv2a -from ._v2b import AHRSv2b +from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c __all__ = [ "AHRS", "AHRSv2a", "AHRSv2b", + "AHRSv2c", "AidedINS", "benchmark", "constants", diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py new file mode 100644 index 00000000..cba3bc67 --- /dev/null +++ b/src/smsfusion/_v2/__init__.py @@ -0,0 +1,5 @@ +from ._v2a import AHRSv2a +from ._v2b import AHRSv2b +from ._v2c import AHRSv2c + +__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c"] diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2/_v2a.py similarity index 98% rename from src/smsfusion/_v2.py rename to src/smsfusion/_v2/_v2a.py index c402417f..e04707e9 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2/_v2a.py @@ -4,9 +4,10 @@ from numba import njit from numpy.typing import ArrayLike, NDArray -from ._ins import _dhda_head, _h_head, _signed_smallest_angle -from ._transforms import _angular_matrix_from_quaternion as T -from ._transforms import _euler_from_quaternion +from .._ins import _dhda_head, _h_head, _signed_smallest_angle +from .._transforms import _angular_matrix_from_quaternion as T +from .._transforms import _euler_from_quaternion +from .._vectorops import _normalize, _skew_symmetric from ._v2common import ( _correct_quat_with_gibbs2, _kalman_update_scalar, @@ -15,7 +16,6 @@ _project_cov_ahead, _vg_b, ) -from ._vectorops import _normalize, _skew_symmetric def _state_transition( diff --git a/src/smsfusion/_v2b.py b/src/smsfusion/_v2/_v2b.py similarity index 98% rename from src/smsfusion/_v2b.py rename to src/smsfusion/_v2/_v2b.py index 6352a1e5..15cd71ee 100644 --- a/src/smsfusion/_v2b.py +++ b/src/smsfusion/_v2/_v2b.py @@ -4,16 +4,16 @@ from numba import njit from numpy.typing import ArrayLike, NDArray -from ._ins import _dhda_head, _h_head, _signed_smallest_angle -from ._transforms import _angular_matrix_from_quaternion as T -from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from .._ins import _dhda_head, _h_head, _signed_smallest_angle +from .._transforms import _angular_matrix_from_quaternion as T +from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from .._vectorops import _normalize, _skew_symmetric from ._v2common import ( _correct_quat_with_gibbs2, _kalman_update_scalar, _kalman_update_sequential, _project_cov_ahead, ) -from ._vectorops import _normalize, _skew_symmetric VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) diff --git a/src/smsfusion/_v2c.py b/src/smsfusion/_v2/_v2c.py similarity index 98% rename from src/smsfusion/_v2c.py rename to src/smsfusion/_v2/_v2c.py index a338ec18..d59caf84 100644 --- a/src/smsfusion/_v2c.py +++ b/src/smsfusion/_v2/_v2c.py @@ -4,9 +4,9 @@ from numba import njit from numpy.typing import ArrayLike, NDArray -from ._ins import _dhda_head, _h_head, _signed_smallest_angle -from ._transforms import _angular_matrix_from_quaternion as T -from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from .._ins import _dhda_head, _h_head, _signed_smallest_angle +from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from .._vectorops import _skew_symmetric from ._v2common import ( _correct_quat_with_gibbs2, _correct_quat_with_rotvec, @@ -14,7 +14,6 @@ _kalman_update_sequential, _project_cov_ahead, ) -from ._vectorops import _normalize, _skew_symmetric VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) diff --git a/src/smsfusion/_v2common.py b/src/smsfusion/_v2/_v2common.py similarity index 99% rename from src/smsfusion/_v2common.py rename to src/smsfusion/_v2/_v2common.py index 7d1e3370..e2127a03 100644 --- a/src/smsfusion/_v2common.py +++ b/src/smsfusion/_v2/_v2common.py @@ -2,7 +2,7 @@ from numba import njit from numpy.typing import NDArray -from ._vectorops import _normalize, _quaternion_product +from .._vectorops import _normalize, _quaternion_product def _nz2vg(nav_frame: str) -> float: From 96f41d9739e774b58713c30401d44f432e7647f5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 10:56:44 +0100 Subject: [PATCH 30/86] project ahead fix --- src/smsfusion/_v2/_v2c.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py index d59caf84..9522f9b6 100644 --- a/src/smsfusion/_v2/_v2c.py +++ b/src/smsfusion/_v2/_v2c.py @@ -387,7 +387,7 @@ def _project_ahead(self) -> None: """ # Velocity (dead reckoning) - self._v_n[:] += self._dvel + self._v_n[:] += self._R_nb @ self._dvel + self._dt * self._g_n # Attitude (dead reckoning) _correct_quat_with_rotvec(self._q_nb, self._dtheta) From 3602c38eef3a85ec0ffb2af71e6317222b7a7617 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 11:00:29 +0100 Subject: [PATCH 31/86] subtract gyro bias --- src/smsfusion/_v2/_v2c.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py index 9522f9b6..58fe378a 100644 --- a/src/smsfusion/_v2/_v2c.py +++ b/src/smsfusion/_v2/_v2c.py @@ -450,7 +450,7 @@ def update( # Update model self._dvel[:] = dvel - self._dtheta[:] = dtheta + self._dtheta[:] = dtheta - self._dt * self._bg_b self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) From f928c8f3b64dd117e6366b7082c6d592be469031 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 11:41:15 +0100 Subject: [PATCH 32/86] v2d --- src/smsfusion/_v2/_v2d.py | 402 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 402 insertions(+) create mode 100644 src/smsfusion/_v2/_v2d.py diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py new file mode 100644 index 00000000..cf41dd7c --- /dev/null +++ b/src/smsfusion/_v2/_v2d.py @@ -0,0 +1,402 @@ +from typing import Self + +import numpy as np +from numba import njit +from numpy.typing import ArrayLike, NDArray + +from .._ins import _dhda_head, _h_head, _signed_smallest_angle +from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from .._vectorops import _skew_symmetric +from .._vectorops import _normalize +from ._v2common import ( + _correct_quat_with_gibbs2, + _correct_quat_with_rotvec, + _kalman_update_scalar, + _kalman_update_sequential, + _project_cov_ahead, + _nz2vg, + _vg_b, +) + +ATT_IDX = slice(0, 3) +BG_IDX = slice(3, 6) + + +def _state_transition( + dt: float, + dtheta: NDArray[np.float64], + gbc: float, +) -> NDArray[np.float64]: + """ + State transition matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + dtheta : ndarray, shape (3,) + Attitude change vector (coning integral). + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + ndarray, shape (6, 6) + State transition matrix. + """ + phi = np.eye(6) + phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta) # NB! update each time step + phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) + phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc + return phi + + +@njit # type: ignore[misc] +def _update_state_transition( + phi: NDArray[np.float64], + dtheta: NDArray[np.float64], +) -> None: + """ + Update the state transition matrix in place. + + Parameters + ---------- + phi : ndarray, shape (9, 9) + State transition matrix to be updated in place. + dtheta : ndarray, shape (3,) + Attitude change vector (coning integral). + """ + dtx, dty, dtz = dtheta + + # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b) + phi[0, 1] = dtz + phi[0, 2] = -dty + phi[1, 0] = -dtz + phi[1, 2] = dtx + phi[2, 0] = dty + phi[2, 1] = -dtx + + +def _process_noise_cov( + dt: float, arw: float, gbs: float, gbc: float +) -> NDArray[np.float64]: + """ + Process noise covariance matrix. + + Parameters + ---------- + dt : float + Time step in seconds. + arw : float + Angular random walk (gyroscope noise density) in rad/√Hz. + gbs : float + Gyro bias stability (bias instability) in rad/s. + gbc : float + Gyro bias correlation time in seconds. + + Returns + ------- + Q : ndarray, shape (9, 9) + Process noise covariance matrix. + """ + Q = np.zeros((6, 6)) + Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) + Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + return Q + + +def _measurement_matrix( + q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] +) -> NDArray[np.float64]: + """ + Measurement matrix. + + Parameters + ---------- + q_nb : ndarray, shape (4,) + Unit quaternion. + vg_b : ndarray, shape (3,) + Gravity reference unit vector expressed in the body frame. + + Returns + ------- + ndarray, shape (4, 6) + Linearized measurement matrix. + """ + dhdx = np.zeros((4, 6)) + dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b) # gravity ref vector + dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading + return dhdx + + +class AHRSv2a: + """ + Attitude and Heading Reference System (AHRS) using a multiplicative extended + Kalman filter (MEKF). + + Parameters + ---------- + fs : float + Sampling rate in Hz. + q_nb : Attitude or array_like, shape (4,), optional + Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults + to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). + bg_b : array_like, shape (3,), optional + Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. + dtheta : array_like, shape (3,), optional + Initial attitude change vector (coning integral). + P : array_like, shape (6, 6), optional + Initial (a priori) estimate of the error covariance matrix, **P**. If not + given, a small diagonal matrix will be used. + gyro_noise_density : float, optional + Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to + 0.00005 (SMS Motion 2 noise level). + gyro_bias_stability : float, optional + Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). + gyro_bias_corr_time : float, optional + Gyroscope bias correlation time in seconds. Defaults to 50.0 s. + nav_frame : {'NED', 'ENU'}, default 'NED' + Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) + (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom + will be expressed relative to this frame. + + """ + + _I: NDArray[np.float64] = np.eye(6) + + def __init__( + self, + fs: float, + q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), + bg_b: ArrayLike = (0.0, 0.0, 0.0), + dtheta: ArrayLike = (0.0, 0.0, 0.0), + P: ArrayLike = 1e-6 * np.eye(6), + gyro_noise_density: float = 0.0001, + gyro_bias_stability: float = 0.00005, + gyro_bias_corr_time: float = 50.0, + nav_frame: str = "NED", + ) -> None: + self._fs = fs + self._dt = 1.0 / fs + self._nav_frame = nav_frame.lower() + self._nz2vg = _nz2vg(self._nav_frame) + + # IMU noise parameters + self._arw = gyro_noise_density # angular random walk + self._gbs = gyro_bias_stability # gyro bias stability + self._gbc = gyro_bias_corr_time # gyro bias correlation time + + # State and covariance estimates + self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() + self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() + self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() + self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() + self._dx = np.zeros(6) + + # Discrete state-space model + self._phi = _state_transition(self._dt, self._dtheta, self._gbc) + self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) + self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg)) + + def quaternion(self) -> NDArray[np.float64]: + """ + Attitude expressed as a unit quaternion. + """ + return self._q_nb.copy() + + def euler(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Attitude expressed as Euler angles (roll, pitch, yaw). + + Parameters + ---------- + degrees : bool, default False + Whether to return the Euler angles in degrees or radians. + + Returns + ------- + numpy.ndarray, shape (3,) + Euler angles (roll, pitch, yaw). + """ + + theta = _euler_from_quaternion(self._q_nb) + + if degrees: + theta = (180.0 / np.pi) * theta + + return theta + + def bias_gyro(self, degrees=False) -> NDArray[np.float64]: + """ + Gyroscope bias estimate (rad/s) expressed in the body frame. + + Parameters + ---------- + degrees : bool, optional + Whether to return the bias in deg/s or rad/s. Defaults to rad/s. + """ + bg_b = self._bg_b.copy() + if degrees: + bg_b = (180.0 / np.pi) * bg_b + return bg_b + + # def angular_rate(self, degrees=False) -> NDArray[np.float64]: + # """ + # Bias corrected angular rate measurement expressed in the body frame. + + # Parameters + # ---------- + # degrees : bool, optional + # Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. + # """ + # w_b = self._w_b.copy() + # if degrees: + # w_b = (180.0 / np.pi) * w_b + # return w_b + + @property + def P(self) -> NDArray[np.float64]: + """ + Copy of the error covariance matrix estimate. + """ + return self._P.copy() + + def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Gravity reference vector part of the measurement matrix, shape (3, 6). + """ + self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) + return self._dhdx[0:3] + + def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Heading (yaw angle) part of the measurement matrix, shape (6,). + """ + self._dhdx[3:4, 0:3] = _dhda_head(q_nb) + return self._dhdx[3] + + def _reset(self) -> None: + """ + Reset state. + """ + + if not self._dx.any(): + return + + _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3]) + self._bg_b[:] += self._dx[3:6] + self._dx[:] = 0.0 + + def _aiding_update_gref( + self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None + ) -> None: + """ + Update with gravity reference vector aiding measurement. + """ + + if vg_meas is None: + return None + + if vg_var is None: + raise ValueError("'vg_var' not provided.") + + vg_b = _vg_b(self._q_nb, self._nz2vg) + dz = vg_meas - vg_b + dhdx = self._dhdx_gref(vg_b) + _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) + + def _aiding_update_head( + self, head_meas: float | None, head_var: float | None, head_degrees: bool + ) -> None: + """ + Update with heading aiding measurement. + """ + + if head_meas is None: + return None + + if head_var is None: + raise ValueError("'head_var' not provided.") + + if head_degrees: + head_meas = (np.pi / 180.0) * head_meas + head_var = (np.pi / 180.0) ** 2 * head_var + + dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) + dhdx = self._dhdx_yaw(self._q_nb) + _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) + + def _project_ahead(self) -> None: + """ + Project state and covariance estimates ahead. + """ + + # Attitude (dead reckoning) + _correct_quat_with_rotvec(self._q_nb, self._dtheta) + + # Covariance + self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) + + def update( + self, + dvel: ArrayLike, + dtheta: ArrayLike, + degrees: bool = False, + head: float | None = None, + head_var: float | None = None, + head_degrees: bool = False, + g_ref: bool = True, + g_var: ArrayLike | None = (0.001, 0.001, 0.001), + ) -> Self: + """ + Update state estimates with IMU and aiding measurements. + + Parameters + ---------- + dvel : array_like, shape (3,), optional + Initial velocity change vector (sculling integral). + dtheta : array_like, shape (3,), optional + Initial attitude change vector (coning integral). + degrees : bool, optional + Specifies whether the unit of the attitude change vector, ``dtheta``, + is degrees or radians. Defaults to radians. + head : float, optional + Heading measurement. I.e., the yaw angle of the 'body' frame relative to the + assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. + If ``None``, compass aiding is not used. See ``head_degrees`` for units. + head_var : float, optional + Variance of heading measurement noise. Units must be compatible with ``head``. + See ``head_degrees`` for units. Required for ``head``. + head_degrees : bool, default False + Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, + or radians and radians^2. Default is in radians and radians^2. + g_ref : bool, optional + Specifies whether the gravity reference vector is used as an aiding measurement. + g_var : array-like, optional + Variance of gravitational reference vector measurement noise. Required for + ``g_ref``. + + Returns + ------- + AHRS + A reference to the instance itself after the update. + """ + + if degrees: + dtheta = np.radians(dtheta) + + # Project (a priori) state and covariance estimates ahead + self._project_ahead() + + # Update (a posteriori) state and covariance estimates with aiding measurements + self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var) + self._aiding_update_head(head, head_var, head_degrees) + + # Reset state + self._reset() + + # Update model + self._dtheta[:] = dtheta - self._dt * self._bg_b + _update_state_transition(self._phi, self._dt, self._dtheta) + + return self From 1cf779f6d77d175cfc5514d97d3f45dc794f8813 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 11:42:42 +0100 Subject: [PATCH 33/86] init --- src/smsfusion/__init__.py | 3 ++- src/smsfusion/_v2/__init__.py | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 1c8a6eca..15bb24ae 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,13 +3,14 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c +from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c, AHRSv2d __all__ = [ "AHRS", "AHRSv2a", "AHRSv2b", "AHRSv2c", + "AHRSv2d", "AidedINS", "benchmark", "constants", diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py index cba3bc67..8f8718ff 100644 --- a/src/smsfusion/_v2/__init__.py +++ b/src/smsfusion/_v2/__init__.py @@ -1,5 +1,6 @@ from ._v2a import AHRSv2a from ._v2b import AHRSv2b from ._v2c import AHRSv2c +from ._v2d import AHRSv2d -__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c"] +__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c", "AHRSv2d"] From 96f6577af69e0de49b81d34afe8901597db226e4 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 11:43:15 +0100 Subject: [PATCH 34/86] fix name --- src/smsfusion/_v2/_v2d.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py index cf41dd7c..7acf5856 100644 --- a/src/smsfusion/_v2/_v2d.py +++ b/src/smsfusion/_v2/_v2d.py @@ -129,7 +129,7 @@ def _measurement_matrix( return dhdx -class AHRSv2a: +class AHRSv2d: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended Kalman filter (MEKF). From 7318c5ab7f7cb07b4cd7fb9a7675b11726fe075d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 11:44:20 +0100 Subject: [PATCH 35/86] fix --- src/smsfusion/_v2/_v2d.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py index 7acf5856..92a6ab9b 100644 --- a/src/smsfusion/_v2/_v2d.py +++ b/src/smsfusion/_v2/_v2d.py @@ -397,6 +397,6 @@ def update( # Update model self._dtheta[:] = dtheta - self._dt * self._bg_b - _update_state_transition(self._phi, self._dt, self._dtheta) + _update_state_transition(self._phi, self._dtheta) return self From e21c6dd09f1552934a0e1b57f6af3a39e58edd12 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 15:48:18 +0100 Subject: [PATCH 36/86] some fixes --- src/smsfusion/_v2/_v2d.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py index 92a6ab9b..eb86eb6a 100644 --- a/src/smsfusion/_v2/_v2d.py +++ b/src/smsfusion/_v2/_v2d.py @@ -169,7 +169,6 @@ def __init__( fs: float, q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), bg_b: ArrayLike = (0.0, 0.0, 0.0), - dtheta: ArrayLike = (0.0, 0.0, 0.0), P: ArrayLike = 1e-6 * np.eye(6), gyro_noise_density: float = 0.0001, gyro_bias_stability: float = 0.00005, @@ -189,12 +188,11 @@ def __init__( # State and covariance estimates self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() self._dx = np.zeros(6) # Discrete state-space model - self._phi = _state_transition(self._dt, self._dtheta, self._gbc) + self._phi = _state_transition(self._dt, np.zeros(3), self._gbc) self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg)) @@ -326,13 +324,13 @@ def _aiding_update_head( dhdx = self._dhdx_yaw(self._q_nb) _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) - def _project_ahead(self) -> None: + def _project_ahead(self, dtheta) -> None: """ Project state and covariance estimates ahead. """ # Attitude (dead reckoning) - _correct_quat_with_rotvec(self._q_nb, self._dtheta) + _correct_quat_with_rotvec(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) @@ -382,11 +380,16 @@ def update( A reference to the instance itself after the update. """ + dvel = np.asarray(dvel) + dtheta = np.asarray(dtheta) + if degrees: dtheta = np.radians(dtheta) + dtheta -= self._dt * self._bg_b + # Project (a priori) state and covariance estimates ahead - self._project_ahead() + self._project_ahead(dtheta) # Update (a posteriori) state and covariance estimates with aiding measurements self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var) @@ -396,7 +399,6 @@ def update( self._reset() # Update model - self._dtheta[:] = dtheta - self._dt * self._bg_b - _update_state_transition(self._phi, self._dtheta) + _update_state_transition(self._phi, dtheta) return self From 52492e514ff09b97e0a33c29c5505266073ab288 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:03:21 +0100 Subject: [PATCH 37/86] fix use correct timestamp dv dth v2c --- src/smsfusion/_v2/_v2c.py | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2/_v2c.py index 58fe378a..8ec4cca0 100644 --- a/src/smsfusion/_v2/_v2c.py +++ b/src/smsfusion/_v2/_v2c.py @@ -171,10 +171,10 @@ class AHRSv2c: to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). bg_b : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - dvel : array_like, shape (3,), optional - Initial velocity change vector (sculling integral). - dtheta : array_like, shape (3,), optional - Initial attitude change vector (coning integral). + dvel_prev : array_like, shape (3,), optional + Previous velocity change vector measurement (sculling integral). + dtheta_prev : array_like, shape (3,), optional + Previous attitude change vector measurement (coning integral). P : array_like, shape (6, 6), optional Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a small diagonal matrix will be used. @@ -206,8 +206,8 @@ def __init__( v_n: ArrayLike = (0.0, 0.0, 0.0), q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), bg_b: ArrayLike = (0.0, 0.0, 0.0), - dvel: ArrayLike = (0.0, 0.0, 0.0), - dtheta: ArrayLike = (0.0, 0.0, 0.0), + dvel_prev: ArrayLike = (0.0, 0.0, 0.0), + dtheta_prev: ArrayLike = (0.0, 0.0, 0.0), P: ArrayLike = 1e-6 * np.eye(9), acc_noise_density: float = 0.0007, gyro_noise_density: float = 0.0001, @@ -239,15 +239,15 @@ def __init__( self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() self._R_nb = _rot_matrix_from_quaternion(self._q_nb) self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy() - self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() + self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy() + self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy() self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() self._dx = np.zeros(9) # Discrete state-space model self._phi = _state_transition( - self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc + self._dt, self._dvel_prev, self._dtheta_prev, self._R_nb, self._gbc ) self._Q = _process_noise_cov( self._dt, self._vrw, self._arw, self._gbs, self._gbc @@ -381,16 +381,16 @@ def _aiding_update_head( dhdx = self._dhdx_yaw(self._q_nb) _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) - def _project_ahead(self) -> None: + def _project_ahead(self, dvel, dtheta) -> None: """ Project state and covariance estimates ahead. """ # Velocity (dead reckoning) - self._v_n[:] += self._R_nb @ self._dvel + self._dt * self._g_n + self._v_n[:] += self._R_nb @ dvel + self._dt * self._g_n # Attitude (dead reckoning) - _correct_quat_with_rotvec(self._q_nb, self._dtheta) + _correct_quat_with_rotvec(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) @@ -435,11 +435,16 @@ def update( A reference to the instance itself after the update. """ + dvel = np.asarray(dvel) + dtheta = np.asarray(dtheta) + if degrees: dtheta = np.radians(dtheta) + dtheta -= self._dt * self._bg_b + # Project (a priori) state and covariance estimates ahead - self._project_ahead() + self._project_ahead(dvel, dtheta) # Update (a posteriori) state and covariance estimates with aiding measurements self._aiding_update_vel(vel, vel_var) @@ -449,9 +454,7 @@ def update( self._reset() # Update model - self._dvel[:] = dvel - self._dtheta[:] = dtheta - self._dt * self._bg_b self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) + _update_state_transition(self._phi, dvel, dtheta, self._R_nb) return self From 8e6a4b18bf18b7102810d5b9c92287745601b102 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:17:09 +0100 Subject: [PATCH 38/86] keep only v2c --- src/smsfusion/__init__.py | 7 +- src/smsfusion/{_v2/_v2c.py => _v2.py} | 296 +++++++++++++++- src/smsfusion/_v2/__init__.py | 6 - src/smsfusion/_v2/_v2a.py | 401 ---------------------- src/smsfusion/_v2/_v2b.py | 466 -------------------------- src/smsfusion/_v2/_v2common.py | 286 ---------------- src/smsfusion/_v2/_v2d.py | 404 ---------------------- 7 files changed, 287 insertions(+), 1579 deletions(-) rename src/smsfusion/{_v2/_v2c.py => _v2.py} (65%) delete mode 100644 src/smsfusion/_v2/__init__.py delete mode 100644 src/smsfusion/_v2/_v2a.py delete mode 100644 src/smsfusion/_v2/_v2b.py delete mode 100644 src/smsfusion/_v2/_v2common.py delete mode 100644 src/smsfusion/_v2/_v2d.py diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 15bb24ae..6dd89429 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,14 +3,11 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import AHRSv2a, AHRSv2b, AHRSv2c, AHRSv2d +from ._v2 import AHRSv2 __all__ = [ "AHRS", - "AHRSv2a", - "AHRSv2b", - "AHRSv2c", - "AHRSv2d", + "AHRSv2", "AidedINS", "benchmark", "constants", diff --git a/src/smsfusion/_v2/_v2c.py b/src/smsfusion/_v2.py similarity index 65% rename from src/smsfusion/_v2/_v2c.py rename to src/smsfusion/_v2.py index 8ec4cca0..7e31526a 100644 --- a/src/smsfusion/_v2/_v2c.py +++ b/src/smsfusion/_v2.py @@ -4,22 +4,296 @@ from numba import njit from numpy.typing import ArrayLike, NDArray -from .._ins import _dhda_head, _h_head, _signed_smallest_angle -from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion -from .._vectorops import _skew_symmetric -from ._v2common import ( - _correct_quat_with_gibbs2, - _correct_quat_with_rotvec, - _kalman_update_scalar, - _kalman_update_sequential, - _project_cov_ahead, -) +from ._ins import _dhda_head, _h_head, _signed_smallest_angle +from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from ._vectorops import _normalize, _quaternion_product, _skew_symmetric VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) BG_IDX = slice(6, 9) +def _nz2vg(nav_frame: str) -> float: + """ + Gravity direction along the navigation frame's z-axis. + """ + if nav_frame == "ned": + return 1.0 + elif nav_frame == "enu": + return -1.0 + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + + +@njit # type: ignore[misc] +def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: + """ + Gravity reference vector expressed in the body frame, computed from a unit quaternion. + + Parameters + ---------- + q_nb : numpy.ndarray, shape (4,) + Unit quaternion. + nz2vg : float + Gravity direction along the navigation frame's z-axis. Should be +1 for + NED and -1 for ENU. + """ + qw, qx, qy, qz = q_nb + + x = 2.0 * (qx * qz - qw * qy) + y = 2.0 * (qy * qz + qw * qx) + z = 1.0 - 2.0 * (qx**2 + qy**2) + + return nz2vg * np.array([x, y, z]) + + +@njit # type: ignore[misc] +def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: + """ + Corrects a unit quaternion, q, with a small attitude error, da, parameterized + as a scaled (2x) Gibbs vector: + + q = q ⊗ dq(da) + + As described in ref [1]_, this correction can be simplified by doing it in two + steps: first a correction, followed by renormalization. The scaling factor becomes + obsolete due to the renormalization step. + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion [qw, qx, qy, qz] (modified in place). + da : ndarray, shape (3,) + Small attitude error parameterized as a scaled (2x) Gibbs vector. + + References + ---------- + Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination + and Control, Eq. (6.27)-(6.28). + """ + + qw, qx, qy, qz = q + dax, day, daz = da + + q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) + q[1] += 0.5 * (qw * dax + qy * daz - qz * day) + q[2] += 0.5 * (qw * day - qx * daz + qz * dax) + q[3] += 0.5 * (qw * daz + qx * day - qy * dax) + q[:] = _normalize(q) + + +@njit # type: ignore[misc] +def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: + """ + Compute the unit quaternion from a rotation vector. + + Parameters + ---------- + r : numpy.ndarray, shape (3,) + Rotation vector (rx, ry, rz). + + Returns + ------- + numpy.ndarray, shape (4,) + Unit quaternion (qw, qx, qy, qz). + """ + # TODO: add reference + + rx, ry, rz = r + + angle2 = rx**2 + ry**2 + rz**2 + + if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) + a = 0.25 * angle2 + c = 1.0 - a / 2.0 + s = 0.5 * (1.0 - a / 6.0) + else: + angle = np.sqrt(angle2) + half_angle = 0.5 * angle + c = np.cos(half_angle) + s = np.sin(half_angle) / angle + + q = np.array([c, s * rx, s * ry, s * rz]) + + return _normalize(q) + + +@njit # type: ignore[misc] +def _correct_quat_with_rotvec( + q: NDArray[np.float64], dtheta: NDArray[np.float64] +) -> None: + """ + Corrects a unit quaternion, q, with a small attitude change vector, dtheta, + parameterized as a rotation vector: + + q = q ⊗ dq(dtheta) + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion (modified in place). + dtheta : ndarray, shape (3,) + Small attitude change parameterized as a rotation vector. + """ + q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) + + +@njit # type: ignore[misc] +def _kalman_gain( + P: NDArray[np.float64], h: NDArray[np.float64], r: float +) -> NDArray[np.float64]: + """ + Compute the Kalman gain for a scalar measurement. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + + Returns + ------- + ndarray, shape (n,) + Kalman gain vector. + """ + + # Innovation covariance (inverse) + Ph = np.dot(P, h) + s_inv = 1.0 / (np.dot(h, Ph) + r) + + # Kalman gain + k = Ph * s_inv + + return k + + +@njit # type: ignore[misc] +def _covariance_update( + P: NDArray[np.float64], + k: NDArray[np.float64], + h: NDArray[np.float64], + r: float, + I_: NDArray[np.float64], +) -> None: + """ + Compute the updated state error covariance matrix estimate (Joseph form). + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + k : ndarray, shape (n,) + Kalman gain vector. + h : ndarray, shape (n,) + Measurement matrix (row vector). + r : float + Scalar measurement noise variance. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + A = I_ - np.outer(k, h) + P = A @ P @ A.T + r * np.outer(k, k) + return P + + +@njit # type: ignore[misc] +def _kalman_update_scalar( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: float, + r: float, + h: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Scalar Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : float + Scalar measurement. + r : float + Scalar measurement noise variance. + h : ndarray, shape (n,) + Measurement matrix (row vector). + I_ : ndarray, shape (n, n) + Identity matrix. + """ + + # Kalman gain + k = _kalman_gain(P, h, r) + + # Updated (a posteriori) state estimate + x[:] += k * (z - np.dot(h, x)) + + # Updated (a posteriori) covariance estimate (Joseph form) + P[:, :] = _covariance_update(P, k, h, r, I_) + + +@njit # type: ignore[misc] +def _kalman_update_sequential( + x: NDArray[np.float64], + P: NDArray[np.float64], + z: NDArray[np.float64], + var: NDArray[np.float64], + H: NDArray[np.float64], + I_: NDArray[np.float64], +) -> None: + """ + Sequential (one-at-a-time) Kalman filter measurement update. + + Parameters + ---------- + x : ndarray, shape (n,) + State estimate to be updated in place. + P : ndarray, shape (n, n) + State error covariance matrix to be updated in place. + z : ndarray, shape (m,) + Measurement vector. + var : ndarray, shape (m,) + Measurement noise variances corresponding to each scalar measurement. + H : ndarray, shape (m, n) + Measurement matrix where each row corresponds to a scalar measurement model. + I_ : ndarray, shape (n, n) + Identity matrix. + """ + m = z.shape[0] + for i in range(m): + _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) + + +@njit # type: ignore[misc] +def _project_cov_ahead( + P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] +) -> None: + """ + Project the error covariance matrix estimate ahead. + + Parameters + ---------- + P : ndarray, shape (n, n) + State error covariance matrix to be projected ahead. + phi : ndarray, shape (n, n) + State transition matrix. + Q : ndarray, shape (n, n) + Process noise covariance matrix. + + Returns + ------- + ndarray, shape (n, n) + Projected error covariance matrix estimate. + """ + P = phi @ P @ phi.T + Q + return P + + def _state_transition( dt: float, dvel: NDArray[np.float64], @@ -155,7 +429,7 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: return dhdx -class AHRSv2c: +class AHRSv2: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended Kalman filter (MEKF). diff --git a/src/smsfusion/_v2/__init__.py b/src/smsfusion/_v2/__init__.py deleted file mode 100644 index 8f8718ff..00000000 --- a/src/smsfusion/_v2/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -from ._v2a import AHRSv2a -from ._v2b import AHRSv2b -from ._v2c import AHRSv2c -from ._v2d import AHRSv2d - -__all__ = ["AHRSv2a", "AHRSv2b", "AHRSv2c", "AHRSv2d"] diff --git a/src/smsfusion/_v2/_v2a.py b/src/smsfusion/_v2/_v2a.py deleted file mode 100644 index e04707e9..00000000 --- a/src/smsfusion/_v2/_v2a.py +++ /dev/null @@ -1,401 +0,0 @@ -from typing import Self - -import numpy as np -from numba import njit -from numpy.typing import ArrayLike, NDArray - -from .._ins import _dhda_head, _h_head, _signed_smallest_angle -from .._transforms import _angular_matrix_from_quaternion as T -from .._transforms import _euler_from_quaternion -from .._vectorops import _normalize, _skew_symmetric -from ._v2common import ( - _correct_quat_with_gibbs2, - _kalman_update_scalar, - _kalman_update_sequential, - _nz2vg, - _project_cov_ahead, - _vg_b, -) - - -def _state_transition( - dt: float, w_b: NDArray[np.float64], gbc: float -) -> NDArray[np.float64]: - """ - State transition matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - w_b : ndarray, shape (3,) - Angular rate measurement (bias corrected) in body frame. - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - ndarray, shape (6, 6) - State transition matrix. - """ - phi = np.eye(6) - phi[0:3, 0:3] -= dt * _skew_symmetric(w_b) # NB! update each time step - phi[0:3, 3:6] -= dt * np.eye(3) - phi[3:6, 3:6] -= dt * np.eye(3) / gbc - return phi - - -@njit # type: ignore[misc] -def _update_state_transition( - phi: NDArray[np.float64], - dt: float, - w_b: NDArray[np.float64], -) -> None: - """ - Update the state transition matrix in place. - - Parameters - ---------- - phi : ndarray, shape (6, 6) - State transition matrix to be updated in place. - dt : float - Time step. - w_b : ndarray, shape (3,) - Angular rate measurement (bias corrected) in body frame. - """ - wx, wy, wz = w_b - phi[0, 1] = dt * wz - phi[0, 2] = -dt * wy - phi[1, 0] = -dt * wz - phi[1, 2] = dt * wx - phi[2, 0] = dt * wy - phi[2, 1] = -dt * wx - - -def _process_noise_cov( - dt: float, arw: float, gbs: float, gbc: float -) -> NDArray[np.float64]: - """ - Process noise covariance matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - arw : float - Angular random walk (gyroscope noise density) in rad/√Hz. - gbs : float - Gyro bias stability (bias instability) in rad/s. - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - Q : ndarray, shape (6, 6) - Process noise covariance matrix. - """ - Q = np.zeros((6, 6)) - Q[0:3, 0:3] = dt * arw**2 * np.eye(3) - Q[3:6, 3:6] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) - return Q - - -def _measurement_matrix( - q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] -) -> NDArray[np.float64]: - """ - Measurement matrix. - - Parameters - ---------- - q_nb : ndarray, shape (4,) - Unit quaternion. - vg_b : ndarray, shape (3,) - Gravity reference unit vector expressed in the body frame. - - Returns - ------- - ndarray, shape (4, 6) - Linearized measurement matrix. - """ - dhdx = np.zeros((4, 6)) - dhdx[0:3, 0:3] = _skew_symmetric(vg_b) # gravity ref vector - dhdx[3:4, 0:3] = _dhda_head(q_nb) # heading - return dhdx - - -class AHRSv2a: - """ - Attitude and Heading Reference System (AHRS) using a multiplicative extended - Kalman filter (MEKF). - - Parameters - ---------- - fs : float - Sampling rate in Hz. - q_nb : Attitude or array_like, shape (4,), optional - Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults - to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). - bg_b : array_like, shape (3,), optional - Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - w_b : array_like, shape (3,), optional - Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame. - Defaults to zero angular rate (stationary). - P : array_like, shape (6, 6), optional - Initial (a priori) estimate of the error covariance matrix, **P**. If not - given, a small diagonal matrix will be used. - gyro_noise_density : float, optional - Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to - 0.00005 (SMS Motion 2 noise level). - gyro_bias_stability : float, optional - Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). - gyro_bias_corr_time : float, optional - Gyroscope bias correlation time in seconds. Defaults to 50.0 s. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. - - """ - - _I: NDArray[np.float64] = np.eye(6) - - def __init__( - self, - fs: float, - q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), - bg_b: ArrayLike = (0.0, 0.0, 0.0), - w_b: ArrayLike = (0.0, 0.0, 0.0), - P: ArrayLike = 1e-6 * np.eye(6), - gyro_noise_density: float = 0.0001, - gyro_bias_stability: float = 0.00005, - gyro_bias_corr_time: float = 50.0, - nav_frame: str = "NED", - ) -> None: - self._fs = fs - self._dt = 1.0 / fs - self._nav_frame = nav_frame.lower() - self._nz2vg = _nz2vg(self._nav_frame) - - # IMU noise parameters - self._arw = gyro_noise_density # angular random walk - self._gbs = gyro_bias_stability # gyro bias stability - self._gbc = gyro_bias_corr_time # gyro bias correlation time - - # State and covariance estimates - self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() - self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy() - self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() - self._dx = np.zeros(6) - - # Discrete state-space model - self._phi = _state_transition(self._dt, self._w_b, self._gbc) - self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) - self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg)) - - def quaternion(self) -> NDArray[np.float64]: - """ - Attitude expressed as a unit quaternion. - """ - return self._q_nb.copy() - - def euler(self, degrees: bool = False) -> NDArray[np.float64]: - """ - Attitude expressed as Euler angles (roll, pitch, yaw). - - Parameters - ---------- - degrees : bool, default False - Whether to return the Euler angles in degrees or radians. - - Returns - ------- - numpy.ndarray, shape (3,) - Euler angles (roll, pitch, yaw). - """ - - theta = _euler_from_quaternion(self._q_nb) - - if degrees: - theta = (180.0 / np.pi) * theta - - return theta - - def bias_gyro(self, degrees=False) -> NDArray[np.float64]: - """ - Gyroscope bias estimate (rad/s) expressed in the body frame. - - Parameters - ---------- - degrees : bool, optional - Whether to return the bias in deg/s or rad/s. Defaults to rad/s. - """ - bg_b = self._bg_b.copy() - if degrees: - bg_b = (180.0 / np.pi) * bg_b - return bg_b - - def angular_rate(self, degrees=False) -> NDArray[np.float64]: - """ - Bias corrected angular rate measurement expressed in the body frame. - - Parameters - ---------- - degrees : bool, optional - Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. - """ - w_b = self._w_b.copy() - if degrees: - w_b = (180.0 / np.pi) * w_b - return w_b - - @property - def P(self) -> NDArray[np.float64]: - """ - Copy of the error covariance matrix estimate. - """ - return self._P.copy() - - def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Gravity reference vector part of the measurement matrix, shape (3, 6). - """ - self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) - return self._dhdx[0:3] - - def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Heading (yaw angle) part of the measurement matrix, shape (6,). - """ - self._dhdx[3:4, 0:3] = _dhda_head(q_nb) - return self._dhdx[3] - - def _reset(self) -> None: - """ - Reset state. - """ - - if not self._dx.any(): - return - - _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3]) - self._bg_b[:] += self._dx[3:6] - self._dx[:] = 0.0 - - def _aiding_update_gref( - self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None - ) -> None: - """ - Update with gravity reference vector aiding measurement. - """ - - if vg_meas is None: - return None - - if vg_var is None: - raise ValueError("'vg_var' not provided.") - - vg_b = _vg_b(self._q_nb, self._nz2vg) - dz = vg_meas - vg_b - dhdx = self._dhdx_gref(vg_b) - _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) - - def _aiding_update_head( - self, head_meas: float | None, head_var: float | None, head_degrees: bool - ) -> None: - """ - Update with heading aiding measurement. - """ - - if head_meas is None: - return None - - if head_var is None: - raise ValueError("'head_var' not provided.") - - if head_degrees: - head_meas = (np.pi / 180.0) * head_meas - head_var = (np.pi / 180.0) ** 2 * head_var - - dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) - dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) - - def _project_ahead(self) -> None: - """ - Project state and covariance estimates ahead. - """ - - # Attitude (dead reckoning) - self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b - self._q_nb[:] = _normalize(self._q_nb) - - # Covariance - self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) - - def update( - self, - f: ArrayLike, - w: ArrayLike, - degrees: bool = False, - head: float | None = None, - head_var: float | None = None, - head_degrees: bool = False, - g_ref: bool = True, - g_var: ArrayLike | None = (0.001, 0.001, 0.001), - ) -> Self: - """ - Update state estimates with IMU and aiding measurements. - - Parameters - ---------- - f : array_like, shape (3,) - Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) - in m/s^2. - w : array_like, shape (3,) - Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See - ``degrees`` parameter for units. - degrees : bool, optional - Specifies whether the unit of the rotation rate, ``w``, is deg/s or - rad/s. Defaults to rad/s. - head : float, optional - Heading measurement. I.e., the yaw angle of the 'body' frame relative to the - assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. - If ``None``, compass aiding is not used. See ``head_degrees`` for units. - head_var : float, optional - Variance of heading measurement noise. Units must be compatible with ``head``. - See ``head_degrees`` for units. Required for ``head``. - head_degrees : bool, default False - Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, - or radians and radians^2. Default is in radians and radians^2. - g_ref : bool, optional - Specifies whether the gravity reference vector is used as an aiding measurement. - g_var : array-like, optional - Variance of gravitational reference vector measurement noise. Required for - ``g_ref``. - - Returns - ------- - AHRS - A reference to the instance itself after the update. - """ - - if degrees: - w = np.radians(w) - - # Project (a priori) state and covariance estimates ahead - self._project_ahead() - - # Update (a posteriori) state and covariance estimates with aiding measurements - self._aiding_update_gref(-_normalize(f) if g_ref else None, g_var) - self._aiding_update_head(head, head_var, head_degrees) - - # Reset state - self._reset() - - # Update model - self._w_b[:] = w - self._bg_b - _update_state_transition(self._phi, self._dt, self._w_b) - - return self diff --git a/src/smsfusion/_v2/_v2b.py b/src/smsfusion/_v2/_v2b.py deleted file mode 100644 index 15cd71ee..00000000 --- a/src/smsfusion/_v2/_v2b.py +++ /dev/null @@ -1,466 +0,0 @@ -from typing import Self - -import numpy as np -from numba import njit -from numpy.typing import ArrayLike, NDArray - -from .._ins import _dhda_head, _h_head, _signed_smallest_angle -from .._transforms import _angular_matrix_from_quaternion as T -from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion -from .._vectorops import _normalize, _skew_symmetric -from ._v2common import ( - _correct_quat_with_gibbs2, - _kalman_update_scalar, - _kalman_update_sequential, - _project_cov_ahead, -) - -VEL_IDX = slice(0, 3) -ATT_IDX = slice(3, 6) -BG_IDX = slice(6, 9) - - -def _state_transition( - dt: float, - f_b: NDArray[np.float64], - w_b: NDArray[np.float64], - R_nb: NDArray[np.float64], - gbc: float, -) -> NDArray[np.float64]: - """ - State transition matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - f_b : ndarray, shape (3,) - Specific force measurement in body frame. - w_b : ndarray, shape (3,) - Angular rate measurement (bias corrected) in body frame. - R_nb : ndarray, shape (3, 3) - Rotation matrix from body to navigation frame. - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - ndarray, shape (9, 9) - State transition matrix. - """ - phi = np.eye(9) - phi[VEL_IDX, ATT_IDX] -= ( - dt * R_nb @ _skew_symmetric(f_b) - ) # NB! update each time step - phi[ATT_IDX, ATT_IDX] -= dt * _skew_symmetric(w_b) # NB! update each time step - phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) - phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc - return phi - - -@njit # type: ignore[misc] -def _update_state_transition( - phi: NDArray[np.float64], - dt: float, - f_b: NDArray[np.float64], - w_b: NDArray[np.float64], - R_nb: NDArray[np.float64], -) -> None: - """ - Update the state transition matrix in place. - - Parameters - ---------- - phi : ndarray, shape (9, 9) - State transition matrix to be updated in place. - dt : float - Time step. - f_b : ndarray, shape (3,) - Specific force measurement in body frame. - w_b : ndarray, shape (3,) - Angular rate measurement (bias corrected) in body frame. - R_nb : ndarray, shape (3, 3) - Rotation matrix from body to navigation frame. - """ - wx, wy, wz = w_b - fx, fy, fz = f_b - - r00, r01, r02 = R_nb[0] - r10, r11, r12 = R_nb[1] - r20, r21, r22 = R_nb[2] - - # phi[3:6, 3:6] = np.eye(3) - dt * S(w_b) - phi[3, 4] = dt * wz - phi[3, 5] = -dt * wy - phi[4, 3] = -dt * wz - phi[4, 5] = dt * wx - phi[5, 3] = dt * wy - phi[5, 4] = -dt * wx - - # phi[0:3, 3:6] = -dt * R_nb @ S(f_b) - phi[0, 3] = -dt * (fz * r01 - fy * r02) - phi[1, 3] = -dt * (fz * r11 - fy * r12) - phi[2, 3] = -dt * (fz * r21 - fy * r22) - phi[0, 4] = -dt * (-fz * r00 + fx * r02) - phi[1, 4] = -dt * (-fz * r10 + fx * r12) - phi[2, 4] = -dt * (-fz * r20 + fx * r22) - phi[0, 5] = -dt * (fy * r00 - fx * r01) - phi[1, 5] = -dt * (fy * r10 - fx * r11) - phi[2, 5] = -dt * (fy * r20 - fx * r21) - - -def _process_noise_cov( - dt: float, vrw: float, arw: float, gbs: float, gbc: float -) -> NDArray[np.float64]: - """ - Process noise covariance matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - vrw : float - Velocity random walk (accelerometer noise density) in m/s/√Hz. - arw : float - Angular random walk (gyroscope noise density) in rad/√Hz. - gbs : float - Gyro bias stability (bias instability) in rad/s. - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - Q : ndarray, shape (9, 9) - Process noise covariance matrix. - """ - Q = np.zeros((9, 9)) - Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3) - Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) - Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) - return Q - - -def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Measurement matrix. - - Parameters - ---------- - q_nb : ndarray, shape (4,) - Unit quaternion. - - Returns - ------- - ndarray, shape (4, 6) - Linearized measurement matrix. - """ - dhdx = np.zeros((4, 9)) - dhdx[0:3, VEL_IDX] = np.eye(3) # velocity - dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading - return dhdx - - -class AHRSv2b: - """ - Attitude and Heading Reference System (AHRS) using a multiplicative extended - Kalman filter (MEKF). - - Parameters - ---------- - fs : float - Sampling rate in Hz. - v_n : array_like, shape (3,), optional - Initial velocity estimate in m/s. - q_nb : Attitude or array_like, shape (4,), optional - Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults - to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). - bg_b : array_like, shape (3,), optional - Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - w_b : array_like, shape (3,), optional - Initial angular rate estimate (wx, wy, wz) in rad/s expressed in the body frame. - Defaults to zero angular rate (stationary). - P : array_like, shape (6, 6), optional - Initial (a priori) estimate of the error covariance matrix, **P**. If not - given, a small diagonal matrix will be used. - acc_noise_density : float, optional - Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to - 0.0007 (SMS Motion 2 noise level). - gyro_noise_density : float, optional - Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to - 0.00005 (SMS Motion 2 noise level). - gyro_bias_stability : float, optional - Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). - gyro_bias_corr_time : float, optional - Gyroscope bias correlation time in seconds. Defaults to 50.0 s. - g : float, default 9.80665 - The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665 - m/s^2. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. - - """ - - _I: NDArray[np.float64] = np.eye(9) - - def __init__( - self, - fs: float, - v_n: ArrayLike = (0.0, 0.0, 0.0), - a_n: ArrayLike = (0.0, 0.0, 0.0), - q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), - bg_b: ArrayLike = (0.0, 0.0, 0.0), - w_b: ArrayLike = (0.0, 0.0, 0.0), - P: ArrayLike = 1e-6 * np.eye(9), - acc_noise_density: float = 0.0007, - gyro_noise_density: float = 0.0001, - gyro_bias_stability: float = 0.00005, - gyro_bias_corr_time: float = 50.0, - g: float = 9.80665, - nav_frame: str = "NED", - ) -> None: - self._fs = fs - self._dt = 1.0 / fs - self._g = g - self._nav_frame = nav_frame.lower() - - if self._nav_frame == "ned": - self._g_n = np.array([0.0, 0.0, g]) - elif self._nav_frame == "enu": - self._g_n = np.array([0.0, 0.0, -g]) - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - - # IMU noise parameters - self._vrw = acc_noise_density # velocity random walk - self._arw = gyro_noise_density # angular random walk - self._gbs = gyro_bias_stability # gyro bias stability - self._gbc = gyro_bias_corr_time # gyro bias correlation time - - # State and covariance estimates - self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() - self._a_n = np.asarray_chkfinite(a_n).reshape(3).copy() - self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() - self._R_nb = _rot_matrix_from_quaternion(self._q_nb) - self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._f_b = self._R_nb.T @ (self._a_n - self._g_n) - self._w_b = np.asarray_chkfinite(w_b).reshape(3).copy() - self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() - self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() - self._dx = np.zeros(9) - - # Discrete state-space model - self._phi = _state_transition( - self._dt, self._f_b, self._w_b, self._R_nb, self._gbc - ) - self._Q = _process_noise_cov( - self._dt, self._vrw, self._arw, self._gbs, self._gbc - ) - self._dhdx = _measurement_matrix(self._q_nb) - - def quaternion(self) -> NDArray[np.float64]: - """ - Attitude expressed as a unit quaternion. - """ - return self._q_nb.copy() - - def euler(self, degrees: bool = False) -> NDArray[np.float64]: - """ - Attitude expressed as Euler angles (roll, pitch, yaw). - - Parameters - ---------- - degrees : bool, default False - Whether to return the Euler angles in degrees or radians. - - Returns - ------- - numpy.ndarray, shape (3,) - Euler angles (roll, pitch, yaw). - """ - - theta = _euler_from_quaternion(self._q_nb) - - if degrees: - theta = (180.0 / np.pi) * theta - - return theta - - def bias_gyro(self, degrees=False) -> NDArray[np.float64]: - """ - Gyroscope bias estimate (rad/s) expressed in the body frame. - - Parameters - ---------- - degrees : bool, optional - Whether to return the bias in deg/s or rad/s. Defaults to rad/s. - """ - bg_b = self._bg_b.copy() - if degrees: - bg_b = (180.0 / np.pi) * bg_b - return bg_b - - def angular_rate(self, degrees=False) -> NDArray[np.float64]: - """ - Bias corrected angular rate measurement expressed in the body frame. - - Parameters - ---------- - degrees : bool, optional - Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. - """ - w_b = self._w_b.copy() - if degrees: - w_b = (180.0 / np.pi) * w_b - return w_b - - @property - def P(self) -> NDArray[np.float64]: - """ - Copy of the error covariance matrix estimate. - """ - return self._P.copy() - - def _dhdx_vel(self) -> NDArray[np.float64]: - """ - Velocity part of the measurement matrix, shape (3, 6). - """ - return self._dhdx[0:3] - - def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Heading (yaw angle) part of the measurement matrix, shape (6,). - """ - self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) - return self._dhdx[3] - - def _reset(self) -> None: - """ - Reset state. - """ - - if not self._dx.any(): - return - - _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) - self._v_n[:] += self._dx[VEL_IDX] - self._bg_b[:] += self._dx[BG_IDX] - self._dx[:] = 0.0 - - def _aiding_update_vel( - self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None - ) -> None: - """ - Update with velocity aiding measurement. - """ - - if vel_meas is None: - return None - - if vel_var is None: - raise ValueError("'vg_var' not provided.") - - dz = vel_meas - self._v_n - dhdx = self._dhdx_vel() - _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I) - - def _aiding_update_head( - self, head_meas: float | None, head_var: float | None, head_degrees: bool - ) -> None: - """ - Update with heading aiding measurement. - """ - - if head_meas is None: - return None - - if head_var is None: - raise ValueError("'head_var' not provided.") - - if head_degrees: - head_meas = (np.pi / 180.0) * head_meas - head_var = (np.pi / 180.0) ** 2 * head_var - - dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) - dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) - - def _project_ahead(self) -> None: - """ - Project state and covariance estimates ahead. - """ - - # Velocity (dead reckoning) - self._v_n[:] += self._dt * self._a_n - - # Attitude (dead reckoning) - self._q_nb[:] += self._dt * T(self._q_nb) @ self._w_b - self._q_nb[:] = _normalize(self._q_nb) - - # Covariance - self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) - - def update( - self, - f: ArrayLike, - w: ArrayLike, - degrees: bool = False, - head: float | None = None, - head_var: float | None = None, - head_degrees: bool = False, - vel: ArrayLike | None = (0.0, 0.0, 0.0), - vel_var: ArrayLike | None = (100.0, 100.0, 100.0), - ) -> Self: - """ - Update state estimates with IMU and aiding measurements. - - Parameters - ---------- - f : array_like, shape (3,) - Specific force (i.e., acceleration + gravity) measurement (fx, fy, fz) - in m/s^2. - w : array_like, shape (3,) - Angular rate measurement (wx, wy, wz) in rad/s (default) or deg/s. See - ``degrees`` parameter for units. - degrees : bool, optional - Specifies whether the unit of the rotation rate, ``w``, is deg/s or - rad/s. Defaults to rad/s. - head : float, optional - Heading measurement. I.e., the yaw angle of the 'body' frame relative to the - assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. - If ``None``, compass aiding is not used. See ``head_degrees`` for units. - head_var : float, optional - Variance of heading measurement noise. Units must be compatible with ``head``. - See ``head_degrees`` for units. Required for ``head``. - head_degrees : bool, default False - Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, - or radians and radians^2. Default is in radians and radians^2. - - Returns - ------- - AHRS - A reference to the instance itself after the update. - """ - - if degrees: - w = np.radians(w) - - # Project (a priori) state and covariance estimates ahead - self._project_ahead() - - # Update (a posteriori) state and covariance estimates with aiding measurements - self._aiding_update_vel(vel, vel_var) - self._aiding_update_head(head, head_var, head_degrees) - - # Reset state - self._reset() - - # Update model - self._f_b[:] = f - self._w_b[:] = w - self._bg_b - self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - self._a_n[:] = self._R_nb @ self._f_b + self._g_n - _update_state_transition(self._phi, self._dt, self._f_b, self._w_b, self._R_nb) - - return self diff --git a/src/smsfusion/_v2/_v2common.py b/src/smsfusion/_v2/_v2common.py deleted file mode 100644 index e2127a03..00000000 --- a/src/smsfusion/_v2/_v2common.py +++ /dev/null @@ -1,286 +0,0 @@ -import numpy as np -from numba import njit -from numpy.typing import NDArray - -from .._vectorops import _normalize, _quaternion_product - - -def _nz2vg(nav_frame: str) -> float: - """ - Gravity direction along the navigation frame's z-axis. - """ - if nav_frame == "ned": - return 1.0 - elif nav_frame == "enu": - return -1.0 - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - - -@njit # type: ignore[misc] -def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: - """ - Gravity reference vector expressed in the body frame, computed from a unit quaternion. - - Parameters - ---------- - q_nb : numpy.ndarray, shape (4,) - Unit quaternion. - nz2vg : float - Gravity direction along the navigation frame's z-axis. Should be +1 for - NED and -1 for ENU. - """ - qw, qx, qy, qz = q_nb - - x = 2.0 * (qx * qz - qw * qy) - y = 2.0 * (qy * qz + qw * qx) - z = 1.0 - 2.0 * (qx**2 + qy**2) - - return nz2vg * np.array([x, y, z]) - - -@njit # type: ignore[misc] -def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: - """ - Corrects a unit quaternion, q, with a small attitude error, da, parameterized - as a scaled (2x) Gibbs vector: - - q = q ⊗ dq(da) - - As described in ref [1]_, this correction can be simplified by doing it in two - steps: first a correction, followed by renormalization. The scaling factor becomes - obsolete due to the renormalization step. - - Parameters - ---------- - q : ndarray, shape (4,) - Unit quaternion [qw, qx, qy, qz] (modified in place). - da : ndarray, shape (3,) - Small attitude error parameterized as a scaled (2x) Gibbs vector. - - References - ---------- - Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination - and Control, Eq. (6.27)-(6.28). - """ - - qw, qx, qy, qz = q - dax, day, daz = da - - q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) - q[1] += 0.5 * (qw * dax + qy * daz - qz * day) - q[2] += 0.5 * (qw * day - qx * daz + qz * dax) - q[3] += 0.5 * (qw * daz + qx * day - qy * dax) - q[:] = _normalize(q) - - -@njit # type: ignore[misc] -def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Compute the unit quaternion from a rotation vector. - - Parameters - ---------- - r : numpy.ndarray, shape (3,) - Rotation vector (rx, ry, rz). - - Returns - ------- - numpy.ndarray, shape (4,) - Unit quaternion (qw, qx, qy, qz). - """ - # TODO: add reference - - rx, ry, rz = r - - angle2 = rx**2 + ry**2 + rz**2 - - if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) - a = 0.25 * angle2 - c = 1.0 - a / 2.0 - s = 0.5 * (1.0 - a / 6.0) - else: - angle = np.sqrt(angle2) - half_angle = 0.5 * angle - c = np.cos(half_angle) - s = np.sin(half_angle) / angle - - q = np.array([c, s * rx, s * ry, s * rz]) - - return _normalize(q) - - -@njit # type: ignore[misc] -def _correct_quat_with_rotvec( - q: NDArray[np.float64], dtheta: NDArray[np.float64] -) -> None: - """ - Corrects a unit quaternion, q, with a small attitude change vector, dtheta, - parameterized as a rotation vector: - - q = q ⊗ dq(dtheta) - - Parameters - ---------- - q : ndarray, shape (4,) - Unit quaternion (modified in place). - dtheta : ndarray, shape (3,) - Small attitude change parameterized as a rotation vector. - """ - q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) - - -@njit # type: ignore[misc] -def _kalman_gain( - P: NDArray[np.float64], h: NDArray[np.float64], r: float -) -> NDArray[np.float64]: - """ - Compute the Kalman gain for a scalar measurement. - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix. - h : ndarray, shape (n,) - Measurement matrix (row vector). - r : float - Scalar measurement noise variance. - - Returns - ------- - ndarray, shape (n,) - Kalman gain vector. - """ - - # Innovation covariance (inverse) - Ph = np.dot(P, h) - s_inv = 1.0 / (np.dot(h, Ph) + r) - - # Kalman gain - k = Ph * s_inv - - return k - - -@njit # type: ignore[misc] -def _covariance_update( - P: NDArray[np.float64], - k: NDArray[np.float64], - h: NDArray[np.float64], - r: float, - I_: NDArray[np.float64], -) -> None: - """ - Compute the updated state error covariance matrix estimate (Joseph form). - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - k : ndarray, shape (n,) - Kalman gain vector. - h : ndarray, shape (n,) - Measurement matrix (row vector). - r : float - Scalar measurement noise variance. - I_ : ndarray, shape (n, n) - Identity matrix. - """ - A = I_ - np.outer(k, h) - P = A @ P @ A.T + r * np.outer(k, k) - return P - - -@njit # type: ignore[misc] -def _kalman_update_scalar( - x: NDArray[np.float64], - P: NDArray[np.float64], - z: float, - r: float, - h: NDArray[np.float64], - I_: NDArray[np.float64], -) -> None: - """ - Scalar Kalman filter measurement update. - - Parameters - ---------- - x : ndarray, shape (n,) - State estimate to be updated in place. - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - z : float - Scalar measurement. - r : float - Scalar measurement noise variance. - h : ndarray, shape (n,) - Measurement matrix (row vector). - I_ : ndarray, shape (n, n) - Identity matrix. - """ - - # Kalman gain - k = _kalman_gain(P, h, r) - - # Updated (a posteriori) state estimate - x[:] += k * (z - np.dot(h, x)) - - # Updated (a posteriori) covariance estimate (Joseph form) - P[:, :] = _covariance_update(P, k, h, r, I_) - - -@njit # type: ignore[misc] -def _kalman_update_sequential( - x: NDArray[np.float64], - P: NDArray[np.float64], - z: NDArray[np.float64], - var: NDArray[np.float64], - H: NDArray[np.float64], - I_: NDArray[np.float64], -) -> None: - """ - Sequential (one-at-a-time) Kalman filter measurement update. - - Parameters - ---------- - x : ndarray, shape (n,) - State estimate to be updated in place. - P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. - z : ndarray, shape (m,) - Measurement vector. - var : ndarray, shape (m,) - Measurement noise variances corresponding to each scalar measurement. - H : ndarray, shape (m, n) - Measurement matrix where each row corresponds to a scalar measurement model. - I_ : ndarray, shape (n, n) - Identity matrix. - """ - m = z.shape[0] - for i in range(m): - _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) - - -@njit # type: ignore[misc] -def _project_cov_ahead( - P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] -) -> None: - """ - Project the error covariance matrix estimate ahead. - - Parameters - ---------- - P : ndarray, shape (n, n) - State error covariance matrix to be projected ahead. - phi : ndarray, shape (n, n) - State transition matrix. - Q : ndarray, shape (n, n) - Process noise covariance matrix. - - Returns - ------- - ndarray, shape (n, n) - Projected error covariance matrix estimate. - """ - P = phi @ P @ phi.T + Q - return P diff --git a/src/smsfusion/_v2/_v2d.py b/src/smsfusion/_v2/_v2d.py deleted file mode 100644 index eb86eb6a..00000000 --- a/src/smsfusion/_v2/_v2d.py +++ /dev/null @@ -1,404 +0,0 @@ -from typing import Self - -import numpy as np -from numba import njit -from numpy.typing import ArrayLike, NDArray - -from .._ins import _dhda_head, _h_head, _signed_smallest_angle -from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion -from .._vectorops import _skew_symmetric -from .._vectorops import _normalize -from ._v2common import ( - _correct_quat_with_gibbs2, - _correct_quat_with_rotvec, - _kalman_update_scalar, - _kalman_update_sequential, - _project_cov_ahead, - _nz2vg, - _vg_b, -) - -ATT_IDX = slice(0, 3) -BG_IDX = slice(3, 6) - - -def _state_transition( - dt: float, - dtheta: NDArray[np.float64], - gbc: float, -) -> NDArray[np.float64]: - """ - State transition matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - dtheta : ndarray, shape (3,) - Attitude change vector (coning integral). - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - ndarray, shape (6, 6) - State transition matrix. - """ - phi = np.eye(6) - phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta) # NB! update each time step - phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) - phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc - return phi - - -@njit # type: ignore[misc] -def _update_state_transition( - phi: NDArray[np.float64], - dtheta: NDArray[np.float64], -) -> None: - """ - Update the state transition matrix in place. - - Parameters - ---------- - phi : ndarray, shape (9, 9) - State transition matrix to be updated in place. - dtheta : ndarray, shape (3,) - Attitude change vector (coning integral). - """ - dtx, dty, dtz = dtheta - - # phi[0:3, 0:3] = np.eye(3) - dt * S(w_b) - phi[0, 1] = dtz - phi[0, 2] = -dty - phi[1, 0] = -dtz - phi[1, 2] = dtx - phi[2, 0] = dty - phi[2, 1] = -dtx - - -def _process_noise_cov( - dt: float, arw: float, gbs: float, gbc: float -) -> NDArray[np.float64]: - """ - Process noise covariance matrix. - - Parameters - ---------- - dt : float - Time step in seconds. - arw : float - Angular random walk (gyroscope noise density) in rad/√Hz. - gbs : float - Gyro bias stability (bias instability) in rad/s. - gbc : float - Gyro bias correlation time in seconds. - - Returns - ------- - Q : ndarray, shape (9, 9) - Process noise covariance matrix. - """ - Q = np.zeros((6, 6)) - Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) - Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) - return Q - - -def _measurement_matrix( - q_nb: NDArray[np.float64], vg_b: NDArray[np.float64] -) -> NDArray[np.float64]: - """ - Measurement matrix. - - Parameters - ---------- - q_nb : ndarray, shape (4,) - Unit quaternion. - vg_b : ndarray, shape (3,) - Gravity reference unit vector expressed in the body frame. - - Returns - ------- - ndarray, shape (4, 6) - Linearized measurement matrix. - """ - dhdx = np.zeros((4, 6)) - dhdx[0:3, ATT_IDX] = _skew_symmetric(vg_b) # gravity ref vector - dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading - return dhdx - - -class AHRSv2d: - """ - Attitude and Heading Reference System (AHRS) using a multiplicative extended - Kalman filter (MEKF). - - Parameters - ---------- - fs : float - Sampling rate in Hz. - q_nb : Attitude or array_like, shape (4,), optional - Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults - to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). - bg_b : array_like, shape (3,), optional - Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - dtheta : array_like, shape (3,), optional - Initial attitude change vector (coning integral). - P : array_like, shape (6, 6), optional - Initial (a priori) estimate of the error covariance matrix, **P**. If not - given, a small diagonal matrix will be used. - gyro_noise_density : float, optional - Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to - 0.00005 (SMS Motion 2 noise level). - gyro_bias_stability : float, optional - Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). - gyro_bias_corr_time : float, optional - Gyroscope bias correlation time in seconds. Defaults to 50.0 s. - nav_frame : {'NED', 'ENU'}, default 'NED' - Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) - (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom - will be expressed relative to this frame. - - """ - - _I: NDArray[np.float64] = np.eye(6) - - def __init__( - self, - fs: float, - q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), - bg_b: ArrayLike = (0.0, 0.0, 0.0), - P: ArrayLike = 1e-6 * np.eye(6), - gyro_noise_density: float = 0.0001, - gyro_bias_stability: float = 0.00005, - gyro_bias_corr_time: float = 50.0, - nav_frame: str = "NED", - ) -> None: - self._fs = fs - self._dt = 1.0 / fs - self._nav_frame = nav_frame.lower() - self._nz2vg = _nz2vg(self._nav_frame) - - # IMU noise parameters - self._arw = gyro_noise_density # angular random walk - self._gbs = gyro_bias_stability # gyro bias stability - self._gbc = gyro_bias_corr_time # gyro bias correlation time - - # State and covariance estimates - self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() - self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._P = np.asarray_chkfinite(P).reshape(6, 6).copy() - self._dx = np.zeros(6) - - # Discrete state-space model - self._phi = _state_transition(self._dt, np.zeros(3), self._gbc) - self._Q = _process_noise_cov(self._dt, self._arw, self._gbs, self._gbc) - self._dhdx = _measurement_matrix(self._q_nb, _vg_b(self._q_nb, self._nz2vg)) - - def quaternion(self) -> NDArray[np.float64]: - """ - Attitude expressed as a unit quaternion. - """ - return self._q_nb.copy() - - def euler(self, degrees: bool = False) -> NDArray[np.float64]: - """ - Attitude expressed as Euler angles (roll, pitch, yaw). - - Parameters - ---------- - degrees : bool, default False - Whether to return the Euler angles in degrees or radians. - - Returns - ------- - numpy.ndarray, shape (3,) - Euler angles (roll, pitch, yaw). - """ - - theta = _euler_from_quaternion(self._q_nb) - - if degrees: - theta = (180.0 / np.pi) * theta - - return theta - - def bias_gyro(self, degrees=False) -> NDArray[np.float64]: - """ - Gyroscope bias estimate (rad/s) expressed in the body frame. - - Parameters - ---------- - degrees : bool, optional - Whether to return the bias in deg/s or rad/s. Defaults to rad/s. - """ - bg_b = self._bg_b.copy() - if degrees: - bg_b = (180.0 / np.pi) * bg_b - return bg_b - - # def angular_rate(self, degrees=False) -> NDArray[np.float64]: - # """ - # Bias corrected angular rate measurement expressed in the body frame. - - # Parameters - # ---------- - # degrees : bool, optional - # Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. - # """ - # w_b = self._w_b.copy() - # if degrees: - # w_b = (180.0 / np.pi) * w_b - # return w_b - - @property - def P(self) -> NDArray[np.float64]: - """ - Copy of the error covariance matrix estimate. - """ - return self._P.copy() - - def _dhdx_gref(self, vg_b: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Gravity reference vector part of the measurement matrix, shape (3, 6). - """ - self._dhdx[0:3, 0:3] = _skew_symmetric(vg_b) - return self._dhdx[0:3] - - def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Heading (yaw angle) part of the measurement matrix, shape (6,). - """ - self._dhdx[3:4, 0:3] = _dhda_head(q_nb) - return self._dhdx[3] - - def _reset(self) -> None: - """ - Reset state. - """ - - if not self._dx.any(): - return - - _correct_quat_with_gibbs2(self._q_nb, self._dx[0:3]) - self._bg_b[:] += self._dx[3:6] - self._dx[:] = 0.0 - - def _aiding_update_gref( - self, vg_meas: ArrayLike | None, vg_var: ArrayLike | None - ) -> None: - """ - Update with gravity reference vector aiding measurement. - """ - - if vg_meas is None: - return None - - if vg_var is None: - raise ValueError("'vg_var' not provided.") - - vg_b = _vg_b(self._q_nb, self._nz2vg) - dz = vg_meas - vg_b - dhdx = self._dhdx_gref(vg_b) - _kalman_update_sequential(self._dx, self._P, dz, vg_var, dhdx, self._I) - - def _aiding_update_head( - self, head_meas: float | None, head_var: float | None, head_degrees: bool - ) -> None: - """ - Update with heading aiding measurement. - """ - - if head_meas is None: - return None - - if head_var is None: - raise ValueError("'head_var' not provided.") - - if head_degrees: - head_meas = (np.pi / 180.0) * head_meas - head_var = (np.pi / 180.0) ** 2 * head_var - - dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) - dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) - - def _project_ahead(self, dtheta) -> None: - """ - Project state and covariance estimates ahead. - """ - - # Attitude (dead reckoning) - _correct_quat_with_rotvec(self._q_nb, dtheta) - - # Covariance - self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) - - def update( - self, - dvel: ArrayLike, - dtheta: ArrayLike, - degrees: bool = False, - head: float | None = None, - head_var: float | None = None, - head_degrees: bool = False, - g_ref: bool = True, - g_var: ArrayLike | None = (0.001, 0.001, 0.001), - ) -> Self: - """ - Update state estimates with IMU and aiding measurements. - - Parameters - ---------- - dvel : array_like, shape (3,), optional - Initial velocity change vector (sculling integral). - dtheta : array_like, shape (3,), optional - Initial attitude change vector (coning integral). - degrees : bool, optional - Specifies whether the unit of the attitude change vector, ``dtheta``, - is degrees or radians. Defaults to radians. - head : float, optional - Heading measurement. I.e., the yaw angle of the 'body' frame relative to the - assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. - If ``None``, compass aiding is not used. See ``head_degrees`` for units. - head_var : float, optional - Variance of heading measurement noise. Units must be compatible with ``head``. - See ``head_degrees`` for units. Required for ``head``. - head_degrees : bool, default False - Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, - or radians and radians^2. Default is in radians and radians^2. - g_ref : bool, optional - Specifies whether the gravity reference vector is used as an aiding measurement. - g_var : array-like, optional - Variance of gravitational reference vector measurement noise. Required for - ``g_ref``. - - Returns - ------- - AHRS - A reference to the instance itself after the update. - """ - - dvel = np.asarray(dvel) - dtheta = np.asarray(dtheta) - - if degrees: - dtheta = np.radians(dtheta) - - dtheta -= self._dt * self._bg_b - - # Project (a priori) state and covariance estimates ahead - self._project_ahead(dtheta) - - # Update (a posteriori) state and covariance estimates with aiding measurements - self._aiding_update_gref(-_normalize(dvel) if g_ref else None, g_var) - self._aiding_update_head(head, head_var, head_degrees) - - # Reset state - self._reset() - - # Update model - _update_state_transition(self._phi, dtheta) - - return self From ce634865c02cd3eea03c344c3e3f80b503a796b5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:29:59 +0100 Subject: [PATCH 39/86] delete obsolete functions --- src/smsfusion/_v2.py | 34 ---------------------------------- 1 file changed, 34 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 7e31526a..3a28eae4 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -13,40 +13,6 @@ BG_IDX = slice(6, 9) -def _nz2vg(nav_frame: str) -> float: - """ - Gravity direction along the navigation frame's z-axis. - """ - if nav_frame == "ned": - return 1.0 - elif nav_frame == "enu": - return -1.0 - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - - -@njit # type: ignore[misc] -def _vg_b(q_nb: NDArray[np.float64], nz2vg: float) -> NDArray[np.float64]: - """ - Gravity reference vector expressed in the body frame, computed from a unit quaternion. - - Parameters - ---------- - q_nb : numpy.ndarray, shape (4,) - Unit quaternion. - nz2vg : float - Gravity direction along the navigation frame's z-axis. Should be +1 for - NED and -1 for ENU. - """ - qw, qx, qy, qz = q_nb - - x = 2.0 * (qx * qz - qw * qy) - y = 2.0 * (qy * qz + qw * qx) - z = 1.0 - 2.0 * (qx**2 + qy**2) - - return nz2vg * np.array([x, y, z]) - - @njit # type: ignore[misc] def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: """ From 32e4dfac41101550cba12be0a8a3efebb5a5f993 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:37:12 +0100 Subject: [PATCH 40/86] docstring fix --- src/smsfusion/_v2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 3a28eae4..6410e430 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -652,9 +652,9 @@ def update( Parameters ---------- dvel : array_like, shape (3,), optional - Initial velocity change vector (sculling integral). + Velocity change vector (sculling integral). dtheta : array_like, shape (3,), optional - Initial attitude change vector (coning integral). + Attitude change vector (coning integral). degrees : bool, optional Specifies whether the unit of the attitude change vector, ``dtheta``, is degrees or radians. Defaults to radians. From 137c2159c30c605950001bd0be3fd6027bc966fc Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:46:38 +0100 Subject: [PATCH 41/86] precalculate dvel_g_corr --- src/smsfusion/_v2.py | 36 ++++++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 6410e430..d1100b93 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -395,6 +395,31 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: return dhdx +def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]: + """ + Gravity vector expressed in the navigation frame ('NED' or 'ENU'). + + Parameters + ---------- + g : float + Gravitational acceleration in m/s^2. + nav_frame : {'NED', 'ENU'} + Navigation frame in which the gravity vector is expressed. + + Returns + ------- + ndarray, shape (3,) + Gravity vector expressed in the navigation frame. + """ + if nav_frame.lower() == "ned": + g_n = np.array([0.0, 0.0, g]) + elif nav_frame.lower() == "enu": + g_n = np.array([0.0, 0.0, -g]) + else: + raise ValueError(f"Unknown navigation frame: {nav_frame}.") + return g_n + + class AHRSv2: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended @@ -460,13 +485,8 @@ def __init__( self._dt = 1.0 / fs self._g = g self._nav_frame = nav_frame.lower() - - if self._nav_frame == "ned": - self._g_n = np.array([0.0, 0.0, g]) - elif self._nav_frame == "enu": - self._g_n = np.array([0.0, 0.0, -g]) - else: - raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + self._g_n = _gravity_nav(self._g, self._nav_frame) + self._dvel_g_corr = self._dt * self._g_n # IMU noise parameters self._vrw = acc_noise_density # velocity random walk @@ -627,7 +647,7 @@ def _project_ahead(self, dvel, dtheta) -> None: """ # Velocity (dead reckoning) - self._v_n[:] += self._R_nb @ dvel + self._dt * self._g_n + self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) _correct_quat_with_rotvec(self._q_nb, dtheta) From c853029f4679a5b9198a493f885444d0e44a2982 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 16:48:15 +0100 Subject: [PATCH 42/86] delete commented out method --- src/smsfusion/_v2.py | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index d1100b93..76a60045 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -556,20 +556,6 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]: bg_b = (180.0 / np.pi) * bg_b return bg_b - # def angular_rate(self, degrees=False) -> NDArray[np.float64]: - # """ - # Bias corrected angular rate measurement expressed in the body frame. - - # Parameters - # ---------- - # degrees : bool, optional - # Whether to return the angular rate in deg/s or rad/s. Defaults to rad/s. - # """ - # w_b = self._w_b.copy() - # if degrees: - # w_b = (180.0 / np.pi) * w_b - # return w_b - @property def P(self) -> NDArray[np.float64]: """ From 9f116e9a845ba4a655e4595ec9dfa6f3ec15b2c3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 17:10:08 +0100 Subject: [PATCH 43/86] fix correct quat with rotvec --- src/smsfusion/_v2.py | 79 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 78 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 76a60045..7b7fe54c 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -104,6 +104,83 @@ def _correct_quat_with_rotvec( q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) +# @njit # type: ignore[misc] +# def _correct_quat_with_rotvec_new( +# q: NDArray[np.float64], dtheta: NDArray[np.float64] +# ) -> None: +# """ +# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, +# parameterized as a rotation vector.: + +# q = q ⊗ dq(dtheta) + +# Parameters +# ---------- +# q : ndarray, shape (4,) +# Unit quaternion (modified in place). +# dtheta : ndarray, shape (3,) +# Small attitude change parameterized as a rotation vector. +# """ + +# rx, ry, rz = dtheta + +# half_angle = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0 + +# if half_angle >= 1e-5: +# psi = dtheta * np.sin(half_angle) / (2.0 * half_angle) +# else: +# psi = 0.5 * dtheta + +# cos_half_angle = np.cos(half_angle) + +# qw_new = cos_half_angle * q[0] - psi @ q[1:] + +# qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] + +# return np.array([qw_new, *qxyz_new]) + + +@njit # type: ignore[misc] +def _correct_quat_with_rotvec_new( + q: NDArray[np.float64], dtheta: NDArray[np.float64] +) -> None: + """ + Corrects a unit quaternion, q, with a small attitude change vector, dtheta, + parameterized as a rotation vector.: + + q = q ⊗ dq(dtheta) + + Parameters + ---------- + q : ndarray, shape (4,) + Unit quaternion (modified in place). + dtheta : ndarray, shape (3,) + Small attitude change parameterized as a rotation vector. + """ + + rx, ry, rz = dtheta + + norm = np.sqrt(rx**2 + ry**2 + rz**2) + half_angle = norm / 2.0 + + if half_angle >= 1e-5: + psi = dtheta * np.sin(half_angle) / norm + else: + psi = 0.5 * dtheta + + cos_half_angle = np.cos(half_angle) + + qw_new = cos_half_angle * q[0] - psi @ q[1:] + + qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] + + q[0] = qw_new + q[1] = qxyz_new[0] + q[2] = qxyz_new[1] + q[3] = qxyz_new[2] + + + @njit # type: ignore[misc] def _kalman_gain( P: NDArray[np.float64], h: NDArray[np.float64], r: float @@ -636,7 +713,7 @@ def _project_ahead(self, dvel, dtheta) -> None: self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) - _correct_quat_with_rotvec(self._q_nb, dtheta) + _correct_quat_with_rotvec_new(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From 9a9050a99f2b6a5a9655fabd319f92a463f72bae Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 17:18:57 +0100 Subject: [PATCH 44/86] black --- src/smsfusion/_v2.py | 84 +++++++++++++++++++++++++++++++++++--------- 1 file changed, 68 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 7b7fe54c..82d53852 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -140,45 +140,97 @@ def _correct_quat_with_rotvec( # return np.array([qw_new, *qxyz_new]) +# @njit # type: ignore[misc] +# def _correct_quat_with_rotvec_new( +# q: NDArray[np.float64], dtheta: NDArray[np.float64] +# ) -> None: +# """ +# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, +# parameterized as a rotation vector.: + +# q = q ⊗ dq(dtheta) + +# Parameters +# ---------- +# q : ndarray, shape (4,) +# Unit quaternion (modified in place). +# dtheta : ndarray, shape (3,) +# Small attitude change parameterized as a rotation vector. +# """ + +# rx, ry, rz = dtheta + +# norm = np.sqrt(rx**2 + ry**2 + rz**2) +# half_angle = norm / 2.0 + +# if half_angle >= 1e-5: +# psi = dtheta * np.sin(half_angle) / norm +# else: +# psi = 0.5 * dtheta + +# cos_half_angle = np.cos(half_angle) + +# qw_new = cos_half_angle * q[0] - psi @ q[1:] + +# qxyz_new = ( +# psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] +# ) + +# q[0] = qw_new +# q[1] = qxyz_new[0] +# q[2] = qxyz_new[1] +# q[3] = qxyz_new[2] + + @njit # type: ignore[misc] def _correct_quat_with_rotvec_new( q: NDArray[np.float64], dtheta: NDArray[np.float64] ) -> None: """ Corrects a unit quaternion, q, with a small attitude change vector, dtheta, - parameterized as a rotation vector.: + parameterized as a rotation vector - q = q ⊗ dq(dtheta) + q_{k+1} = M(dtheta) @ q_k Parameters ---------- q : ndarray, shape (4,) - Unit quaternion (modified in place). + Unit quaternion [w, x, y, z] (scalar first), modified in place. dtheta : ndarray, shape (3,) - Small attitude change parameterized as a rotation vector. + Delta-Theta rotation vector (radians). + + References + ---------- + .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1) """ rx, ry, rz = dtheta - norm = np.sqrt(rx**2 + ry**2 + rz**2) - half_angle = norm / 2.0 + gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0 - if half_angle >= 1e-5: - psi = dtheta * np.sin(half_angle) / norm + if gamma >= 1e-5: + psi = np.sin(gamma) / (2.0 * gamma) * dtheta else: psi = 0.5 * dtheta - cos_half_angle = np.cos(half_angle) + cos_gamma = np.cos(gamma) + p1, p2, p3 = psi - qw_new = cos_half_angle * q[0] - psi @ q[1:] - - qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] + qw_new = cos_gamma * q[0] - p1 * q[1] - p2 * q[2] - p3 * q[3] + qx_new = p1 * q[0] + cos_gamma * q[1] + p3 * q[2] - p2 * q[3] + qy_new = p2 * q[0] - p3 * q[1] + cos_gamma * q[2] + p1 * q[3] + qz_new = p3 * q[0] + p2 * q[1] - p1 * q[2] + cos_gamma * q[3] q[0] = qw_new - q[1] = qxyz_new[0] - q[2] = qxyz_new[1] - q[3] = qxyz_new[2] - + q[1] = qx_new + q[2] = qy_new + q[3] = qz_new + + norm = np.sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2) + q[0] /= norm + q[1] /= norm + q[2] /= norm + q[3] /= norm @njit # type: ignore[misc] From 9a6fee9e7c10bde50ead1d7a8551952cec94f431 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 17:36:24 +0100 Subject: [PATCH 45/86] some changes --- src/smsfusion/_v2.py | 31 ++++++++++++++----------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 82d53852..86111c8a 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -195,15 +195,21 @@ def _correct_quat_with_rotvec_new( Parameters ---------- q : ndarray, shape (4,) - Unit quaternion [w, x, y, z] (scalar first), modified in place. + Unit quaternion. dtheta : ndarray, shape (3,) - Delta-Theta rotation vector (radians). + Rotation vector. + + Returns + ------- + ndarray, shape (4,) + Updated unit quaternion after applying the rotation vector correction. References ---------- .. [1] https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-coning (Eq. 2.5.1) """ + qw, qx, qy, qz = q rx, ry, rz = dtheta gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0 @@ -216,21 +222,12 @@ def _correct_quat_with_rotvec_new( cos_gamma = np.cos(gamma) p1, p2, p3 = psi - qw_new = cos_gamma * q[0] - p1 * q[1] - p2 * q[2] - p3 * q[3] - qx_new = p1 * q[0] + cos_gamma * q[1] + p3 * q[2] - p2 * q[3] - qy_new = p2 * q[0] - p3 * q[1] + cos_gamma * q[2] + p1 * q[3] - qz_new = p3 * q[0] + p2 * q[1] - p1 * q[2] + cos_gamma * q[3] - - q[0] = qw_new - q[1] = qx_new - q[2] = qy_new - q[3] = qz_new + qw_new = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz + qx_new = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz + qy_new = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz + qz_new = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz - norm = np.sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2) - q[0] /= norm - q[1] /= norm - q[2] /= norm - q[3] /= norm + return _normalize(np.array((qw_new, qx_new, qy_new, qz_new))) @njit # type: ignore[misc] @@ -765,7 +762,7 @@ def _project_ahead(self, dvel, dtheta) -> None: self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) - _correct_quat_with_rotvec_new(self._q_nb, dtheta) + self._q_nb[:] = _correct_quat_with_rotvec_new(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From e63bdf8278989b0b1316b394314f4ad74b6c25c5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 13 Mar 2026 17:40:59 +0100 Subject: [PATCH 46/86] small fix --- src/smsfusion/_v2.py | 92 ++++++++++++++++++++++---------------------- 1 file changed, 46 insertions(+), 46 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 86111c8a..ff01d69d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -48,60 +48,60 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - q[:] = _normalize(q) -@njit # type: ignore[misc] -def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Compute the unit quaternion from a rotation vector. +# @njit # type: ignore[misc] +# def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: +# """ +# Compute the unit quaternion from a rotation vector. - Parameters - ---------- - r : numpy.ndarray, shape (3,) - Rotation vector (rx, ry, rz). +# Parameters +# ---------- +# r : numpy.ndarray, shape (3,) +# Rotation vector (rx, ry, rz). - Returns - ------- - numpy.ndarray, shape (4,) - Unit quaternion (qw, qx, qy, qz). - """ - # TODO: add reference +# Returns +# ------- +# numpy.ndarray, shape (4,) +# Unit quaternion (qw, qx, qy, qz). +# """ +# # TODO: add reference - rx, ry, rz = r +# rx, ry, rz = r - angle2 = rx**2 + ry**2 + rz**2 +# angle2 = rx**2 + ry**2 + rz**2 - if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) - a = 0.25 * angle2 - c = 1.0 - a / 2.0 - s = 0.5 * (1.0 - a / 6.0) - else: - angle = np.sqrt(angle2) - half_angle = 0.5 * angle - c = np.cos(half_angle) - s = np.sin(half_angle) / angle +# if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) +# a = 0.25 * angle2 +# c = 1.0 - a / 2.0 +# s = 0.5 * (1.0 - a / 6.0) +# else: +# angle = np.sqrt(angle2) +# half_angle = 0.5 * angle +# c = np.cos(half_angle) +# s = np.sin(half_angle) / angle - q = np.array([c, s * rx, s * ry, s * rz]) +# q = np.array([c, s * rx, s * ry, s * rz]) - return _normalize(q) +# return _normalize(q) -@njit # type: ignore[misc] -def _correct_quat_with_rotvec( - q: NDArray[np.float64], dtheta: NDArray[np.float64] -) -> None: - """ - Corrects a unit quaternion, q, with a small attitude change vector, dtheta, - parameterized as a rotation vector: +# @njit # type: ignore[misc] +# def _correct_quat_with_rotvec( +# q: NDArray[np.float64], dtheta: NDArray[np.float64] +# ) -> None: +# """ +# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, +# parameterized as a rotation vector: - q = q ⊗ dq(dtheta) +# q = q ⊗ dq(dtheta) - Parameters - ---------- - q : ndarray, shape (4,) - Unit quaternion (modified in place). - dtheta : ndarray, shape (3,) - Small attitude change parameterized as a rotation vector. - """ - q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) +# Parameters +# ---------- +# q : ndarray, shape (4,) +# Unit quaternion (modified in place). +# dtheta : ndarray, shape (3,) +# Small attitude change parameterized as a rotation vector. +# """ +# q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) # @njit # type: ignore[misc] @@ -183,9 +183,9 @@ def _correct_quat_with_rotvec( @njit # type: ignore[misc] -def _correct_quat_with_rotvec_new( +def _update_quaternion( q: NDArray[np.float64], dtheta: NDArray[np.float64] -) -> None: +) -> NDArray[np.float64]: """ Corrects a unit quaternion, q, with a small attitude change vector, dtheta, parameterized as a rotation vector @@ -762,7 +762,7 @@ def _project_ahead(self, dvel, dtheta) -> None: self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) - self._q_nb[:] = _correct_quat_with_rotvec_new(self._q_nb, dtheta) + self._q_nb[:] = _update_quaternion(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From 122a884267ddfd0a27866f6d41c8c499a3a19647 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 08:41:43 +0100 Subject: [PATCH 47/86] delete old commented out code --- src/smsfusion/_v2.py | 134 ------------------------------------------- 1 file changed, 134 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index ff01d69d..e0798d5e 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -48,140 +48,6 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - q[:] = _normalize(q) -# @njit # type: ignore[misc] -# def _quat_from_rotvec(r: NDArray[np.float64]) -> NDArray[np.float64]: -# """ -# Compute the unit quaternion from a rotation vector. - -# Parameters -# ---------- -# r : numpy.ndarray, shape (3,) -# Rotation vector (rx, ry, rz). - -# Returns -# ------- -# numpy.ndarray, shape (4,) -# Unit quaternion (qw, qx, qy, qz). -# """ -# # TODO: add reference - -# rx, ry, rz = r - -# angle2 = rx**2 + ry**2 + rz**2 - -# if angle2 < 1e-6: # 2nd order approximation (avoids division by zero) -# a = 0.25 * angle2 -# c = 1.0 - a / 2.0 -# s = 0.5 * (1.0 - a / 6.0) -# else: -# angle = np.sqrt(angle2) -# half_angle = 0.5 * angle -# c = np.cos(half_angle) -# s = np.sin(half_angle) / angle - -# q = np.array([c, s * rx, s * ry, s * rz]) - -# return _normalize(q) - - -# @njit # type: ignore[misc] -# def _correct_quat_with_rotvec( -# q: NDArray[np.float64], dtheta: NDArray[np.float64] -# ) -> None: -# """ -# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, -# parameterized as a rotation vector: - -# q = q ⊗ dq(dtheta) - -# Parameters -# ---------- -# q : ndarray, shape (4,) -# Unit quaternion (modified in place). -# dtheta : ndarray, shape (3,) -# Small attitude change parameterized as a rotation vector. -# """ -# q[:] = _normalize(_quaternion_product(q, _quat_from_rotvec(dtheta))) - - -# @njit # type: ignore[misc] -# def _correct_quat_with_rotvec_new( -# q: NDArray[np.float64], dtheta: NDArray[np.float64] -# ) -> None: -# """ -# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, -# parameterized as a rotation vector.: - -# q = q ⊗ dq(dtheta) - -# Parameters -# ---------- -# q : ndarray, shape (4,) -# Unit quaternion (modified in place). -# dtheta : ndarray, shape (3,) -# Small attitude change parameterized as a rotation vector. -# """ - -# rx, ry, rz = dtheta - -# half_angle = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0 - -# if half_angle >= 1e-5: -# psi = dtheta * np.sin(half_angle) / (2.0 * half_angle) -# else: -# psi = 0.5 * dtheta - -# cos_half_angle = np.cos(half_angle) - -# qw_new = cos_half_angle * q[0] - psi @ q[1:] - -# qxyz_new = psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] - -# return np.array([qw_new, *qxyz_new]) - - -# @njit # type: ignore[misc] -# def _correct_quat_with_rotvec_new( -# q: NDArray[np.float64], dtheta: NDArray[np.float64] -# ) -> None: -# """ -# Corrects a unit quaternion, q, with a small attitude change vector, dtheta, -# parameterized as a rotation vector.: - -# q = q ⊗ dq(dtheta) - -# Parameters -# ---------- -# q : ndarray, shape (4,) -# Unit quaternion (modified in place). -# dtheta : ndarray, shape (3,) -# Small attitude change parameterized as a rotation vector. -# """ - -# rx, ry, rz = dtheta - -# norm = np.sqrt(rx**2 + ry**2 + rz**2) -# half_angle = norm / 2.0 - -# if half_angle >= 1e-5: -# psi = dtheta * np.sin(half_angle) / norm -# else: -# psi = 0.5 * dtheta - -# cos_half_angle = np.cos(half_angle) - -# qw_new = cos_half_angle * q[0] - psi @ q[1:] - -# qxyz_new = ( -# psi * q[0] + cos_half_angle * np.eye(3) @ q[1:] - _skew_symmetric(psi) @ q[1:] -# ) - -# q[0] = qw_new -# q[1] = qxyz_new[0] -# q[2] = qxyz_new[1] -# q[3] = qxyz_new[2] - - @njit # type: ignore[misc] def _update_quaternion( q: NDArray[np.float64], dtheta: NDArray[np.float64] From 6a900d3183c212ab56c788531163409a60b15462 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 08:50:13 +0100 Subject: [PATCH 48/86] few changes --- src/smsfusion/_v2.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index e0798d5e..3bb456f6 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -6,7 +6,7 @@ from ._ins import _dhda_head, _h_head, _signed_smallest_angle from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion -from ._vectorops import _normalize, _quaternion_product, _skew_symmetric +from ._vectorops import _normalize, _skew_symmetric VEL_IDX = slice(0, 3) ATT_IDX = slice(3, 6) @@ -14,12 +14,10 @@ @njit # type: ignore[misc] -def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: +def _correct_quaternion_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: """ Corrects a unit quaternion, q, with a small attitude error, da, parameterized - as a scaled (2x) Gibbs vector: - - q = q ⊗ dq(da) + as a scaled (2x) Gibbs vector. As described in ref [1]_, this correction can be simplified by doing it in two steps: first a correction, followed by renormalization. The scaling factor becomes @@ -49,7 +47,7 @@ def _correct_quat_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) - @njit # type: ignore[misc] -def _update_quaternion( +def _update_quaternion_with_rotvec( q: NDArray[np.float64], dtheta: NDArray[np.float64] ) -> NDArray[np.float64]: """ @@ -576,7 +574,7 @@ def _reset(self) -> None: if not self._dx.any(): return - _correct_quat_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) + _correct_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) self._v_n[:] += self._dx[VEL_IDX] self._bg_b[:] += self._dx[BG_IDX] self._dx[:] = 0.0 @@ -628,7 +626,7 @@ def _project_ahead(self, dvel, dtheta) -> None: self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) - self._q_nb[:] = _update_quaternion(self._q_nb, dtheta) + self._q_nb[:] = _update_quaternion_with_rotvec(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From 2dc3856cac6ca355621cf4b6219cfccec457ab8f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 08:50:31 +0100 Subject: [PATCH 49/86] style --- src/smsfusion/_v2.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 3bb456f6..76ded857 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -14,7 +14,9 @@ @njit # type: ignore[misc] -def _correct_quaternion_with_gibbs2(q: NDArray[np.float64], da: NDArray[np.float64]) -> None: +def _correct_quaternion_with_gibbs2( + q: NDArray[np.float64], da: NDArray[np.float64] +) -> None: """ Corrects a unit quaternion, q, with a small attitude error, da, parameterized as a scaled (2x) Gibbs vector. From 82ece316c0fa19b3d8dd1dd890ef84afa4523106 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 08:52:15 +0100 Subject: [PATCH 50/86] delete repeded code --- src/smsfusion/_v2.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 76ded857..b251ab5d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -493,7 +493,6 @@ def __init__( self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy() self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy() - self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() self._dx = np.zeros(9) From 144f59ad693175cddc66ffb96cb1734c6621b140 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 08:54:57 +0100 Subject: [PATCH 51/86] store dvel_prev --- src/smsfusion/_v2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index b251ab5d..06bc657a 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -691,6 +691,8 @@ def update( self._reset() # Update model + self._dvel_prev[:] = dvel + self._dtheta_prev[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) _update_state_transition(self._phi, dvel, dtheta, self._R_nb) From 3cbbef61fdcba66f839626d20db60c1240cc3e2b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 09:02:08 +0100 Subject: [PATCH 52/86] some changes --- src/smsfusion/_v2.py | 34 ++++++++++++++-------------------- 1 file changed, 14 insertions(+), 20 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 06bc657a..d21b2f3d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -428,10 +428,10 @@ class AHRSv2: to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). bg_b : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - dvel_prev : array_like, shape (3,), optional - Previous velocity change vector measurement (sculling integral). - dtheta_prev : array_like, shape (3,), optional - Previous attitude change vector measurement (coning integral). + dvel : array_like, shape (3,), optional + Initial velocity change vector measurement (sculling integral). + dtheta : array_like, shape (3,), optional + Initial attitude change vector measurement (coning integral). P : array_like, shape (6, 6), optional Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a small diagonal matrix will be used. @@ -463,8 +463,8 @@ def __init__( v_n: ArrayLike = (0.0, 0.0, 0.0), q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), bg_b: ArrayLike = (0.0, 0.0, 0.0), - dvel_prev: ArrayLike = (0.0, 0.0, 0.0), - dtheta_prev: ArrayLike = (0.0, 0.0, 0.0), + dvel: ArrayLike = (0.0, 0.0, 0.0), + dtheta: ArrayLike = (0.0, 0.0, 0.0), P: ArrayLike = 1e-6 * np.eye(9), acc_noise_density: float = 0.0007, gyro_noise_density: float = 0.0001, @@ -491,14 +491,14 @@ def __init__( self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() self._R_nb = _rot_matrix_from_quaternion(self._q_nb) self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() - self._dvel_prev = np.asarray_chkfinite(dvel_prev).reshape(3).copy() - self._dtheta_prev = np.asarray_chkfinite(dtheta_prev).reshape(3).copy() + self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy() + self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() self._dx = np.zeros(9) # Discrete state-space model self._phi = _state_transition( - self._dt, self._dvel_prev, self._dtheta_prev, self._R_nb, self._gbc + self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc ) self._Q = _process_noise_cov( self._dt, self._vrw, self._arw, self._gbs, self._gbc @@ -672,16 +672,12 @@ def update( A reference to the instance itself after the update. """ - dvel = np.asarray(dvel) - dtheta = np.asarray(dtheta) - - if degrees: - dtheta = np.radians(dtheta) - - dtheta -= self._dt * self._bg_b + self._dvel[:] = dvel + self._dtheta[:] = np.degrees(dtheta) if degrees else dtheta + self._dtheta -= self._dt * self._bg_b # Project (a priori) state and covariance estimates ahead - self._project_ahead(dvel, dtheta) + self._project_ahead(self._dvel, self._dtheta) # Update (a posteriori) state and covariance estimates with aiding measurements self._aiding_update_vel(vel, vel_var) @@ -691,9 +687,7 @@ def update( self._reset() # Update model - self._dvel_prev[:] = dvel - self._dtheta_prev[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - _update_state_transition(self._phi, dvel, dtheta, self._R_nb) + _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) return self From f672efcdc93498f215c4b2ba26c0f26f068bfea2 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 09:06:21 +0100 Subject: [PATCH 53/86] dvel and dtheta methods --- src/smsfusion/_v2.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index d21b2f3d..c104970d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -547,6 +547,27 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]: bg_b = (180.0 / np.pi) * bg_b return bg_b + def dvel(self) -> NDArray[np.float64]: + """ + Previous velocity change vector measurement (sculling integral). + """ + return self._dvel.copy() + + def dtheta(self, degrees=False) -> NDArray[np.float64]: + """ + Previous attitude change vector measurement (coning integral). + + Parameters + ---------- + degrees : bool, optional + Whether to return the coning integral in degrees or radians. Defaults + to radians. + """ + dtheta = self._dtheta.copy() + if degrees: + dtheta = (180.0 / np.pi) * dtheta + return dtheta + @property def P(self) -> NDArray[np.float64]: """ From 4c9ade026024315ffc22e1621f48d83c5a97b7f2 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 14 Mar 2026 09:14:08 +0100 Subject: [PATCH 54/86] renaming --- src/smsfusion/_v2.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index c104970d..0a320709 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -421,12 +421,12 @@ class AHRSv2: ---------- fs : float Sampling rate in Hz. - v_n : array_like, shape (3,), optional + v : array_like, shape (3,), optional Initial velocity estimate in m/s. - q_nb : Attitude or array_like, shape (4,), optional + q : Attitude or array_like, shape (4,), optional Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). - bg_b : array_like, shape (3,), optional + bg : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. dvel : array_like, shape (3,), optional Initial velocity change vector measurement (sculling integral). @@ -460,9 +460,9 @@ class AHRSv2: def __init__( self, fs: float, - v_n: ArrayLike = (0.0, 0.0, 0.0), - q_nb: ArrayLike = (1.0, 0.0, 0.0, 0.0), - bg_b: ArrayLike = (0.0, 0.0, 0.0), + v: ArrayLike = (0.0, 0.0, 0.0), + q: ArrayLike = (1.0, 0.0, 0.0, 0.0), + bg: ArrayLike = (0.0, 0.0, 0.0), dvel: ArrayLike = (0.0, 0.0, 0.0), dtheta: ArrayLike = (0.0, 0.0, 0.0), P: ArrayLike = 1e-6 * np.eye(9), @@ -487,10 +487,10 @@ def __init__( self._gbc = gyro_bias_corr_time # gyro bias correlation time # State and covariance estimates - self._v_n = np.asarray_chkfinite(v_n).reshape(3).copy() - self._q_nb = np.asarray_chkfinite(q_nb).reshape(4).copy() + self._v_n = np.asarray_chkfinite(v).reshape(3).copy() + self._q_nb = np.asarray_chkfinite(q).reshape(4).copy() self._R_nb = _rot_matrix_from_quaternion(self._q_nb) - self._bg_b = np.asarray_chkfinite(bg_b).reshape(3).copy() + self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy() self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy() self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() From bfe3d18c476a559843ef7e41295fbccd35786192 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 08:00:01 +0100 Subject: [PATCH 55/86] small fix --- src/smsfusion/_v2.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 0a320709..2a653a84 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -41,10 +41,10 @@ def _correct_quaternion_with_gibbs2( qw, qx, qy, qz = q dax, day, daz = da - q[0] -= 0.5 * (qx * dax + qy * day + qz * daz) - q[1] += 0.5 * (qw * dax + qy * daz - qz * day) - q[2] += 0.5 * (qw * day - qx * daz + qz * dax) - q[3] += 0.5 * (qw * daz + qx * day - qy * dax) + q[0] = qw - 0.5 * (qx * dax + qy * day + qz * daz) + q[1] = qx + 0.5 * (qw * dax + qy * daz - qz * day) + q[2] = qy + 0.5 * (qw * day - qx * daz + qz * dax) + q[3] = qz + 0.5 * (qw * daz + qx * day - qy * dax) q[:] = _normalize(q) From 3fdbb0a281998a4bb94b5514ffeb2c0506a03936 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 08:19:55 +0100 Subject: [PATCH 56/86] some changes --- src/smsfusion/_v2.py | 53 ++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 29 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 2a653a84..1225a0f6 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -14,11 +14,11 @@ @njit # type: ignore[misc] -def _correct_quaternion_with_gibbs2( +def _update_quaternion_with_gibbs2( q: NDArray[np.float64], da: NDArray[np.float64] ) -> None: """ - Corrects a unit quaternion, q, with a small attitude error, da, parameterized + Update/correct a unit quaternion, q, with a small attitude error, da, parameterized as a scaled (2x) Gibbs vector. As described in ref [1]_, this correction can be simplified by doing it in two @@ -28,13 +28,13 @@ def _correct_quaternion_with_gibbs2( Parameters ---------- q : ndarray, shape (4,) - Unit quaternion [qw, qx, qy, qz] (modified in place). + Unit quaternion (qw, qx, qy, qz) to be updated (in place). da : ndarray, shape (3,) - Small attitude error parameterized as a scaled (2x) Gibbs vector. + Attitude error correction parameterized as a scaled (2x) Gibbs vector. References ---------- - Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination + .. [1] Markley & Crassidis (2014), Fundamentals of Spacecraft Attitude Determination and Control, Eq. (6.27)-(6.28). """ @@ -53,22 +53,15 @@ def _update_quaternion_with_rotvec( q: NDArray[np.float64], dtheta: NDArray[np.float64] ) -> NDArray[np.float64]: """ - Corrects a unit quaternion, q, with a small attitude change vector, dtheta, - parameterized as a rotation vector - - q_{k+1} = M(dtheta) @ q_k + Update a unit quaternion, q, with a small attitude increment, dtheta, parameterized + as a rotation vector. Parameters ---------- q : ndarray, shape (4,) - Unit quaternion. + Unit quaternion (qw, qx, qy, qz) to be updated (in place). dtheta : ndarray, shape (3,) - Rotation vector. - - Returns - ------- - ndarray, shape (4,) - Updated unit quaternion after applying the rotation vector correction. + Attitude increment (rotation vector). References ---------- @@ -78,22 +71,24 @@ def _update_quaternion_with_rotvec( qw, qx, qy, qz = q rx, ry, rz = dtheta - gamma = np.sqrt(rx**2 + ry**2 + rz**2) / 2.0 + gamma = 0.5 * np.sqrt(rx**2 + ry**2 + rz**2) + cos_gamma = np.cos(gamma) if gamma >= 1e-5: - psi = np.sin(gamma) / (2.0 * gamma) * dtheta + scale = np.sin(gamma) / (2.0 * gamma) else: - psi = 0.5 * dtheta - - cos_gamma = np.cos(gamma) - p1, p2, p3 = psi + scale = 0.5 - qw_new = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz - qx_new = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz - qy_new = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz - qz_new = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz + # Psi + p1 = scale * rx + p2 = scale * ry + p3 = scale * rz - return _normalize(np.array((qw_new, qx_new, qy_new, qz_new))) + q[0] = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz + q[1] = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz + q[2] = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz + q[3] = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz + q[:] = _normalize(q) @njit # type: ignore[misc] @@ -596,7 +591,7 @@ def _reset(self) -> None: if not self._dx.any(): return - _correct_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) + _update_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) self._v_n[:] += self._dx[VEL_IDX] self._bg_b[:] += self._dx[BG_IDX] self._dx[:] = 0.0 @@ -648,7 +643,7 @@ def _project_ahead(self, dvel, dtheta) -> None: self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr # Attitude (dead reckoning) - self._q_nb[:] = _update_quaternion_with_rotvec(self._q_nb, dtheta) + _update_quaternion_with_rotvec(self._q_nb, dtheta) # Covariance self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) From ecb87fff55f8d78b9bc577a1e1f76a50d7e4a2eb Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 08:28:57 +0100 Subject: [PATCH 57/86] small fix --- src/smsfusion/_v2.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 1225a0f6..a88502ef 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -232,7 +232,7 @@ def _project_cov_ahead( Parameters ---------- P : ndarray, shape (n, n) - State error covariance matrix to be projected ahead. + State error covariance matrix to be projected ahead (in place). phi : ndarray, shape (n, n) State transition matrix. Q : ndarray, shape (n, n) @@ -243,8 +243,7 @@ def _project_cov_ahead( ndarray, shape (n, n) Projected error covariance matrix estimate. """ - P = phi @ P @ phi.T + Q - return P + P[:, :] = phi @ P @ phi.T + Q def _state_transition( @@ -646,7 +645,7 @@ def _project_ahead(self, dvel, dtheta) -> None: _update_quaternion_with_rotvec(self._q_nb, dtheta) # Covariance - self._P[:, :] = _project_cov_ahead(self._P, self._phi, self._Q) + _project_cov_ahead(self._P, self._phi, self._Q) def update( self, From 42a31695ff9ef1915774e93804aeb00099d84f91 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 08:40:08 +0100 Subject: [PATCH 58/86] remove idx slices --- src/smsfusion/_v2.py | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index a88502ef..9062b3e4 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -8,10 +8,6 @@ from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion from ._vectorops import _normalize, _skew_symmetric -VEL_IDX = slice(0, 3) -ATT_IDX = slice(3, 6) -BG_IDX = slice(6, 9) - @njit # type: ignore[misc] def _update_quaternion_with_gibbs2( @@ -275,10 +271,10 @@ def _state_transition( State transition matrix. """ phi = np.eye(9) - phi[VEL_IDX, ATT_IDX] -= R_nb @ _skew_symmetric(dvel) # NB! update each time step - phi[ATT_IDX, ATT_IDX] -= _skew_symmetric(dtheta) # NB! update each time step - phi[ATT_IDX, BG_IDX] -= dt * np.eye(3) - phi[BG_IDX, BG_IDX] -= dt * np.eye(3) / gbc + phi[0:3, 3:6] -= R_nb @ _skew_symmetric(dvel) # NB! update each time step + phi[3:6, 3:6] -= _skew_symmetric(dtheta) # NB! update each time step + phi[3:6, 6:9] -= dt * np.eye(3) + phi[6:9, 6:9] -= dt * np.eye(3) / gbc return phi @@ -355,9 +351,9 @@ def _process_noise_cov( Process noise covariance matrix. """ Q = np.zeros((9, 9)) - Q[VEL_IDX, VEL_IDX] = dt * vrw**2 * np.eye(3) - Q[ATT_IDX, ATT_IDX] = dt * arw**2 * np.eye(3) - Q[BG_IDX, BG_IDX] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) + Q[0:3, 0:3] = dt * vrw**2 * np.eye(3) + Q[3:6, 3:6] = dt * arw**2 * np.eye(3) + Q[6:9, 6:9] = dt * (2.0 * gbs**2 / gbc) * np.eye(3) return Q @@ -376,8 +372,8 @@ def _measurement_matrix(q_nb: NDArray[np.float64]) -> NDArray[np.float64]: Linearized measurement matrix. """ dhdx = np.zeros((4, 9)) - dhdx[0:3, VEL_IDX] = np.eye(3) # velocity - dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) # heading + dhdx[0:3, 0:3] = np.eye(3) # velocity + dhdx[3:4, 3:6] = _dhda_head(q_nb) # heading return dhdx @@ -579,7 +575,7 @@ def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: """ Heading (yaw angle) part of the measurement matrix, shape (6,). """ - self._dhdx[3:4, ATT_IDX] = _dhda_head(q_nb) + self._dhdx[3:4, 3:6] = _dhda_head(q_nb) return self._dhdx[3] def _reset(self) -> None: @@ -590,9 +586,9 @@ def _reset(self) -> None: if not self._dx.any(): return - _update_quaternion_with_gibbs2(self._q_nb, self._dx[ATT_IDX]) - self._v_n[:] += self._dx[VEL_IDX] - self._bg_b[:] += self._dx[BG_IDX] + _update_quaternion_with_gibbs2(self._q_nb, self._dx[3:6]) + self._v_n[:] += self._dx[0:3] + self._bg_b[:] += self._dx[6:9] self._dx[:] = 0.0 def _aiding_update_vel( From b5d96e8c91fc70041e04f74de5ecd6d957083e8d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 09:16:02 +0100 Subject: [PATCH 59/86] docstring fix --- src/smsfusion/_v2.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 9062b3e4..cc1a4f09 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -426,19 +426,20 @@ class AHRSv2: Initial (a priori) estimate of the error covariance matrix, **P**. If not given, a small diagonal matrix will be used. acc_noise_density : float, optional - Accelerometer noise density (velocity random walk) in m/s/√Hz. Defaults to - 0.0007 (SMS Motion 2 noise level). + Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to + 0.0007 (m/s)/√Hz (SMS Motion 2 noise level). gyro_noise_density : float, optional Gyroscope noise density (angular random walk) in (rad/s)/√Hz. Defaults to - 0.00005 (SMS Motion 2 noise level). + 0.00005 (rad/s)/√Hz (SMS Motion 2 noise level). gyro_bias_stability : float, optional - Gyroscope bias stability in rad/s. Defaults to 0.00005 (SMS Motion 2 noise level). + Gyroscope bias stability in rad/s. Defaults to 0.00005 rad/s (SMS Motion 2 + noise level). gyro_bias_corr_time : float, optional Gyroscope bias correlation time in seconds. Defaults to 50.0 s. - g : float, default 9.80665 - The gravitational acceleration m/s^2. Default is 'standard gravity' of 9.80665 - m/s^2. - nav_frame : {'NED', 'ENU'}, default 'NED' + g : float, optional + The gravitational acceleration in m/s^2. Default is 'standard gravity' of + 9.80665 m/s^2. + nav_frame : {'NED', 'ENU'}, optional Specifies the assumed inertial-like 'navigation' frame. Should be 'NED' (North-East-Down) (default) or 'ENU' (East-North-Up). The body's (or IMU sensor's) degrees of freedom will be expressed relative to this frame. From ba368d10310d23e8bfa7bef084ca411e09d2fd6d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 16 Mar 2026 12:08:01 +0100 Subject: [PATCH 60/86] small change --- src/smsfusion/_v2.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index cc1a4f09..70fa23b3 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -685,7 +685,11 @@ def update( """ self._dvel[:] = dvel - self._dtheta[:] = np.degrees(dtheta) if degrees else dtheta + self._dtheta[:] = dtheta + + if degrees: + self._dtheta *= np.pi / 180.0 + self._dtheta -= self._dt * self._bg_b # Project (a priori) state and covariance estimates ahead From 8f3ff975efaf5bd0456b3d9a80cbbeccb9f4b47a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 17 Mar 2026 14:57:26 +0100 Subject: [PATCH 61/86] psi xyz --- src/smsfusion/_v2.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 70fa23b3..8400e6e7 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -76,14 +76,14 @@ def _update_quaternion_with_rotvec( scale = 0.5 # Psi - p1 = scale * rx - p2 = scale * ry - p3 = scale * rz - - q[0] = cos_gamma * qw - p1 * qx - p2 * qy - p3 * qz - q[1] = p1 * qw + cos_gamma * qx + p3 * qy - p2 * qz - q[2] = p2 * qw - p3 * qx + cos_gamma * qy + p1 * qz - q[3] = p3 * qw + p2 * qx - p1 * qy + cos_gamma * qz + px = scale * rx + py = scale * ry + pz = scale * rz + + q[0] = cos_gamma * qw - px * qx - py * qy - pz * qz + q[1] = px * qw + cos_gamma * qx + pz * qy - py * qz + q[2] = py * qw - pz * qx + cos_gamma * qy + px * qz + q[3] = pz * qw + py * qx - px * qy + cos_gamma * qz q[:] = _normalize(q) From ea457c6cb48039a76e5af9a4da0d6b8bd5519e5f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 09:40:13 +0100 Subject: [PATCH 62/86] velocity method --- src/smsfusion/_v2.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 8400e6e7..81f17cd7 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -501,6 +501,12 @@ def quaternion(self) -> NDArray[np.float64]: Attitude expressed as a unit quaternion. """ return self._q_nb.copy() + + def velocity(self) -> NDArray[np.float64]: + """ + Velocity expressed in the navigation frame. + """ + return self._v_n.copy() def euler(self, degrees: bool = False) -> NDArray[np.float64]: """ From 4618eb6244ac534c7c26f1c90bbde82a5f280be3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 09:40:19 +0100 Subject: [PATCH 63/86] test v2 --- tests/test_v2.py | 120 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 tests/test_v2.py diff --git a/tests/test_v2.py b/tests/test_v2.py new file mode 100644 index 00000000..e79dd048 --- /dev/null +++ b/tests/test_v2.py @@ -0,0 +1,120 @@ +import numpy as np +import pytest +from scipy.signal import resample_poly + +import smsfusion as sf +from smsfusion import AHRSv2 +from smsfusion.benchmark import ( + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, +) + + +class Test_v2: + @pytest.mark.parametrize( + "benchmark_gen", + [benchmark_full_pva_beat_202311A], #, benchmark_full_pva_chirp_202311A], + ) + def test_benchmark(self, benchmark_gen): + fs_imu = 100.0 + fs_aiding = 1.0 + fs_ratio = np.ceil(fs_imu / fs_aiding) + warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning + compass_noise_std = 0.5 + vel_noise_std = 0.1 + + # Reference signals (without noise) + t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) + + # IMU measurements (with noise) + err_acc_true = sf.constants.ERR_ACC_MOTION2 + err_gyro_true = sf.constants.ERR_GYRO_MOTION2 + bg = np.array([0.01, -0.02, 0.015]) + noise_model = sf.noise.IMUNoise( + err_acc=err_acc_true, err_gyro=err_gyro_true, seed=0 + ) + imu_noise = noise_model(fs_imu, len(t)) + acc_noise = acc_ref + imu_noise[:, :3] + gyro_noise = gyro_ref + imu_noise[:, 3:] + bg + + # Compass / heading (aiding) measurements + head_meas = euler_ref[:, 2] + sf.noise.white_noise( + compass_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=1 + ) + + # Velocity (aiding) measurements + vel_noise = np.column_stack( + [ + sf.noise.white_noise( + vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=5 + ), + sf.noise.white_noise( + vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=6 + ), + sf.noise.white_noise( + vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=7 + ), + ] + ) + vel_meas = vel_ref + vel_noise + + # MEKF + v0 = vel_ref[0] + q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) + mekf = AHRSv2( + fs_imu, + v=v0, + q=q0, + acc_noise_density=sf.constants.ERR_ACC_MOTION2["N"], + gyro_noise_density=sf.constants.ERR_GYRO_MOTION2["N"], + gyro_bias_stability=sf.constants.ERR_GYRO_MOTION2["B"], + gyro_bias_corr_time=sf.constants.ERR_GYRO_MOTION2["tau_cb"], + ) + + # Apply filter + vel_out, euler_out, bias_gyro_out = [], [], [] + for i, (f_i, w_i, v_i, h_i) in enumerate( + zip(acc_noise, gyro_noise, vel_meas, head_meas) + ): + if not (i % fs_ratio): # with aiding + mekf.update( + f_i / fs_imu, + w_i / fs_imu, + degrees=False, + vel=v_i, + vel_var=vel_noise_std**2 * np.ones(3), + head=h_i, + head_var=compass_noise_std**2, + head_degrees=True, + ) + else: # without aiding + mekf.update(f_i / fs_imu, w_i / fs_imu, degrees=False) + vel_out.append(mekf.velocity()) + euler_out.append(mekf.euler(degrees=False)) + bias_gyro_out.append(mekf.bias_gyro(degrees=False)) + + vel_out = np.array(vel_out) + euler_out = np.array(euler_out) + bias_gyro_out = np.array(bias_gyro_out) + + # Half-sample shift (compensates for the delay introduced by Euler integration) + vel_out = resample_poly(vel_out, 2, 1)[1:-1:2] + vel_ref = vel_ref[:-1, :] + euler_out = resample_poly(euler_out, 2, 1)[1:-1:2] + euler_ref = euler_ref[:-1, :] + + vel_x_rms, vel_y_rms, vel_z_rms = np.std((vel_out - vel_ref)[warmup:], axis=0) + roll_rms, pitch_rms, yaw_rms = np.std((euler_out - euler_ref)[warmup:], axis=0) + bias_gyro_x_rms, bias_gyro_y_rms, bias_gyro_z_rms = np.std( + (bias_gyro_out - bg)[warmup:], axis=0 + ) + + assert vel_x_rms <= 0.05 + assert vel_y_rms <= 0.05 + assert vel_z_rms <= 0.05 + assert np.degrees(roll_rms) <= 0.02 + assert np.degrees(pitch_rms) <= 0.02 + assert np.degrees(yaw_rms) <= 0.1 + assert np.degrees(bias_gyro_x_rms) <= 1e-3 + assert np.degrees(bias_gyro_y_rms) <= 1e-3 + assert np.degrees(bias_gyro_z_rms) <= 1e-3 From 4eab3e03bf7f339837870b6fb46abae2f72bdbef Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 09:43:53 +0100 Subject: [PATCH 64/86] adjust tolerance test --- src/smsfusion/_v2.py | 4 ++-- tests/test_v2.py | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 81f17cd7..0b9e5f1d 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -667,9 +667,9 @@ def update( Parameters ---------- dvel : array_like, shape (3,), optional - Velocity change vector (sculling integral). + Velocity change vector (sculling integral) in m/s. dtheta : array_like, shape (3,), optional - Attitude change vector (coning integral). + Attitude change vector (coning integral) in radians. degrees : bool, optional Specifies whether the unit of the attitude change vector, ``dtheta``, is degrees or radians. Defaults to radians. diff --git a/tests/test_v2.py b/tests/test_v2.py index e79dd048..55a39d8f 100644 --- a/tests/test_v2.py +++ b/tests/test_v2.py @@ -112,9 +112,9 @@ def test_benchmark(self, benchmark_gen): assert vel_x_rms <= 0.05 assert vel_y_rms <= 0.05 assert vel_z_rms <= 0.05 - assert np.degrees(roll_rms) <= 0.02 - assert np.degrees(pitch_rms) <= 0.02 - assert np.degrees(yaw_rms) <= 0.1 - assert np.degrees(bias_gyro_x_rms) <= 1e-3 - assert np.degrees(bias_gyro_y_rms) <= 1e-3 - assert np.degrees(bias_gyro_z_rms) <= 1e-3 + assert np.degrees(roll_rms) <= 0.1 + assert np.degrees(pitch_rms) <= 0.1 + assert np.degrees(yaw_rms) <= 0.2 + assert np.degrees(bias_gyro_x_rms) <= 0.005 + assert np.degrees(bias_gyro_y_rms) <= 0.005 + assert np.degrees(bias_gyro_z_rms) <= 0.005 From 53ef2d15d7f315fc6f2e95504f4b22f7e104b970 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 09:51:06 +0100 Subject: [PATCH 65/86] test fix head aiding --- src/smsfusion/_v2.py | 2 +- tests/test_v2.py | 44 +++++++++++++++++++------------------------- 2 files changed, 20 insertions(+), 26 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index 0b9e5f1d..a2483e14 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -501,7 +501,7 @@ def quaternion(self) -> NDArray[np.float64]: Attitude expressed as a unit quaternion. """ return self._q_nb.copy() - + def velocity(self) -> NDArray[np.float64]: """ Velocity expressed in the navigation frame. diff --git a/tests/test_v2.py b/tests/test_v2.py index 55a39d8f..0a7ee862 100644 --- a/tests/test_v2.py +++ b/tests/test_v2.py @@ -13,50 +13,34 @@ class Test_v2: @pytest.mark.parametrize( "benchmark_gen", - [benchmark_full_pva_beat_202311A], #, benchmark_full_pva_chirp_202311A], + [benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A], ) def test_benchmark(self, benchmark_gen): fs_imu = 100.0 fs_aiding = 1.0 fs_ratio = np.ceil(fs_imu / fs_aiding) warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning - compass_noise_std = 0.5 + compass_noise_std = np.radians(0.5) vel_noise_std = 0.1 # Reference signals (without noise) t, _, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) # IMU measurements (with noise) - err_acc_true = sf.constants.ERR_ACC_MOTION2 - err_gyro_true = sf.constants.ERR_GYRO_MOTION2 bg = np.array([0.01, -0.02, 0.015]) noise_model = sf.noise.IMUNoise( - err_acc=err_acc_true, err_gyro=err_gyro_true, seed=0 + err_acc=sf.constants.ERR_ACC_MOTION2, err_gyro=sf.constants.ERR_GYRO_MOTION2, seed=0 ) imu_noise = noise_model(fs_imu, len(t)) acc_noise = acc_ref + imu_noise[:, :3] gyro_noise = gyro_ref + imu_noise[:, 3:] + bg - # Compass / heading (aiding) measurements - head_meas = euler_ref[:, 2] + sf.noise.white_noise( - compass_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=1 + # Aiding measurements (with noise) + rng = np.random.default_rng(seed=42) + head_meas = euler_ref[:, 2] + compass_noise_std * rng.standard_normal( + euler_ref.shape[0] ) - - # Velocity (aiding) measurements - vel_noise = np.column_stack( - [ - sf.noise.white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=5 - ), - sf.noise.white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=6 - ), - sf.noise.white_noise( - vel_noise_std / np.sqrt(fs_aiding), fs_aiding, len(t), seed=7 - ), - ] - ) - vel_meas = vel_ref + vel_noise + vel_meas = vel_ref + vel_noise_std * rng.standard_normal(vel_ref.shape) # MEKF v0 = vel_ref[0] @@ -76,6 +60,16 @@ def test_benchmark(self, benchmark_gen): for i, (f_i, w_i, v_i, h_i) in enumerate( zip(acc_noise, gyro_noise, vel_meas, head_meas) ): + # mekf.update( + # f_i / fs_imu, + # w_i / fs_imu, + # degrees=False, + # vel=v_i, + # vel_var=vel_noise_std**2 * np.ones(3), + # head=h_i, + # head_var=compass_noise_std**2, + # head_degrees=False, + # ) if not (i % fs_ratio): # with aiding mekf.update( f_i / fs_imu, @@ -85,7 +79,7 @@ def test_benchmark(self, benchmark_gen): vel_var=vel_noise_std**2 * np.ones(3), head=h_i, head_var=compass_noise_std**2, - head_degrees=True, + head_degrees=False, ) else: # without aiding mekf.update(f_i / fs_imu, w_i / fs_imu, degrees=False) From 6471514681b788530db9b4352cde12ab311c7065 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 14:28:49 +0100 Subject: [PATCH 66/86] docstring --- src/smsfusion/_v2.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_v2.py index a2483e14..fb69610a 100644 --- a/src/smsfusion/_v2.py +++ b/src/smsfusion/_v2.py @@ -412,19 +412,19 @@ class AHRSv2: fs : float Sampling rate in Hz. v : array_like, shape (3,), optional - Initial velocity estimate in m/s. + Initial velocity estimate in m/s. Defaults to zero velocity (stationary). q : Attitude or array_like, shape (4,), optional Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). bg : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. dvel : array_like, shape (3,), optional - Initial velocity change vector measurement (sculling integral). + Initial velocity change vector measurement (sculling integral). Defaults to zero. dtheta : array_like, shape (3,), optional - Initial attitude change vector measurement (coning integral). + Initial attitude change vector measurement (coning integral). Defaults to zero. P : array_like, shape (6, 6), optional - Initial (a priori) estimate of the error covariance matrix, **P**. If not - given, a small diagonal matrix will be used. + Initial (a priori) estimate of the error covariance matrix. Defaults to + a small diagonal matrix (1e-6 * np.eye(9)). acc_noise_density : float, optional Accelerometer noise density (velocity random walk) in (m/s)/√Hz. Defaults to 0.0007 (m/s)/√Hz (SMS Motion 2 noise level). From f8ec4fb1ac37a9c6f966011ee088a26046452800 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 15:14:21 +0100 Subject: [PATCH 67/86] rename module to ins v2 --- src/smsfusion/__init__.py | 2 +- src/smsfusion/{_v2.py => _ins_v2.py} | 0 tests/{test_v2.py => test_ins_v2.py} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename src/smsfusion/{_v2.py => _ins_v2.py} (100%) rename tests/{test_v2.py => test_ins_v2.py} (100%) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 6dd89429..35b6b599 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -3,7 +3,7 @@ from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._v2 import AHRSv2 +from ._ins_v2 import AHRSv2 __all__ = [ "AHRS", diff --git a/src/smsfusion/_v2.py b/src/smsfusion/_ins_v2.py similarity index 100% rename from src/smsfusion/_v2.py rename to src/smsfusion/_ins_v2.py diff --git a/tests/test_v2.py b/tests/test_ins_v2.py similarity index 100% rename from tests/test_v2.py rename to tests/test_ins_v2.py From 35da43726f7e2e52cbc0a3c6bdd11c004621c458 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 15:21:32 +0100 Subject: [PATCH 68/86] remove eye prealloc --- src/smsfusion/_ins_v2.py | 21 +++++---------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index fb69610a..4156a9d3 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -125,7 +125,6 @@ def _covariance_update( k: NDArray[np.float64], h: NDArray[np.float64], r: float, - I_: NDArray[np.float64], ) -> None: """ Compute the updated state error covariance matrix estimate (Joseph form). @@ -140,10 +139,8 @@ def _covariance_update( Measurement matrix (row vector). r : float Scalar measurement noise variance. - I_ : ndarray, shape (n, n) - Identity matrix. """ - A = I_ - np.outer(k, h) + A = np.eye(k.size) - np.outer(k, h) P = A @ P @ A.T + r * np.outer(k, k) return P @@ -155,7 +152,6 @@ def _kalman_update_scalar( z: float, r: float, h: NDArray[np.float64], - I_: NDArray[np.float64], ) -> None: """ Scalar Kalman filter measurement update. @@ -172,8 +168,6 @@ def _kalman_update_scalar( Scalar measurement noise variance. h : ndarray, shape (n,) Measurement matrix (row vector). - I_ : ndarray, shape (n, n) - Identity matrix. """ # Kalman gain @@ -183,7 +177,7 @@ def _kalman_update_scalar( x[:] += k * (z - np.dot(h, x)) # Updated (a posteriori) covariance estimate (Joseph form) - P[:, :] = _covariance_update(P, k, h, r, I_) + P[:, :] = _covariance_update(P, k, h, r) @njit # type: ignore[misc] @@ -193,7 +187,6 @@ def _kalman_update_sequential( z: NDArray[np.float64], var: NDArray[np.float64], H: NDArray[np.float64], - I_: NDArray[np.float64], ) -> None: """ Sequential (one-at-a-time) Kalman filter measurement update. @@ -210,12 +203,10 @@ def _kalman_update_sequential( Measurement noise variances corresponding to each scalar measurement. H : ndarray, shape (m, n) Measurement matrix where each row corresponds to a scalar measurement model. - I_ : ndarray, shape (n, n) - Identity matrix. """ m = z.shape[0] for i in range(m): - _kalman_update_scalar(x, P, z[i], var[i], H[i], I_) + _kalman_update_scalar(x, P, z[i], var[i], H[i]) @njit # type: ignore[misc] @@ -446,8 +437,6 @@ class AHRSv2: """ - _I: NDArray[np.float64] = np.eye(9) - def __init__( self, fs: float, @@ -613,7 +602,7 @@ def _aiding_update_vel( dz = vel_meas - self._v_n dhdx = self._dhdx_vel() - _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx, self._I) + _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx) def _aiding_update_head( self, head_meas: float | None, head_var: float | None, head_degrees: bool @@ -634,7 +623,7 @@ def _aiding_update_head( dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx, self._I) + _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx) def _project_ahead(self, dvel, dtheta) -> None: """ From 63ec7cfa747f65a30e5867d2ed0f8e1a2a96cd6d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 15:22:29 +0100 Subject: [PATCH 69/86] docstring fix --- src/smsfusion/_ins_v2.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 4156a9d3..7d1b1c5d 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -224,11 +224,6 @@ def _project_cov_ahead( State transition matrix. Q : ndarray, shape (n, n) Process noise covariance matrix. - - Returns - ------- - ndarray, shape (n, n) - Projected error covariance matrix estimate. """ P[:, :] = phi @ P @ phi.T + Q From f246bd9e39d239d563c12e2ac56d04d2f282317f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 19 Mar 2026 15:46:28 +0100 Subject: [PATCH 70/86] docstring --- src/smsfusion/_ins_v2.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 7d1b1c5d..99bb0df4 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -125,7 +125,7 @@ def _covariance_update( k: NDArray[np.float64], h: NDArray[np.float64], r: float, -) -> None: +) -> NDArray[np.float64]: """ Compute the updated state error covariance matrix estimate (Joseph form). @@ -139,6 +139,11 @@ def _covariance_update( Measurement matrix (row vector). r : float Scalar measurement noise variance. + + Returns + ------- + ndarray, shape (n, n) + Updated state error covariance matrix. """ A = np.eye(k.size) - np.outer(k, h) P = A @ P @ A.T + r * np.outer(k, k) From f27053b1c0c7378f82ebed35d29c1416cf679cfe Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 20 Mar 2026 09:44:55 +0100 Subject: [PATCH 71/86] small fix --- src/smsfusion/_ins_v2.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 99bb0df4..887413de 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -679,16 +679,16 @@ def update( A reference to the instance itself after the update. """ - self._dvel[:] = dvel - self._dtheta[:] = dtheta + dvel = np.asarray(dvel) + dtheta = np.asarray(dtheta) if degrees: - self._dtheta *= np.pi / 180.0 + dtheta = np.radians(dtheta) - self._dtheta -= self._dt * self._bg_b + dtheta = dtheta - self._dt * self._bg_b # Project (a priori) state and covariance estimates ahead - self._project_ahead(self._dvel, self._dtheta) + self._project_ahead(dvel, dtheta) # Update (a posteriori) state and covariance estimates with aiding measurements self._aiding_update_vel(vel, vel_var) @@ -698,6 +698,8 @@ def update( self._reset() # Update model + self._dvel[:] = dvel + self._dtheta[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) From edcdc01156e0ab15acf4323746c6a157404d79b7 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Wed, 25 Mar 2026 20:22:35 +0100 Subject: [PATCH 72/86] check if aid is None --- src/smsfusion/_ins_v2.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 887413de..1c85eba5 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -594,9 +594,6 @@ def _aiding_update_vel( Update with velocity aiding measurement. """ - if vel_meas is None: - return None - if vel_var is None: raise ValueError("'vg_var' not provided.") @@ -611,9 +608,6 @@ def _aiding_update_head( Update with heading aiding measurement. """ - if head_meas is None: - return None - if head_var is None: raise ValueError("'head_var' not provided.") @@ -691,8 +685,10 @@ def update( self._project_ahead(dvel, dtheta) # Update (a posteriori) state and covariance estimates with aiding measurements - self._aiding_update_vel(vel, vel_var) - self._aiding_update_head(head, head_var, head_degrees) + if vel is not None: + self._aiding_update_vel(vel, vel_var) + if head is not None: + self._aiding_update_head(head, head_var, head_degrees) # Reset state self._reset() From 3bc96d7cf8d6f884d571d52a40dea32fa938c1c4 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 26 Mar 2026 11:27:00 +0100 Subject: [PATCH 73/86] remove dhdx methods --- src/smsfusion/__init__.py | 2 +- src/smsfusion/_ins_v2.py | 21 ++++----------------- tests/test_ins_v2.py | 4 +++- 3 files changed, 8 insertions(+), 19 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 35b6b599..de008f93 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,9 +1,9 @@ from . import benchmark, calibrate, constants, noise from ._coning_sculling import ConingScullingAlg from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity +from ._ins_v2 import AHRSv2 from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from ._ins_v2 import AHRSv2 __all__ = [ "AHRS", diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 1c85eba5..604491e8 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -561,19 +561,6 @@ def P(self) -> NDArray[np.float64]: """ return self._P.copy() - def _dhdx_vel(self) -> NDArray[np.float64]: - """ - Velocity part of the measurement matrix, shape (3, 6). - """ - return self._dhdx[0:3] - - def _dhdx_yaw(self, q_nb: NDArray[np.float64]) -> NDArray[np.float64]: - """ - Heading (yaw angle) part of the measurement matrix, shape (6,). - """ - self._dhdx[3:4, 3:6] = _dhda_head(q_nb) - return self._dhdx[3] - def _reset(self) -> None: """ Reset state. @@ -598,8 +585,7 @@ def _aiding_update_vel( raise ValueError("'vg_var' not provided.") dz = vel_meas - self._v_n - dhdx = self._dhdx_vel() - _kalman_update_sequential(self._dx, self._P, dz, vel_var, dhdx) + _kalman_update_sequential(self._dx, self._P, dz, vel_var, self._dhdx[0:3]) def _aiding_update_head( self, head_meas: float | None, head_var: float | None, head_degrees: bool @@ -615,9 +601,10 @@ def _aiding_update_head( head_meas = (np.pi / 180.0) * head_meas head_var = (np.pi / 180.0) ** 2 * head_var + self._dhdx[3, 3:6] = _dhda_head(self._q_nb) + dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) - dhdx = self._dhdx_yaw(self._q_nb) - _kalman_update_scalar(self._dx, self._P, dz, head_var, dhdx) + _kalman_update_scalar(self._dx, self._P, dz, head_var, self._dhdx[3]) def _project_ahead(self, dvel, dtheta) -> None: """ diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py index 0a7ee862..a6f7d54a 100644 --- a/tests/test_ins_v2.py +++ b/tests/test_ins_v2.py @@ -29,7 +29,9 @@ def test_benchmark(self, benchmark_gen): # IMU measurements (with noise) bg = np.array([0.01, -0.02, 0.015]) noise_model = sf.noise.IMUNoise( - err_acc=sf.constants.ERR_ACC_MOTION2, err_gyro=sf.constants.ERR_GYRO_MOTION2, seed=0 + err_acc=sf.constants.ERR_ACC_MOTION2, + err_gyro=sf.constants.ERR_GYRO_MOTION2, + seed=0, ) imu_noise = noise_model(fs_imu, len(t)) acc_noise = acc_ref + imu_noise[:, :3] From 40473d76b3806cf41c39b2f08aeb8758475fb8dd Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 26 Mar 2026 14:22:30 +0100 Subject: [PATCH 74/86] hide v2 --- src/smsfusion/__init__.py | 3 +-- tests/test_ins_v2.py | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index de008f93..4e91acfa 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,13 +1,12 @@ from . import benchmark, calibrate, constants, noise from ._coning_sculling import ConingScullingAlg from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity -from ._ins_v2 import AHRSv2 from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler +from . import _ins_v2 # hidden __all__ = [ "AHRS", - "AHRSv2", "AidedINS", "benchmark", "constants", diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py index a6f7d54a..3dcd7586 100644 --- a/tests/test_ins_v2.py +++ b/tests/test_ins_v2.py @@ -3,7 +3,7 @@ from scipy.signal import resample_poly import smsfusion as sf -from smsfusion import AHRSv2 +from smsfusion._ins_v2 import AHRSv2 from smsfusion.benchmark import ( benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A, From d531b5b3f51f607a9fecf970710182b44326945d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 26 Mar 2026 14:22:51 +0100 Subject: [PATCH 75/86] delete commented out code in test --- tests/test_ins_v2.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/tests/test_ins_v2.py b/tests/test_ins_v2.py index 3dcd7586..a21ddc75 100644 --- a/tests/test_ins_v2.py +++ b/tests/test_ins_v2.py @@ -62,16 +62,6 @@ def test_benchmark(self, benchmark_gen): for i, (f_i, w_i, v_i, h_i) in enumerate( zip(acc_noise, gyro_noise, vel_meas, head_meas) ): - # mekf.update( - # f_i / fs_imu, - # w_i / fs_imu, - # degrees=False, - # vel=v_i, - # vel_var=vel_noise_std**2 * np.ones(3), - # head=h_i, - # head_var=compass_noise_std**2, - # head_degrees=False, - # ) if not (i % fs_ratio): # with aiding mekf.update( f_i / fs_imu, From a137e1817e28631952ad0b098ae40c055dc70a1f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 11:02:11 +0100 Subject: [PATCH 76/86] raise fix --- src/smsfusion/_ins_v2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 604491e8..c20a354f 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -582,7 +582,7 @@ def _aiding_update_vel( """ if vel_var is None: - raise ValueError("'vg_var' not provided.") + raise ValueError("'vel_var' not provided.") dz = vel_meas - self._v_n _kalman_update_sequential(self._dx, self._P, dz, vel_var, self._dhdx[0:3]) From 838988cfa92c08f25149e2b81fd9c03b9a0b038b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 11:36:39 +0100 Subject: [PATCH 77/86] refactor project ahead --- src/smsfusion/_ins_v2.py | 57 +++++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index c20a354f..5dd9eb3f 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -233,6 +233,40 @@ def _project_cov_ahead( P[:, :] = phi @ P @ phi.T + Q +@njit # type: ignore[misc] +def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr): + """ + Project the state estimate ahead. + + Parameters + ---------- + dvel : ndarray, shape (3,) + Velocity change vector measurement (sculling integral). + dtheta : ndarray, shape (3,) + Attitude change vector measurement (coning integral). + v_n : ndarray, shape (3,) + Current velocity estimate expressed in the navigation frame. Will be projected + ahead (in place). + q_nb : ndarray, shape (4,) + Current attitude estimate parameterized as a unit quaternion. Will be projected + ahead (in place). + R_nb : ndarray, shape (3, 3) + Current rotation matrix from body to navigation frame. + dvel_g_corr : ndarray, shape (3,) + Gravity correction term for the velocity change vector. + + Returns + ------- + None + The state estimates are updated in place. + """ + # Velocity state estimate + v_n[:] += R_nb @ dvel + dvel_g_corr + + # Attitude state estimate + _update_quaternion_with_rotvec(q_nb, dtheta) + + def _state_transition( dt: float, dvel: NDArray[np.float64], @@ -606,20 +640,6 @@ def _aiding_update_head( dz = _signed_smallest_angle(head_meas - _h_head(self._q_nb)) _kalman_update_scalar(self._dx, self._P, dz, head_var, self._dhdx[3]) - def _project_ahead(self, dvel, dtheta) -> None: - """ - Project state and covariance estimates ahead. - """ - - # Velocity (dead reckoning) - self._v_n[:] += self._R_nb @ dvel + self._dvel_g_corr - - # Attitude (dead reckoning) - _update_quaternion_with_rotvec(self._q_nb, dtheta) - - # Covariance - _project_cov_ahead(self._P, self._phi, self._Q) - def update( self, dvel: ArrayLike, @@ -668,8 +688,13 @@ def update( dtheta = dtheta - self._dt * self._bg_b - # Project (a priori) state and covariance estimates ahead - self._project_ahead(dvel, dtheta) + # Project state estimates ahead (a priori) + _project_state_ahead( + dvel, dtheta, self._v_n, self._q_nb, self._R_nb, self._dvel_g_corr + ) + + # Project error covariance matrix estimate ahead (a priori) + _project_cov_ahead(self._P, self._phi, self._Q) # Update (a posteriori) state and covariance estimates with aiding measurements if vel is not None: From 3c50e8bc2d75dfcaf14c90f6474f749b196c0751 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 11:56:00 +0100 Subject: [PATCH 78/86] refactor reset --- src/smsfusion/_ins_v2.py | 46 +++++++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 5dd9eb3f..ce73f554 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -236,7 +236,7 @@ def _project_cov_ahead( @njit # type: ignore[misc] def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr): """ - Project the state estimate ahead. + Project the state estimate ahead (in place). Parameters ---------- @@ -254,11 +254,6 @@ def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr): Current rotation matrix from body to navigation frame. dvel_g_corr : ndarray, shape (3,) Gravity correction term for the velocity change vector. - - Returns - ------- - None - The state estimates are updated in place. """ # Velocity state estimate v_n[:] += R_nb @ dvel + dvel_g_corr @@ -427,6 +422,30 @@ def _gravity_nav(g: float, nav_frame: str) -> NDArray[np.float64]: return g_n +@njit # type: ignore[misc] +def _reset(v_n, q_nb, bg_b, dx) -> None: + """ + Reset state, moving information from the error-state estimate to the nominal + state estimates. + + Parameters + ---------- + v_n : ndarray, shape (3,) + Velocity state estimate to be reset in place. + q_nb : ndarray, shape (4,) + Attitude state estimate parameterized as a unit quaternion to be reset in place. + bg_b : ndarray, shape (3,) + Gyroscope bias state estimate to be reset in place. + dx : ndarray, shape (9,) + Error state vector containing the corrections to be applied to the state + estimates. Will be reset to zero after applying the corrections. + """ + _update_quaternion_with_gibbs2(q_nb, dx[3:6]) + v_n[:] += dx[0:3] + bg_b[:] += dx[6:9] + dx[:] = 0.0 + + class AHRSv2: """ Attitude and Heading Reference System (AHRS) using a multiplicative extended @@ -595,19 +614,6 @@ def P(self) -> NDArray[np.float64]: """ return self._P.copy() - def _reset(self) -> None: - """ - Reset state. - """ - - if not self._dx.any(): - return - - _update_quaternion_with_gibbs2(self._q_nb, self._dx[3:6]) - self._v_n[:] += self._dx[0:3] - self._bg_b[:] += self._dx[6:9] - self._dx[:] = 0.0 - def _aiding_update_vel( self, vel_meas: ArrayLike | None, vel_var: ArrayLike | None ) -> None: @@ -703,7 +709,7 @@ def update( self._aiding_update_head(head, head_var, head_degrees) # Reset state - self._reset() + _reset(self._v_n, self._q_nb, self._bg_b, self._dx) # Update model self._dvel[:] = dvel From 5fb27c7c92b103877438ae04096a7423a89d4892 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:03:50 +0100 Subject: [PATCH 79/86] small fix --- src/smsfusion/_ins_v2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index ce73f554..f6f58a65 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -440,8 +440,8 @@ def _reset(v_n, q_nb, bg_b, dx) -> None: Error state vector containing the corrections to be applied to the state estimates. Will be reset to zero after applying the corrections. """ - _update_quaternion_with_gibbs2(q_nb, dx[3:6]) v_n[:] += dx[0:3] + _update_quaternion_with_gibbs2(q_nb, dx[3:6]) bg_b[:] += dx[6:9] dx[:] = 0.0 From e0d68b044010dce06fc3023aa98c51a5fa3c83a1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:14:38 +0100 Subject: [PATCH 80/86] some changes --- src/smsfusion/__init__.py | 2 +- src/smsfusion/_ins_v2.py | 22 +++++++--------------- 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 4e91acfa..1da80bfc 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,9 +1,9 @@ +from . import _ins_v2 # hidden from . import benchmark, calibrate, constants, noise from ._coning_sculling import ConingScullingAlg from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler -from . import _ins_v2 # hidden __all__ = [ "AHRS", diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index f6f58a65..09486e86 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -234,33 +234,24 @@ def _project_cov_ahead( @njit # type: ignore[misc] -def _project_state_ahead(dvel, dtheta, v_n, q_nb, R_nb, dvel_g_corr): +def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr): """ - Project the state estimate ahead (in place). + Project velocity estimate ahead (in place). Parameters ---------- dvel : ndarray, shape (3,) Velocity change vector measurement (sculling integral). - dtheta : ndarray, shape (3,) - Attitude change vector measurement (coning integral). v_n : ndarray, shape (3,) Current velocity estimate expressed in the navigation frame. Will be projected ahead (in place). - q_nb : ndarray, shape (4,) - Current attitude estimate parameterized as a unit quaternion. Will be projected - ahead (in place). R_nb : ndarray, shape (3, 3) Current rotation matrix from body to navigation frame. dvel_g_corr : ndarray, shape (3,) Gravity correction term for the velocity change vector. """ - # Velocity state estimate v_n[:] += R_nb @ dvel + dvel_g_corr - # Attitude state estimate - _update_quaternion_with_rotvec(q_nb, dtheta) - def _state_transition( dt: float, @@ -694,10 +685,11 @@ def update( dtheta = dtheta - self._dt * self._bg_b - # Project state estimates ahead (a priori) - _project_state_ahead( - dvel, dtheta, self._v_n, self._q_nb, self._R_nb, self._dvel_g_corr - ) + # Project velocity estimate ahead (a priori) + _project_vel_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr) + + # Project attitude estimate ahead (a priori) + _update_quaternion_with_rotvec(self._q_nb, dtheta) # Project error covariance matrix estimate ahead (a priori) _project_cov_ahead(self._P, self._phi, self._Q) From 98570e87c082a3e71918d894b9210fb1f7b147ec Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:23:35 +0100 Subject: [PATCH 81/86] rename functions --- src/smsfusion/_ins_v2.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 09486e86..cfb5f8fa 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -215,7 +215,7 @@ def _kalman_update_sequential( @njit # type: ignore[misc] -def _project_cov_ahead( +def _project_covariance_ahead( P: NDArray[np.float64], phi: NDArray[np.float64], Q: NDArray[np.float64] ) -> None: """ @@ -234,7 +234,7 @@ def _project_cov_ahead( @njit # type: ignore[misc] -def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr): +def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr): """ Project velocity estimate ahead (in place). @@ -253,7 +253,7 @@ def _project_vel_ahead(dvel, v_n, R_nb, dvel_g_corr): v_n[:] += R_nb @ dvel + dvel_g_corr -def _state_transition( +def _state_transition_matrix( dt: float, dvel: NDArray[np.float64], dtheta: NDArray[np.float64], @@ -290,7 +290,7 @@ def _state_transition( @njit # type: ignore[misc] -def _update_state_transition( +def _update_state_transition_matrix( phi: NDArray[np.float64], dvel: NDArray[np.float64], dtheta: NDArray[np.float64], @@ -337,7 +337,7 @@ def _update_state_transition( phi[2, 5] = -(dvy * r20 - dvx * r21) -def _process_noise_cov( +def _process_noise_covariance_matrix( dt: float, vrw: float, arw: float, gbs: float, gbc: float ) -> NDArray[np.float64]: """ @@ -521,10 +521,10 @@ def __init__( self._dx = np.zeros(9) # Discrete state-space model - self._phi = _state_transition( + self._phi = _state_transition_matrix( self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc ) - self._Q = _process_noise_cov( + self._Q = _process_noise_covariance_matrix( self._dt, self._vrw, self._arw, self._gbs, self._gbc ) self._dhdx = _measurement_matrix(self._q_nb) @@ -686,13 +686,13 @@ def update( dtheta = dtheta - self._dt * self._bg_b # Project velocity estimate ahead (a priori) - _project_vel_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr) + _project_velocity_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr) # Project attitude estimate ahead (a priori) _update_quaternion_with_rotvec(self._q_nb, dtheta) # Project error covariance matrix estimate ahead (a priori) - _project_cov_ahead(self._P, self._phi, self._Q) + _project_covariance_ahead(self._P, self._phi, self._Q) # Update (a posteriori) state and covariance estimates with aiding measurements if vel is not None: @@ -707,6 +707,6 @@ def update( self._dvel[:] = dvel self._dtheta[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - _update_state_transition(self._phi, self._dvel, self._dtheta, self._R_nb) + _update_state_transition_matrix(self._phi, self._dvel, self._dtheta, self._R_nb) return self From 14978421e097af25b4ab75b1ee0a25ef080a0d6c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:39:16 +0100 Subject: [PATCH 82/86] comment --- src/smsfusion/_ins_v2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index cfb5f8fa..ae1f6104 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -703,7 +703,7 @@ def update( # Reset state _reset(self._v_n, self._q_nb, self._bg_b, self._dx) - # Update model + # Update state space model self._dvel[:] = dvel self._dtheta[:] = dtheta self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) From c27890e1e0e7ef9a8cd3657f4d3df1160e5a76e9 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:43:46 +0100 Subject: [PATCH 83/86] docstring fixes --- src/smsfusion/_ins_v2.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index ae1f6104..8184c44d 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -127,12 +127,12 @@ def _covariance_update( r: float, ) -> NDArray[np.float64]: """ - Compute the updated state error covariance matrix estimate (Joseph form). + Compute the updated error covariance matrix estimate (Joseph form). Parameters ---------- P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. + Error covariance matrix to be updated in place. k : ndarray, shape (n,) Kalman gain vector. h : ndarray, shape (n,) @@ -166,7 +166,7 @@ def _kalman_update_scalar( x : ndarray, shape (n,) State estimate to be updated in place. P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. + Error covariance matrix to be updated in place. z : float Scalar measurement. r : float @@ -201,7 +201,7 @@ def _kalman_update_sequential( x : ndarray, shape (n,) State estimate to be updated in place. P : ndarray, shape (n, n) - State error covariance matrix to be updated in place. + Error covariance matrix to be updated in place. z : ndarray, shape (m,) Measurement vector. var : ndarray, shape (m,) @@ -224,7 +224,7 @@ def _project_covariance_ahead( Parameters ---------- P : ndarray, shape (n, n) - State error covariance matrix to be projected ahead (in place). + Error covariance matrix to be projected ahead (in place). phi : ndarray, shape (n, n) State transition matrix. Q : ndarray, shape (n, n) From c297bb1188d4edbf648c459e9aea48f97b970108 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 27 Mar 2026 14:56:35 +0100 Subject: [PATCH 84/86] docstring fix --- src/smsfusion/_ins_v2.py | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index 8184c44d..ca3be52e 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -241,14 +241,14 @@ def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr): Parameters ---------- dvel : ndarray, shape (3,) - Velocity change vector measurement (sculling integral). + Velocity increment measurement (sculling integral). v_n : ndarray, shape (3,) - Current velocity estimate expressed in the navigation frame. Will be projected - ahead (in place). + Current velocity estimate expressed in the navigation frame (projected ahead + in place). R_nb : ndarray, shape (3, 3) Current rotation matrix from body to navigation frame. dvel_g_corr : ndarray, shape (3,) - Gravity correction term for the velocity change vector. + Gravity correction. """ v_n[:] += R_nb @ dvel + dvel_g_corr @@ -268,9 +268,9 @@ def _state_transition_matrix( dt : float Time step in seconds. dvel : ndarray, shape (3,) - Velocity change vector (sculling integral). + Velocity increment measurement (sculling integral). dtheta : ndarray, shape (3,) - Attitude change vector (coning integral). + Attitude increment measurement (coning integral). R_nb : ndarray, shape (3, 3) Rotation matrix from body to navigation frame. gbc : float @@ -304,9 +304,9 @@ def _update_state_transition_matrix( phi : ndarray, shape (9, 9) State transition matrix to be updated in place. dvel : ndarray, shape (3,) - Velocity change vector (sculling integral). + Velocity increment measurement (sculling integral). dtheta : ndarray, shape (3,) - Attitude change vector (coning integral). + Attitude increment measurement (coning integral). R_nb : ndarray, shape (3, 3) Rotation matrix from body to navigation frame. """ @@ -454,9 +454,9 @@ class AHRSv2: bg : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. dvel : array_like, shape (3,), optional - Initial velocity change vector measurement (sculling integral). Defaults to zero. + Initial velocity increment (sculling integral). Defaults to zero (stationary). dtheta : array_like, shape (3,), optional - Initial attitude change vector measurement (coning integral). Defaults to zero. + Initial attitude increment (coning integral). Defaults to zero (stationary). P : array_like, shape (6, 6), optional Initial (a priori) estimate of the error covariance matrix. Defaults to a small diagonal matrix (1e-6 * np.eye(9)). @@ -579,13 +579,13 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]: def dvel(self) -> NDArray[np.float64]: """ - Previous velocity change vector measurement (sculling integral). + Previous velocity increment measurement (sculling integral). """ return self._dvel.copy() def dtheta(self, degrees=False) -> NDArray[np.float64]: """ - Previous attitude change vector measurement (coning integral). + Previous bias corrected attitude increment (coning integral). Parameters ---------- @@ -654,12 +654,12 @@ def update( Parameters ---------- dvel : array_like, shape (3,), optional - Velocity change vector (sculling integral) in m/s. + Velocity increment (sculling integral) in m/s. dtheta : array_like, shape (3,), optional - Attitude change vector (coning integral) in radians. + Attitude increment (coning integral) in radians. degrees : bool, optional - Specifies whether the unit of the attitude change vector, ``dtheta``, - is degrees or radians. Defaults to radians. + Specifies whether the unit of the attitude increment, ``dtheta``, is + degrees or radians. Defaults to radians. head : float, optional Heading measurement. I.e., the yaw angle of the 'body' frame relative to the assumed 'navigation' frame ('NED' or 'ENU') specified during initialization. From 9b4d080e4dda728143f44a863f870c94398bf505 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 28 Mar 2026 09:19:53 +0100 Subject: [PATCH 85/86] remove need for dvel and dtheta attributes --- src/smsfusion/_ins_v2.py | 43 ++++++---------------------------------- 1 file changed, 6 insertions(+), 37 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index ca3be52e..e5c0334b 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -453,10 +453,6 @@ class AHRSv2: to the identity quaternion (1.0, 0.0, 0.0, 0.0) (i.e., no rotation). bg : array_like, shape (3,), optional Initial gyroscope bias estimate (bgx, bgy, bgz) in rad/s. Defaults to zero bias. - dvel : array_like, shape (3,), optional - Initial velocity increment (sculling integral). Defaults to zero (stationary). - dtheta : array_like, shape (3,), optional - Initial attitude increment (coning integral). Defaults to zero (stationary). P : array_like, shape (6, 6), optional Initial (a priori) estimate of the error covariance matrix. Defaults to a small diagonal matrix (1e-6 * np.eye(9)). @@ -487,8 +483,6 @@ def __init__( v: ArrayLike = (0.0, 0.0, 0.0), q: ArrayLike = (1.0, 0.0, 0.0, 0.0), bg: ArrayLike = (0.0, 0.0, 0.0), - dvel: ArrayLike = (0.0, 0.0, 0.0), - dtheta: ArrayLike = (0.0, 0.0, 0.0), P: ArrayLike = 1e-6 * np.eye(9), acc_noise_density: float = 0.0007, gyro_noise_density: float = 0.0001, @@ -515,14 +509,12 @@ def __init__( self._q_nb = np.asarray_chkfinite(q).reshape(4).copy() self._R_nb = _rot_matrix_from_quaternion(self._q_nb) self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy() - self._dvel = np.asarray_chkfinite(dvel).reshape(3).copy() - self._dtheta = np.asarray_chkfinite(dtheta).reshape(3).copy() self._P = np.asarray_chkfinite(P).reshape(9, 9).copy() self._dx = np.zeros(9) # Discrete state-space model self._phi = _state_transition_matrix( - self._dt, self._dvel, self._dtheta, self._R_nb, self._gbc + self._dt, np.zeros(3), np.zeros(3), self._R_nb, self._gbc ) self._Q = _process_noise_covariance_matrix( self._dt, self._vrw, self._arw, self._gbs, self._gbc @@ -577,27 +569,6 @@ def bias_gyro(self, degrees=False) -> NDArray[np.float64]: bg_b = (180.0 / np.pi) * bg_b return bg_b - def dvel(self) -> NDArray[np.float64]: - """ - Previous velocity increment measurement (sculling integral). - """ - return self._dvel.copy() - - def dtheta(self, degrees=False) -> NDArray[np.float64]: - """ - Previous bias corrected attitude increment (coning integral). - - Parameters - ---------- - degrees : bool, optional - Whether to return the coning integral in degrees or radians. Defaults - to radians. - """ - dtheta = self._dtheta.copy() - if degrees: - dtheta = (180.0 / np.pi) * dtheta - return dtheta - @property def P(self) -> NDArray[np.float64]: """ @@ -685,8 +656,12 @@ def update( dtheta = dtheta - self._dt * self._bg_b + # Update state-space model + R_nb = _rot_matrix_from_quaternion(self._q_nb) + _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb) + # Project velocity estimate ahead (a priori) - _project_velocity_ahead(dvel, self._v_n, self._R_nb, self._dvel_g_corr) + _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr) # Project attitude estimate ahead (a priori) _update_quaternion_with_rotvec(self._q_nb, dtheta) @@ -703,10 +678,4 @@ def update( # Reset state _reset(self._v_n, self._q_nb, self._bg_b, self._dx) - # Update state space model - self._dvel[:] = dvel - self._dtheta[:] = dtheta - self._R_nb[:] = _rot_matrix_from_quaternion(self._q_nb) - _update_state_transition_matrix(self._phi, self._dvel, self._dtheta, self._R_nb) - return self From 7f17d488f06872a28a63640f96a64e8a2773e5f8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sat, 28 Mar 2026 09:21:21 +0100 Subject: [PATCH 86/86] small fix --- src/smsfusion/_ins_v2.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_ins_v2.py b/src/smsfusion/_ins_v2.py index e5c0334b..f56ac3b6 100644 --- a/src/smsfusion/_ins_v2.py +++ b/src/smsfusion/_ins_v2.py @@ -660,13 +660,11 @@ def update( R_nb = _rot_matrix_from_quaternion(self._q_nb) _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb) - # Project velocity estimate ahead (a priori) + # Project (a priori) state estimates ahead _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr) - - # Project attitude estimate ahead (a priori) _update_quaternion_with_rotvec(self._q_nb, dtheta) - # Project error covariance matrix estimate ahead (a priori) + # Project (a priori) error covariance matrix estimate ahead _project_covariance_ahead(self._P, self._phi, self._Q) # Update (a posteriori) state and covariance estimates with aiding measurements