diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 48ec7ea4..e00d3aa1 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,5 +1,5 @@ from . import benchmark, calibrate, constants, noise -from ._coning_sculling import ConingScullingAlg +from ._coning_sculling import ConingScullingAlg, ConingScullingAlgCalibrated from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler @@ -18,4 +18,5 @@ "VRU", "quaternion_from_euler", "ConingScullingAlg", + "ConingScullingAlgCalibrated", ] diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py index b4c676cd..ab626023 100644 --- a/src/smsfusion/_coning_sculling.py +++ b/src/smsfusion/_coning_sculling.py @@ -1,7 +1,7 @@ import numpy as np from numpy.typing import ArrayLike -from smsfusion._vectorops import _cross +from smsfusion._vectorops import _adjugate_and_det_3_by_3, _cross class ConingScullingAlg: @@ -13,8 +13,8 @@ class ConingScullingAlg: Can be used in a strapdown algorithm as: - vel[m+1] = vel[m] + R(q[m]) @ dvel[m] + dvel_corr - q[m+1] = q[m] ⊗ dq(dtheta[m]) + vel[m+1] = vel[m] + R(q[m]) @ dvel[m+1] + dvel_corr + q[m+1] = q[m] ⊗ dq(dtheta[m+1]) where, @@ -23,11 +23,11 @@ class ConingScullingAlg: and, - - dvel[m] is the sculling integral, i.e., the velocity vector change (no gravity + - dvel[m+1] is the sculling integral, i.e., the velocity vector change (no gravity correction) from time step m to m+1. - - dtheta[m] is the coning integral, i.e., the rotation vector change from time + - dtheta[m+1] is the coning integral, i.e., the rotation vector change from time step m to m+1. - - dq(dtheta[m]) is the unit quaternion representation of the rotation increment + - dq(dtheta[m+1]) is the unit quaternion representation of the rotation increment over the interval [m, m+1]. - R(q[m]) is the rotation matrix (body-to-nav) corresponding to the attitude quaternion q[m]. @@ -106,32 +106,20 @@ def update(self, f: ArrayLike, w: ArrayLike, degrees: bool = False): dv_prev[:] = dv dtheta_prev[:] = dtheta - def dtheta(self, degrees=False): - """ - Peek at the accumulated 'body attitude change' vector. I.e., the rotation - vector describing the total rotation over all samples since initialization - (or last reset). - - Parameters - ---------- - degrees : bool, default False - Specifies whether the returned rotation vector should be in degrees - or radians (default). - """ - dtheta = self._theta + self._dtheta_con - return np.degrees(dtheta) if degrees else dtheta - @property def _dvel_rot(self): return 0.5 * _cross(self._theta, self._vel) - def dvel(self): + def _calc_dtheta_dvel(self, degrees=False): """ - Peek at the accumulated specific force velocity change vector. I.e., - the total change in velocity (no gravity correction) over all samples since - initialization (or last reset). + Calculate the coning and sculling corrected dtheta and dvel. """ - return self._vel + self._dvel_rot + self._dvel_scul + dtheta = self._theta + self._dtheta_con + dtheta = np.degrees(dtheta) if degrees else dtheta + # Equation (7.2.2.2-23) in ref [2]_ + dvel = self._vel + self._dvel_rot + self._dvel_scul + + return dtheta, dvel def flush(self, degrees=False): """ @@ -148,15 +136,93 @@ def flush(self, degrees=False): Returns ------- dtheta : ndarray, shape (3,) - The accumulated 'body attitude change' vector, see :meth:`dtheta`. + The accumulated 'body attitude change' vector. I.e., the rotation vector + describing the total rotation over all samples since initialization (or + last reset). dvel : ndarray, shape (3,) - The accumulated specific force velocity change vector, see :meth:`dvel`. + The accumulated specific force velocity change vector. I.e., the total change + in velocity (no gravity correction) over all samples since initialization + (or last reset). """ - dtheta = self.dtheta(degrees=degrees) - dvel = self.dvel() + dtheta, dvel = self._calc_dtheta_dvel(degrees) self._theta[:] = np.zeros(3, dtype=float) self._dtheta_con[:] = np.zeros(3, dtype=float) self._dvel_scul[:] = np.zeros(3, dtype=float) self._vel[:] = np.zeros(3, dtype=float) return dtheta, dvel + + +class ConingScullingAlgCalibrated(ConingScullingAlg): + """Extension of :class:`ConingScullingAlg` that applies a calibration matrix and + bias correction to the measurements while minimizing the number of operations. See + :class:`ConingScullingAlg` for full API and more algorithm details. + + Parameters + ---------- + fs : float + Sampling frequency of the measurements (Hz). + W_w : array-like, shape (3, 3), optional + Gyroscope calibration matrix (default: identity). + W_f : array-like, shape (3, 3), optional + Accelerometer calibration matrix (default: identity). + b_w : array-like, shape (3,), optional + Gyroscope bias vector (default: zero). + b_f : array-like, shape (3,), optional + Accelerometer bias vector (default: zero). + bias_alt : bool, default False + If set to ``True``, the bias definition of the alternative calibration model + is returned. See Notes. + + Notes + ----- + The calibration model is defined as:: + + xyz_ref = W @ xyz + bias + + The alternative calibration model where biases are added first is defined as:: + + xyz_ref = W @ (xyz + bias) + + The alternative model is enabled by setting ``bias_alt=True``. + """ + + def __init__( + self, + fs, + W_w: np.ndarray = np.eye(3), + W_f: np.ndarray = np.eye(3), + b_w: np.ndarray = np.zeros(3), + b_f: np.ndarray = np.zeros(3), + bias_alt: bool = False, + ): + adj_W_w, W_w_det = _adjugate_and_det_3_by_3(W_w) + if W_w_det == 0: + raise ValueError("W_w must be invertible") + W_w_inv = adj_W_w / W_w_det + self.cof_W = W_w_inv.T * W_w_det + self.W_star = W_w_inv @ W_f + if bias_alt: + self.b_f_star = b_f + self.b_w_star = b_w + else: + adj_W_f, W_f_det = _adjugate_and_det_3_by_3(W_f) + if W_f_det == 0: + raise ValueError("W_f must be invertible.") + W_f_inv = adj_W_f / W_f_det + self.b_f_star = W_f_inv @ b_f + self.b_w_star = W_w_inv @ b_w + self.W_w = W_w + super().__init__(fs) + + def update(self, f, w, degrees=False): + f_adjusted = self.W_star @ (f + self.b_f_star) + w_adjusted = w + self.b_w_star + super().update(f_adjusted, w_adjusted, degrees) + + def _calc_dtheta_dvel(self, degrees=False): + dtheta = self.W_w @ self._theta + self.cof_W @ self._dtheta_con + dtheta = np.degrees(dtheta) if degrees else dtheta + + dvel = self.W_w @ self._vel + self.cof_W @ (self._dvel_rot + self._dvel_scul) + return dtheta, dvel diff --git a/src/smsfusion/_vectorops.py b/src/smsfusion/_vectorops.py index 99ded2e9..072bd167 100644 --- a/src/smsfusion/_vectorops.py +++ b/src/smsfusion/_vectorops.py @@ -90,3 +90,47 @@ def _skew_symmetric(a: NDArray[np.float64]) -> NDArray[np.float64]: Skew symmetric matrix. """ return np.array([[0.0, -a[2], a[1]], [a[2], 0.0, -a[0]], [-a[1], a[0], 0.0]]) + + +@njit # type: ignore[misc] +def _adjugate_and_det_3_by_3( + m: NDArray[np.float64], +) -> tuple[NDArray[np.float64], float]: + """ + Calculates and returns the adjugate matrix and determinant of the matrix + ```python + m = [[a, b, c], + [d, e, f], + [g, h, i]] + ``` + If the determinant is non-zero, one can calculate the inverse of m as + ``inv(m) = adj(m) / det(m)``. + + Parameters + ---------- + m : numpy.ndarray, shape (3, 3) + The matrix for which to calculate the adjugate and determinant. + + Returns + ------- + adj_m : numpy.ndarray, shape (3, 3) + The adjugate of the input matrix m. + det_m : float + The determinant of the input matrix m. + """ + a, b, c = m[0] + d, e, f = m[1] + g, h, i = m[2] + A = e * i - f * h + B = -(d * i - f * g) + C = d * h - e * g + D = -(b * i - c * h) + E = a * i - c * g + F = -(a * h - b * g) + G = b * f - c * e + H = -(a * f - c * d) + I_ = a * e - b * d + # Equations fetched from https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices + adj_m = np.array([[A, D, G], [B, E, H], [C, F, I_]]) + det_m = a * A + b * B + c * C + return adj_m, det_m diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py index 44899ea7..709a73b9 100644 --- a/tests/test_coning_sculling.py +++ b/tests/test_coning_sculling.py @@ -1,5 +1,4 @@ from pathlib import Path -from turtle import down import numpy as np import pytest @@ -77,14 +76,17 @@ def test__init__(self): np.testing.assert_allclose(alg._dvel_scul, np.zeros(3)) np.testing.assert_allclose(alg._dv_prev, np.zeros(3)) - def test_update(self, data_ag, data_dtheta_dvel): + @pytest.mark.parametrize( + "algorithm", [sf.ConingScullingAlg, sf.ConingScullingAlgCalibrated] + ) + def test_update(self, data_ag, data_dtheta_dvel, algorithm): f, w = data_ag dvel_ref, dtheta_ref = data_dtheta_dvel fs_highfreq = 200.0 fs_lowfreq = 10.0 step = int(fs_highfreq / fs_lowfreq) - alg = sf.ConingScullingAlg(200.0) + alg = algorithm(200.0) dtheta_out = [] dvel_out = [] @@ -284,52 +286,131 @@ def test_surge_sway_heave(self): np.testing.assert_allclose(dvel_out, dvel_expect) np.testing.assert_allclose(dtheta_out, np.zeros(3)) - def test_dvel(self): + def test_flush(self): fs = 100.0 alg = sf.ConingScullingAlg(fs) f = np.array([1.0, 2.0, 3.0]) # m/s^2 - w = np.array([0.0, 0.0, 0.0]) # rad/s - - for i in range(int(fs * 1.0)): # 1 second - alg.update(f, w) - - dvel_out = alg.dvel() - - dvel_expect = np.array([1.0, 2.0, 3.0]) - np.testing.assert_allclose(dvel_out, dvel_expect) - - def test_dtheta(self): - fs = 100.0 - alg = sf.ConingScullingAlg(fs) - - f = np.array([0.0, 0.0, 0.0]) # m/s^2 w = np.array([np.radians(30.0), -np.radians(45.0), np.radians(60.0)]) # rad/s for i in range(int(fs * 1.0)): # 1 second alg.update(f, w) - dtheta_out = alg.dtheta() + dtheta_out, dvel_out = alg.flush() + # Check that flush returns non-zero values + assert np.all(np.abs(dtheta_out) > 0.1) + assert np.all(np.abs(dvel_out) > 0.1) - dtheta_expect = np.array( - [np.radians(30.0), -np.radians(45.0), np.radians(60.0)] - ) - np.testing.assert_allclose(dtheta_out, dtheta_expect) + # Flushing again should yield all zeros + dtheta_out, dvel_out = alg.flush() + np.testing.assert_allclose(dtheta_out, np.zeros(3)) + np.testing.assert_allclose(dvel_out, np.zeros(3)) - def test_flush(self): + def test_verify_calibration(self, data_ag, data_dtheta_dvel): + """Tests that the algorithm with built-in calibration produces the same results as the + uncalibrated algorithm with manual calibration applied to the inputs. + """ + rng = np.random.default_rng(seed=42) + FS_HIGH = 200.0 + FS_LOW = 10.0 + downsample_factor = int(FS_HIGH / FS_LOW) + # Calibration matrices and biases + A1, A2 = rng.random((3, 3)), rng.random((3, 3)) + b_w, b_f = rng.random(3) * np.radians(0.1), rng.random(3) + W_w = A1 @ A1.T + np.eye(3) # Positive semi-definite + W_f = A2 @ A2.T + np.eye(3) # Positive semi-definite + W_f_inv, W_w_inv = np.linalg.inv(W_f), np.linalg.inv(W_w) + + f_true, w_true = data_ag + dvel_true, dtheta_true = data_dtheta_dvel + + # Generate measurements with scaling, misalignment and bias + f_meas = np.empty_like(f_true) + w_meas = np.empty_like(w_true) + for i in range(len(f_true)): + f_meas[i] = W_f_inv @ (f_true[i] - b_f) + w_meas[i] = W_w_inv @ (w_true[i] - b_w) + + # Generate measurements with scaling, misalignment and bias - alternative bias method + f_meas_alt = np.empty_like(f_true) + w_meas_alt = np.empty_like(w_true) + for i in range(len(f_true)): + f_meas_alt[i] = W_f_inv @ f_true[i] - b_f + w_meas_alt[i] = W_w_inv @ w_true[i] - b_w + + # Calculate dtheta and dvel when calibrating each measurement manually before feeding to the uncalibrated algorithm + alg_naive_calibration = sf.ConingScullingAlg(FS_HIGH) + dtheta_naive, dvel_naive = [], [] + for i, (f_i, w_i) in enumerate(zip(f_meas, w_meas)): + # Apply calibration to measurements + f_i_naive = W_f @ (f_i) + b_f + w_i_naive = W_w @ (w_i) + b_w + + alg_naive_calibration.update(f_i_naive, w_i_naive) + if (i != 0) and (i % downsample_factor == 0): + dtheta_i, dvel_i = alg_naive_calibration.flush() + dtheta_naive.append(dtheta_i) + dvel_naive.append(dvel_i) + dtheta_naive = np.array(dtheta_naive) + dvel_naive = np.array(dvel_naive) + + # Calculate dtheta and dvel using the built-in calibration algorithm + alg_calibrated = sf.ConingScullingAlgCalibrated( + FS_HIGH, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f + ) + dtheta_calibrated, dvel_calibrated = [], [] + for i, (f_i, w_i) in enumerate(zip(f_meas, w_meas)): + alg_calibrated.update(f_i, w_i) + if (i != 0) and (i % downsample_factor == 0): + dtheta_i, dvel_i = alg_calibrated.flush() + dtheta_calibrated.append(dtheta_i) + dvel_calibrated.append(dvel_i) + dtheta_calibrated = np.array(dtheta_calibrated) + dvel_calibrated = np.array(dvel_calibrated) + + # Calculate dtheta and dvel using the built-in calibration algorithm - alternative bias method + alg_calibrated_alt = sf.ConingScullingAlgCalibrated( + FS_HIGH, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f, bias_alt=True + ) + dtheta_calibrated_alt, dvel_calibrated_alt = [], [] + for i, (f_i, w_i) in enumerate(zip(f_meas_alt, w_meas_alt)): + alg_calibrated_alt.update(f_i, w_i) + if (i != 0) and (i % downsample_factor == 0): + dtheta_i, dvel_i = alg_calibrated_alt.flush() + dtheta_calibrated_alt.append(dtheta_i) + dvel_calibrated_alt.append(dvel_i) + dtheta_calibrated_alt = np.array(dtheta_calibrated_alt) + dvel_calibrated_alt = np.array(dvel_calibrated_alt) + + # Test that the different methods match + np.testing.assert_allclose(dtheta_calibrated, dtheta_true, atol=1e-8) + np.testing.assert_allclose(dvel_calibrated, dvel_true, atol=1e-8) + np.testing.assert_allclose(dtheta_calibrated_alt, dtheta_true, atol=1e-8) + np.testing.assert_allclose(dvel_calibrated_alt, dvel_true, atol=1e-8) + np.testing.assert_allclose(dtheta_naive, dtheta_true, atol=1e-8) + np.testing.assert_allclose(dvel_naive, dvel_true, atol=1e-8) + + @pytest.mark.parametrize( + "w_singular, f_singular", [(True, False), (False, True), (True, True)] + ) + def test_raises_singular_matrix(self, w_singular, f_singular): fs = 100.0 - alg = sf.ConingScullingAlg(fs) - - f = np.array([1.0, 2.0, 3.0]) # m/s^2 - w = np.array([np.radians(30.0), -np.radians(45.0), np.radians(60.0)]) # rad/s - - for i in range(int(fs * 1.0)): # 1 second - alg.update(f, w) - - dtheta_expect = alg.dtheta() - dvel_expect = alg.dvel() - dtheta_out, dvel_out = alg.flush() - np.testing.assert_allclose(dtheta_out, dtheta_expect) - np.testing.assert_allclose(dvel_out, dvel_expect) - assert np.allclose(alg.dtheta(), np.zeros(3)) - assert np.allclose(alg.dvel(), np.zeros(3)) + if w_singular: + W_w = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]]) + else: + W_w = np.eye(3) + if f_singular: + W_f = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]]) + else: + W_f = np.eye(3) + b_w = np.zeros(3) + b_f = np.zeros(3) + with pytest.raises(ValueError, match="must be invertible"): + sf.ConingScullingAlgCalibrated( + fs, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f, bias_alt=False + ) + if w_singular: + with pytest.raises(ValueError, match="must be invertible"): + sf.ConingScullingAlgCalibrated( + fs, W_w=W_w, W_f=W_f, b_w=b_w, b_f=b_f, bias_alt=True + ) diff --git a/tests/test_vectorops.py b/tests/test_vectorops.py index ad282aec..951faae8 100644 --- a/tests/test_vectorops.py +++ b/tests/test_vectorops.py @@ -70,3 +70,16 @@ def test__skew_symmetric(): out = _vectorops._skew_symmetric(a) @ b expected = np.cross(a, b) np.testing.assert_array_equal(out, expected) + + +def test__adjugate_and_det_3_by_3(): + """Test matrix inversion against numpy""" + rng = np.random.default_rng(42) + for _ in range(5): + m = rng.random((3, 3)) + + adj, det = _vectorops._adjugate_and_det_3_by_3(m) + inv = adj / det + + inv_expected = np.linalg.inv(m) + np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)