tests

Author: ali-cetin-4ss

src/smsfusion/_ins/_common.py

@@ -44,7 +44,7 @@ def _dhda_head(q: NDArray[np.float64]) -> NDArray[np.float64]:
     return dhda  # type: ignore[no-any-return]
 
 
-@njit
+@njit  # type: ignore[misc]
 def _h_head(q: NDArray[np.float64]) -> float:
     """
     Compute yaw angle from unit quaternion.
@@ -73,7 +73,7 @@ def _h_head(q: NDArray[np.float64]) -> float:
     return np.arctan2(u_y, u_x)  # type: ignore[no-any-return]
 
 
-@njit
+@njit  # type: ignore[misc]
 def _signed_smallest_angle(angle: float, degrees: bool = True) -> float:
     """
     Convert the given angle to the smallest angle between [-180., 180) degrees.
@@ -324,6 +324,7 @@ def _project_covariance_ahead(
     return P
 
 
+@njit  # type: ignore[misc]
 def _nz2vg(nav_frame: str) -> float:
     """
     Gravity direction along the navigation frame's z-axis. Transforms the z-axis

src/smsfusion/_ins/_vru.py

@@ -26,6 +26,7 @@
 )
 
 
+@njit  # type: ignore[misc]
 def _state_transition_matrix_init(
     dt: float,
     dtheta: NDArray[np.float64],
@@ -82,6 +83,7 @@ def _state_transition_matrix_update(
     return phi
 
 
+@njit  # type: ignore[misc]
 def _process_noise_covariance_matrix(
     dt: float, arw: float, gbs: float, gbc: float
 ) -> NDArray[np.float64]:
@@ -110,6 +112,7 @@ def _process_noise_covariance_matrix(
     return Q
 
 
+@njit  # type: ignore[misc]
 def _measurement_matrix_init() -> NDArray[np.float64]:
     """
     Measurement matrix.

tests/conftest.py

@@ -0,0 +1,2 @@
+import os
+os.environ["NUMBA_DISABLE_JIT"] = "1"

tests/test_ins_/test_common.py

@@ -194,3 +194,28 @@ def test__update_quaternion_with_gibbs2(quaternion, da, quaternion_update_expect
     np.testing.assert_allclose(
         quaternion_update, quaternion_update_expected, atol=1e-10
     )
+
+
+@pytest.mark.parametrize(
+    "q_nb,nav_frame_factor",
+    [
+        (np.array([1.0, 0.0, 0.0, 0.0]), 1.0),
+        (np.array([1.0, 0.0, 0.0, 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), np.sin(0.1/2), 0.0, 0.0]), 1.0),
+        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), -1.0),
+        (np.array([np.cos(0.1/2), 0.0, np.sin(0.1/2), 0.0]), 1.0),
+        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), -1.0),
+        (np.array([np.cos(0.1/2), 0.0, 0.0, np.sin(0.1/2)]), 1.0)
+    ]
+)
+def test__nz_b_from_quat(q_nb, nav_frame_factor):
+    out = _common._nz_b_from_quat(q_nb, nav_frame_factor=nav_frame_factor)
+
+    expect = Rotation.from_quat(q_nb, scalar_first=True).apply(nav_frame_factor * np.array([0.0, 0.0, 1.0]), inverse=True)
+    np.testing.assert_allclose(out, expect)
+
+
+def test__nz2vg():
+    assert _common._nz2vg("NED") == 1.0
+    assert _common._nz2vg("ENU") == -1.0

tests/test_ins_/test_utils.py

@@ -39,6 +39,12 @@ def test_euler_from_acc(euler):
     euler_enu = _utils.euler_from_acc(acc_enu, nav_frame="ENU", yaw=euler[2], degrees=True)
     np.testing.assert_allclose(euler_enu, euler_degrees)
 
+def test_euler_from_acc_invalid_frame():
+ 
+    acc_ned = np.array([0.0, 0.0, 1])
+    with pytest.raises(ValueError):
+        _utils.euler_from_acc(acc_ned, nav_frame="STAR")
+
 
 @pytest.mark.parametrize(
     "mu, g_expect",

tests/test_ins_/test_vru.py

@@ -122,10 +122,10 @@ def test_vru_methods():
 
 
 @pytest.mark.parametrize(
-    "benchmark_gen",
-    [benchmark_pure_attitude_beat_202311A, benchmark_pure_attitude_chirp_202311A],
+    "benchmark_gen, degrees",
+    [(benchmark_pure_attitude_beat_202311A, False), (benchmark_pure_attitude_chirp_202311A, True)],
 )
-def test_vru_benchmark(benchmark_gen):
+def test_vru_benchmark(benchmark_gen, degrees):
     fs_imu = 100.0
     warmup = int(fs_imu * 600.0)  # truncate 600 seconds from the beginning
 
@@ -143,6 +143,9 @@ def test_vru_benchmark(benchmark_gen):
     acc_noise = acc_ref + imu_noise[:, :3]
     gyro_noise = gyro_ref + imu_noise[:, 3:] + bg
 
+    if degrees:
+        gyro_noise = np.degrees(gyro_noise)
+
     # MEKF
     q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False)
     mekf = VRU(
@@ -158,13 +161,14 @@ def test_vru_benchmark(benchmark_gen):
     for i, (f_i, w_i) in enumerate(
         zip(acc_noise, gyro_noise)
     ):
+
         dvel = f_i / fs_imu
         dtheta = w_i / fs_imu
 
         mekf.update(
             dvel,
             dtheta,
-            degrees=False,
+            degrees=degrees,
             gref=True
         )