From f74c6747314e3ca132a900a3d4e61c2f400990b7 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Tue, 12 May 2026 10:23:51 +0200
Subject: [PATCH 1/8] Update docstring

---
 src/smsfusion/_coning_sculling.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index b4c676cd..2c6bbe03 100644
--- a/src/smsfusion/_coning_sculling.py
+++ b/src/smsfusion/_coning_sculling.py
@@ -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].
@@ -131,6 +131,7 @@ def dvel(self):
         the total change in velocity (no gravity correction) over all samples since
         initialization (or last reset).
         """
+        # Equation (7.2.2.2-23) in ref [2]_
         return self._vel + self._dvel_rot + self._dvel_scul
 
     def flush(self, degrees=False):

From 1dc6e14bdf4babb007c0d528bdb33b7eb1d355d4 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 21 May 2026 11:08:10 +0200
Subject: [PATCH 2/8] Remove dtheta and dvel methods

---
 src/smsfusion/_coning_sculling.py | 36 +++++++-----------------
 tests/test_coning_sculling.py     | 46 ++++++-------------------------
 2 files changed, 18 insertions(+), 64 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index 2c6bbe03..bafbab11 100644
--- a/src/smsfusion/_coning_sculling.py
+++ b/src/smsfusion/_coning_sculling.py
@@ -106,34 +106,10 @@ 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):
-        """
-        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).
-        """
-        # Equation (7.2.2.2-23) in ref [2]_
-        return self._vel + self._dvel_rot + self._dvel_scul
-
     def flush(self, degrees=False):
         """
         Return dtheta (the accumulated 'body attitude change' vector) and
@@ -153,8 +129,16 @@ def flush(self, degrees=False):
         dvel : ndarray, shape (3,)
             The accumulated specific force velocity change vector, see :meth:`dvel`.
         """
-        dtheta = self.dtheta(degrees=degrees)
-        dvel = self.dvel()
+        # The accumulated 'body attitude change' vector. I.e., the rotation vector
+        # describing the total rotation over all samples since initialization (or
+        # last reset).
+        dtheta = self._theta + self._dtheta_con
+        dtheta = np.degrees(dtheta) if degrees else dtheta
+
+        # 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). Equation (7.2.2.2-23) in ref [2]_
+        dvel = self._vel + self._dvel_rot + self._dvel_scul
 
         self._theta[:] = np.zeros(3, dtype=float)
         self._dtheta_con[:] = np.zeros(3, dtype=float)
diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py
index 44899ea7..f8fd6fdd 100644
--- a/tests/test_coning_sculling.py
+++ b/tests/test_coning_sculling.py
@@ -284,38 +284,6 @@ 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):
-        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_expect = np.array(
-            [np.radians(30.0), -np.radians(45.0), np.radians(60.0)]
-        )
-        np.testing.assert_allclose(dtheta_out, dtheta_expect)
-
     def test_flush(self):
         fs = 100.0
         alg = sf.ConingScullingAlg(fs)
@@ -326,10 +294,12 @@ def test_flush(self):
         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))
+        # 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)
+
+        # 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))

From 9308832ecee6a71bd02cc073f134cd3ff69396be Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 21 May 2026 16:31:55 +0200
Subject: [PATCH 3/8] Add calibrated version

---
 src/smsfusion/__init__.py         |  3 +-
 src/smsfusion/_coning_sculling.py | 78 ++++++++++++++++++++++++++-----
 2 files changed, 68 insertions(+), 13 deletions(-)

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 bafbab11..a855c8c4 100644
--- a/src/smsfusion/_coning_sculling.py
+++ b/src/smsfusion/_coning_sculling.py
@@ -110,6 +110,17 @@ def update(self, f: ArrayLike, w: ArrayLike, degrees: bool = False):
     def _dvel_rot(self):
         return 0.5 * _cross(self._theta, self._vel)
 
+    def _calc_dtheta_dvel(self, degrees=False):
+        """
+        Calculate the coning and sculling corrected dtheta and dvel.
+        """
+        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):
         """
         Return dtheta (the accumulated 'body attitude change' vector) and
@@ -125,23 +136,66 @@ 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).
         """
-        # The accumulated 'body attitude change' vector. I.e., the rotation vector
-        # describing the total rotation over all samples since initialization (or
-        # last reset).
-        dtheta = self._theta + self._dtheta_con
-        dtheta = np.degrees(dtheta) if degrees else dtheta
-
-        # 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). Equation (7.2.2.2-23) in ref [2]_
-        dvel = self._vel + self._dvel_rot + self._dvel_scul
+        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).
+    """
+
+    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),
+    ):
+        W_w_inv = np.linalg.inv(W_w)
+        self.cof_W = W_w_inv.T * np.linalg.det(W_w)
+        self.W_star = W_w_inv @ W_f
+        self.b_f_star = np.linalg.inv(W_f) @ 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

From 01f8f1723335aea2e0707d39b292f817b486f6d0 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 21 May 2026 16:36:49 +0200
Subject: [PATCH 4/8] Test calibrated version

---
 tests/test_coning_sculling.py | 69 +++++++++++++++++++++++++++++++++--
 1 file changed, 66 insertions(+), 3 deletions(-)

diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py
index f8fd6fdd..2c34b9ce 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 = []
@@ -303,3 +305,64 @@ def test_flush(self):
         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_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)
+
+        # 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)
+
+        # 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_naive, dtheta_true, atol=1e-8)
+        np.testing.assert_allclose(dvel_naive, dvel_true, atol=1e-8)

From f126e23cedfe716960b803f90b39b27e06bbf552 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 28 May 2026 12:25:49 +0200
Subject: [PATCH 5/8] Create and use determinant and inversion functions

---
 src/smsfusion/_coning_sculling.py | 79 +++++++++++++++++++++++++++++--
 tests/test_coning_sculling.py     | 22 +++++++++
 2 files changed, 98 insertions(+), 3 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index a855c8c4..34d57a4e 100644
--- a/src/smsfusion/_coning_sculling.py
+++ b/src/smsfusion/_coning_sculling.py
@@ -180,10 +180,11 @@ def __init__(
         b_w: np.ndarray = np.zeros(3),
         b_f: np.ndarray = np.zeros(3),
     ):
-        W_w_inv = np.linalg.inv(W_w)
-        self.cof_W = W_w_inv.T * np.linalg.det(W_w)
+        W_w_det = _determinant_3_by_3(W_w)
+        W_w_inv = _inverse_3_by_3(W_w, determinant=W_w_det)
+        self.cof_W = W_w_inv.T * W_w_det
         self.W_star = W_w_inv @ W_f
-        self.b_f_star = np.linalg.inv(W_f) @ b_f
+        self.b_f_star = _inverse_3_by_3(W_f) @ b_f
         self.b_w_star = W_w_inv @ b_w
         self.W_w = W_w
         super().__init__(fs)
@@ -199,3 +200,75 @@ def _calc_dtheta_dvel(self, degrees=False):
 
         dvel = self.W_w @ self._vel + self.cof_W @ (self._dvel_rot + self._dvel_scul)
         return dtheta, dvel
+
+
+def _determinant_3_by_3(m: np.ndarray) -> float:
+    """
+    Calculates and returns the determinant of the matrix
+    ```python
+    m = [[a, b, c],
+         [d, e, f],
+         [g, h, i]]
+    ```
+
+    Parameters
+    ----------
+    m : array-like, shape (3, 3)
+        The matrix for which to calculate the determinant.
+
+    Returns
+    -------
+    det : 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
+
+    return a * A + b * B + c * C
+
+
+def _inverse_3_by_3(m: np.ndarray, determinant: float | None = None):
+    """
+    Calculates and returns the inverse of the matrix
+    ```python
+    m = [[a, b, c],
+         [d, e, f],
+         [g, h, i]]
+    ```
+
+    Parameters
+    ----------
+    m : array-like, shape (3, 3)
+        The matrix to be inverted.
+    determinant : float, optional
+        The determinant of the input matrix m. If not provided, it will be calculated
+        internally.
+
+    Returns
+    -------
+    inv_m : ndarray, shape (3, 3)
+        The inverse of the input matrix m.
+    """
+    if m.shape != (3, 3):
+        raise ValueError("Input matrix must be 3x3.")
+    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
+    if determinant is None:
+        determinant = _determinant_3_by_3(m)
+    if determinant == 0:
+        raise ValueError("Input matrix is singular and cannot be inverted.")
+    return np.array([[A, D, G], [B, E, H], [C, F, I]]) / determinant
diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py
index 2c34b9ce..11977707 100644
--- a/tests/test_coning_sculling.py
+++ b/tests/test_coning_sculling.py
@@ -366,3 +366,25 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
         np.testing.assert_allclose(dvel_calibrated, 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)
+
+    def test__inverse_and_determinant_3_by_3(self):
+        """Test matrix inversion and determinant against numpy"""
+        rng = np.random.default_rng(42)
+        for _ in range(5):
+            m = rng.random((3, 3))
+            det = sf._coning_sculling._determinant_3_by_3(m)
+            inv = sf._coning_sculling._inverse_3_by_3(m)
+            inv_with_det = sf._coning_sculling._inverse_3_by_3(m, determinant=det)
+            det_expected = np.linalg.det(m)
+            inv_expected = np.linalg.inv(m)
+            np.testing.assert_allclose(det, det_expected, rtol=1e-12, atol=1e-12)
+            np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)
+            np.testing.assert_allclose(
+                inv_with_det, inv_expected, rtol=1e-12, atol=1e-12
+            )
+
+    def test__inverse_3_by_3_singular(self):
+        """Test that the inverse function raises an error for singular matrices"""
+        singular_matrix = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
+        with pytest.raises(ValueError):
+            sf._coning_sculling._inverse_3_by_3(singular_matrix)

From 54ad52f969d7b65a25e2938feb53aac534eb9629 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 28 May 2026 14:00:59 +0200
Subject: [PATCH 6/8] Allow for alternative calibration model

---
 src/smsfusion/_coning_sculling.py | 24 ++++++++++++++++++++++--
 tests/test_coning_sculling.py     | 23 +++++++++++++++++++++++
 2 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index 34d57a4e..e67977d7 100644
--- a/src/smsfusion/_coning_sculling.py
+++ b/src/smsfusion/_coning_sculling.py
@@ -170,6 +170,21 @@ class ConingScullingAlgCalibrated(ConingScullingAlg):
         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__(
@@ -179,13 +194,18 @@ def __init__(
         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,
     ):
         W_w_det = _determinant_3_by_3(W_w)
         W_w_inv = _inverse_3_by_3(W_w, determinant=W_w_det)
         self.cof_W = W_w_inv.T * W_w_det
         self.W_star = W_w_inv @ W_f
-        self.b_f_star = _inverse_3_by_3(W_f) @ b_f
-        self.b_w_star = W_w_inv @ b_w
+        if bias_alt:
+            self.b_f_star = b_f
+            self.b_w_star = b_w
+        else:
+            self.b_f_star = _inverse_3_by_3(W_f) @ b_f
+            self.b_w_star = W_w_inv @ b_w
         self.W_w = W_w
         super().__init__(fs)
 
diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py
index 11977707..c9b87c16 100644
--- a/tests/test_coning_sculling.py
+++ b/tests/test_coning_sculling.py
@@ -331,6 +331,13 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
             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 = [], []
@@ -361,9 +368,25 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
         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)
 

From 411bd33089c90d91a4384d325b91ffcb4a23a017 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 28 May 2026 14:38:29 +0200
Subject: [PATCH 7/8] Move helper functions into _vectorops

---
 src/smsfusion/_coning_sculling.py | 74 +------------------------------
 src/smsfusion/_vectorops.py       | 74 +++++++++++++++++++++++++++++++
 tests/test_coning_sculling.py     | 22 ---------
 tests/test_vectorops.py           | 22 +++++++++
 4 files changed, 97 insertions(+), 95 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index e67977d7..e9d38c73 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 _cross, _determinant_3_by_3, _inverse_3_by_3
 
 
 class ConingScullingAlg:
@@ -220,75 +220,3 @@ def _calc_dtheta_dvel(self, degrees=False):
 
         dvel = self.W_w @ self._vel + self.cof_W @ (self._dvel_rot + self._dvel_scul)
         return dtheta, dvel
-
-
-def _determinant_3_by_3(m: np.ndarray) -> float:
-    """
-    Calculates and returns the determinant of the matrix
-    ```python
-    m = [[a, b, c],
-         [d, e, f],
-         [g, h, i]]
-    ```
-
-    Parameters
-    ----------
-    m : array-like, shape (3, 3)
-        The matrix for which to calculate the determinant.
-
-    Returns
-    -------
-    det : 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
-
-    return a * A + b * B + c * C
-
-
-def _inverse_3_by_3(m: np.ndarray, determinant: float | None = None):
-    """
-    Calculates and returns the inverse of the matrix
-    ```python
-    m = [[a, b, c],
-         [d, e, f],
-         [g, h, i]]
-    ```
-
-    Parameters
-    ----------
-    m : array-like, shape (3, 3)
-        The matrix to be inverted.
-    determinant : float, optional
-        The determinant of the input matrix m. If not provided, it will be calculated
-        internally.
-
-    Returns
-    -------
-    inv_m : ndarray, shape (3, 3)
-        The inverse of the input matrix m.
-    """
-    if m.shape != (3, 3):
-        raise ValueError("Input matrix must be 3x3.")
-    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
-    if determinant is None:
-        determinant = _determinant_3_by_3(m)
-    if determinant == 0:
-        raise ValueError("Input matrix is singular and cannot be inverted.")
-    return np.array([[A, D, G], [B, E, H], [C, F, I]]) / determinant
diff --git a/src/smsfusion/_vectorops.py b/src/smsfusion/_vectorops.py
index 99ded2e9..c99c1d18 100644
--- a/src/smsfusion/_vectorops.py
+++ b/src/smsfusion/_vectorops.py
@@ -90,3 +90,77 @@ 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 _determinant_3_by_3(m: np.ndarray) -> float:
+    """
+    Calculates and returns the determinant of the matrix
+    ```python
+    m = [[a, b, c],
+         [d, e, f],
+         [g, h, i]]
+    ```
+
+    Parameters
+    ----------
+    m : array-like, shape (3, 3)
+        The matrix for which to calculate the determinant.
+
+    Returns
+    -------
+    det : 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
+
+    return a * A + b * B + c * C
+
+
+@njit  # type: ignore[misc]
+def _inverse_3_by_3(m: np.ndarray, determinant: float | None = None):
+    """
+    Calculates and returns the inverse of the matrix
+    ```python
+    m = [[a, b, c],
+         [d, e, f],
+         [g, h, i]]
+    ```
+
+    Parameters
+    ----------
+    m : array-like, shape (3, 3)
+        The matrix to be inverted.
+    determinant : float, optional
+        The determinant of the input matrix m. If not provided, it will be calculated
+        internally.
+
+    Returns
+    -------
+    inv_m : ndarray, shape (3, 3)
+        The inverse of the input matrix m.
+    """
+    if m.shape != (3, 3):
+        raise ValueError("Input matrix must be 3x3.")
+    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
+    if determinant is None:
+        determinant = _determinant_3_by_3(m)
+    if determinant == 0:
+        raise ValueError("Input matrix is singular and cannot be inverted.")
+    return np.array([[A, D, G], [B, E, H], [C, F, I]]) / determinant
diff --git a/tests/test_coning_sculling.py b/tests/test_coning_sculling.py
index c9b87c16..c4c268b2 100644
--- a/tests/test_coning_sculling.py
+++ b/tests/test_coning_sculling.py
@@ -389,25 +389,3 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
         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)
-
-    def test__inverse_and_determinant_3_by_3(self):
-        """Test matrix inversion and determinant against numpy"""
-        rng = np.random.default_rng(42)
-        for _ in range(5):
-            m = rng.random((3, 3))
-            det = sf._coning_sculling._determinant_3_by_3(m)
-            inv = sf._coning_sculling._inverse_3_by_3(m)
-            inv_with_det = sf._coning_sculling._inverse_3_by_3(m, determinant=det)
-            det_expected = np.linalg.det(m)
-            inv_expected = np.linalg.inv(m)
-            np.testing.assert_allclose(det, det_expected, rtol=1e-12, atol=1e-12)
-            np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)
-            np.testing.assert_allclose(
-                inv_with_det, inv_expected, rtol=1e-12, atol=1e-12
-            )
-
-    def test__inverse_3_by_3_singular(self):
-        """Test that the inverse function raises an error for singular matrices"""
-        singular_matrix = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
-        with pytest.raises(ValueError):
-            sf._coning_sculling._inverse_3_by_3(singular_matrix)
diff --git a/tests/test_vectorops.py b/tests/test_vectorops.py
index ad282aec..d393ebb5 100644
--- a/tests/test_vectorops.py
+++ b/tests/test_vectorops.py
@@ -70,3 +70,25 @@ def test__skew_symmetric():
     out = _vectorops._skew_symmetric(a) @ b
     expected = np.cross(a, b)
     np.testing.assert_array_equal(out, expected)
+
+
+def test__inverse_and_determinant_3_by_3():
+    """Test matrix inversion and determinant against numpy"""
+    rng = np.random.default_rng(42)
+    for _ in range(5):
+        m = rng.random((3, 3))
+        det = _vectorops._determinant_3_by_3(m)
+        inv = _vectorops._inverse_3_by_3(m)
+        inv_with_det = _vectorops._inverse_3_by_3(m, determinant=det)
+        det_expected = np.linalg.det(m)
+        inv_expected = np.linalg.inv(m)
+        np.testing.assert_allclose(det, det_expected, rtol=1e-12, atol=1e-12)
+        np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)
+        np.testing.assert_allclose(inv_with_det, inv_expected, rtol=1e-12, atol=1e-12)
+
+
+def test__inverse_3_by_3_singular():
+    """Test that the inverse function raises an error for singular matrices"""
+    singular_matrix = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
+    with pytest.raises(ValueError):
+        _vectorops._inverse_3_by_3(singular_matrix)

From 870a6b03a0166e8b66b88d74d2ec066cdc395a56 Mon Sep 17 00:00:00 2001
From: hakon-molven-4ss <70759227+hakon-molven-4ss@users.noreply.github.com>
Date: Thu, 28 May 2026 15:22:51 +0200
Subject: [PATCH 8/8] Some more optimization

---
 src/smsfusion/_coning_sculling.py | 14 +++++--
 src/smsfusion/_vectorops.py       | 62 ++++++++-----------------------
 tests/test_coning_sculling.py     | 25 +++++++++++++
 tests/test_vectorops.py           | 21 +++--------
 4 files changed, 57 insertions(+), 65 deletions(-)

diff --git a/src/smsfusion/_coning_sculling.py b/src/smsfusion/_coning_sculling.py
index e9d38c73..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, _determinant_3_by_3, _inverse_3_by_3
+from smsfusion._vectorops import _adjugate_and_det_3_by_3, _cross
 
 
 class ConingScullingAlg:
@@ -196,15 +196,21 @@ def __init__(
         b_f: np.ndarray = np.zeros(3),
         bias_alt: bool = False,
     ):
-        W_w_det = _determinant_3_by_3(W_w)
-        W_w_inv = _inverse_3_by_3(W_w, determinant=W_w_det)
+        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:
-            self.b_f_star = _inverse_3_by_3(W_f) @ b_f
+            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)
diff --git a/src/smsfusion/_vectorops.py b/src/smsfusion/_vectorops.py
index c99c1d18..072bd167 100644
--- a/src/smsfusion/_vectorops.py
+++ b/src/smsfusion/_vectorops.py
@@ -93,23 +93,29 @@ def _skew_symmetric(a: NDArray[np.float64]) -> NDArray[np.float64]:
 
 
 @njit  # type: ignore[misc]
-def _determinant_3_by_3(m: np.ndarray) -> float:
+def _adjugate_and_det_3_by_3(
+    m: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], float]:
     """
-    Calculates and returns the determinant of the matrix
+    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 : array-like, shape (3, 3)
-        The matrix for which to calculate the determinant.
+    m : numpy.ndarray, shape (3, 3)
+        The matrix for which to calculate the adjugate and determinant.
 
     Returns
     -------
-    det : float
+    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]
@@ -118,49 +124,13 @@ def _determinant_3_by_3(m: np.ndarray) -> float:
     A = e * i - f * h
     B = -(d * i - f * g)
     C = d * h - e * g
-
-    return a * A + b * B + c * C
-
-
-@njit  # type: ignore[misc]
-def _inverse_3_by_3(m: np.ndarray, determinant: float | None = None):
-    """
-    Calculates and returns the inverse of the matrix
-    ```python
-    m = [[a, b, c],
-         [d, e, f],
-         [g, h, i]]
-    ```
-
-    Parameters
-    ----------
-    m : array-like, shape (3, 3)
-        The matrix to be inverted.
-    determinant : float, optional
-        The determinant of the input matrix m. If not provided, it will be calculated
-        internally.
-
-    Returns
-    -------
-    inv_m : ndarray, shape (3, 3)
-        The inverse of the input matrix m.
-    """
-    if m.shape != (3, 3):
-        raise ValueError("Input matrix must be 3x3.")
-    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
-    if determinant is None:
-        determinant = _determinant_3_by_3(m)
-    if determinant == 0:
-        raise ValueError("Input matrix is singular and cannot be inverted.")
-    return np.array([[A, D, G], [B, E, H], [C, F, I]]) / determinant
+    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 c4c268b2..709a73b9 100644
--- a/tests/test_coning_sculling.py
+++ b/tests/test_coning_sculling.py
@@ -389,3 +389,28 @@ def test_verify_calibration(self, data_ag, data_dtheta_dvel):
         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
+        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 d393ebb5..951faae8 100644
--- a/tests/test_vectorops.py
+++ b/tests/test_vectorops.py
@@ -72,23 +72,14 @@ def test__skew_symmetric():
     np.testing.assert_array_equal(out, expected)
 
 
-def test__inverse_and_determinant_3_by_3():
-    """Test matrix inversion and determinant against numpy"""
+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))
-        det = _vectorops._determinant_3_by_3(m)
-        inv = _vectorops._inverse_3_by_3(m)
-        inv_with_det = _vectorops._inverse_3_by_3(m, determinant=det)
-        det_expected = np.linalg.det(m)
-        inv_expected = np.linalg.inv(m)
-        np.testing.assert_allclose(det, det_expected, rtol=1e-12, atol=1e-12)
-        np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)
-        np.testing.assert_allclose(inv_with_det, inv_expected, rtol=1e-12, atol=1e-12)
 
+        adj, det = _vectorops._adjugate_and_det_3_by_3(m)
+        inv = adj / det
 
-def test__inverse_3_by_3_singular():
-    """Test that the inverse function raises an error for singular matrices"""
-    singular_matrix = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
-    with pytest.raises(ValueError):
-        _vectorops._inverse_3_by_3(singular_matrix)
+        inv_expected = np.linalg.inv(m)
+        np.testing.assert_allclose(inv, inv_expected, rtol=1e-12, atol=1e-12)