test benchmark

Author: vegard-solum-4ss

tests/test_smoothing.py

@@ -1,6 +1,7 @@
 import pytest
 
 import numpy as np
+from scipy.signal import resample_poly
 import smsfusion as sf
 from smsfusion import FixedIntervalSmoother
 from smsfusion.benchmark import (
@@ -29,11 +30,7 @@ def test__init__(self, ains):
         assert smoother.bias_acc().size == 0
         assert smoother.bias_gyro().size == 0
 
-    @pytest.mark.parametrize(
-        "benchmark_gen",
-        [benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A],
-    )
-    def test_benchmark(self, benchmark_gen):
+    def test_benchmark(self):
         fs_imu = 10.0
         fs_aiding = 1.0
         fs_ratio = np.ceil(fs_imu / fs_aiding)
@@ -43,7 +40,7 @@ def test_benchmark(self, benchmark_gen):
         vel_noise_std = 0.1
 
         # Reference signals (without noise)
-        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu)
+        t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_full_pva_beat_202311A(fs_imu)
         euler_ref = np.degrees(euler_ref)
         gyro_ref = np.degrees(gyro_ref)
 
@@ -55,7 +52,7 @@ def test_benchmark(self, benchmark_gen):
         noise_model = sf.noise.IMUNoise(err_acc, err_gyro, seed=0)
         imu_noise = noise_model(fs_imu, len(t))
         acc_meas = acc_ref + imu_noise[:, :3]
-        gyro_meas = gyro_ref + imu_noise[:, 3:]
+        gyro_meas = gyro_ref + np.degrees(imu_noise[:, 3:])
 
         # Compass / heading (aiding) measurements
         head_noise = compass_noise_std * rng.standard_normal(len(t))
@@ -81,6 +78,7 @@ def test_benchmark(self, benchmark_gen):
 
         smoother = FixedIntervalSmoother(ains, cov_smoothing=True)
 
+        pos_ains, vel_ains, euler_ains, bias_acc_ains, bias_gyro_ains = [], [], [], [], []
         for i, (acc_i, gyro_i, pos_i, vel_i, head_i) in enumerate(
             zip(acc_meas, gyro_meas, pos_meas, vel_meas, head_meas)
         ):
@@ -101,3 +99,42 @@ def test_benchmark(self, benchmark_gen):
                 )
             else:  # without aiding
                 smoother.update(acc_i, gyro_i, degrees=True)
+
+            pos_ains.append(smoother.ains.position())
+            vel_ains.append(smoother.ains.velocity())
+            euler_ains.append(smoother.ains.euler(degrees=True))
+            bias_acc_ains.append(smoother.ains.bias_acc())
+            bias_gyro_ains.append(smoother.ains.bias_gyro(degrees=True))
+
+        pos_ains = np.array(pos_ains)
+        vel_ains = np.array(vel_ains)
+        euler_ains = np.array(euler_ains)
+        bias_acc_ains = np.array(bias_acc_ains)
+        bias_gyro_ains = np.array(bias_gyro_ains)
+
+        pos_smth = smoother.position()
+        vel_smth = smoother.velocity()
+        euler_smth = smoother.euler(degrees=True)
+        bias_acc_smth = smoother.bias_acc()
+        bias_gyro_smth = smoother.bias_gyro(degrees=True)
+
+        pos_smth = np.array(pos_smth)
+        vel_smth = np.array(vel_smth)
+        euler_smth = np.array(euler_smth)
+        bias_acc_smth = np.array(bias_acc_smth)
+        bias_gyro_smth = np.array(bias_gyro_smth)
+
+        # Half-sample shift (compensates for the delay introduced by Euler integration)
+        pos_ains = resample_poly(pos_ains, 2, 1)[1:-1:2]
+        pos_smth = resample_poly(pos_smth, 2, 1)[1:-1:2]
+        pos_ref = pos_ref[:-1, :]
+        vel_ains = resample_poly(vel_ains, 2, 1)[1:-1:2]
+        vel_smth = resample_poly(vel_smth, 2, 1)[1:-1:2]
+        vel_ref = vel_ref[:-1, :]
+        euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2]
+        euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2]
+        euler_ref = euler_ref[:-1, :]
+
+        pos_err_smth = np.std((pos_smth - pos_ref)[warmup:], axis=0)
+        pos_err_ains = np.std((pos_ains - pos_ref)[warmup:], axis=0)
+        assert pos_err_smth[0] < pos_err_ains[0]