test vru

Author: vegard-solum-4ss

tests/test_smoothing.py

@@ -35,6 +35,7 @@ def test_benchmark_ains(self):
         fs = 10.24  # sampling rate in Hz
         _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs)
         euler = np.degrees(euler)
+        gyro = np.degrees(gyro)
         head = euler[:, 2]
 
         # IMU measurements
@@ -70,12 +71,12 @@ def test_benchmark_ains(self):
             smoother.update(
                 f_i,
                 w_i,
-                degrees=False,
+                degrees=True,
                 pos=p_i,
                 pos_var=pos_noise_std**2 * np.ones(3),
                 head=h_i,
                 head_var=head_noise_std**2,
-                head_degrees=False,
+                head_degrees=True,
             )
 
             pos_ains.append(smoother.ains.position())
@@ -123,6 +124,7 @@ def test_benchmark_ahrs(self):
         fs = 10.24  # sampling rate in Hz
         _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs)
         euler = np.degrees(euler)
+        gyro = np.degrees(gyro)
         head = euler[:, 2]
 
         # IMU measurements
@@ -156,10 +158,10 @@ def test_benchmark_ahrs(self):
             smoother.update(
                 f_i,
                 w_i,
-                degrees=False,
+                degrees=True,
                 head=h_i,
                 head_var=head_noise_std**2,
-                head_degrees=False,
+                head_degrees=True,
             )
 
             euler_ains.append(smoother.ains.euler(degrees=True))
@@ -168,14 +170,76 @@ def test_benchmark_ahrs(self):
         euler_ains = np.array(euler_ains)
 
         # Smoothed state estimates
-        euler_smth = smoother.euler(degrees=False)
+        euler_smth = smoother.euler(degrees=True)
+
+        # Half-sample shift
+        # # (compensates for the delay introduced by Euler integration)
+        euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2]
+        euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2]
+        euler = euler[:-1, :]
+
+        warmup = int(fs * 600.0)  # truncate 600 seconds from the beginning
+
+        euler_err_smth = np.std((euler_smth - euler)[warmup:], axis=0)
+        euler_err_ains = np.std((euler_ains - euler)[warmup:], axis=0)
+        np.testing.assert_array_less(euler_err_smth, euler_err_ains)
+
+    def test_benchmark_vru(self):
+        # Reference signal
+        fs = 10.24  # sampling rate in Hz
+        _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs)
+        euler = np.degrees(euler)
+        gyro = np.degrees(gyro)
+        head = euler[:, 2]
+
+        # IMU measurements
+        err_acc = sf.constants.ERR_ACC_MOTION2  # m/s^2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2  # rad/s
+        imu_noise = sf.noise.IMUNoise(err_acc, err_gyro)(fs, len(acc))
+        acc_imu = acc + imu_noise[:, :3]
+        gyro_imu = gyro + np.degrees(imu_noise[:, 3:])
+
+        # AINS
+        p0 = pos[0]  # position [m]
+        v0 = vel[0]  # velocity [m/s]
+        q0 = sf.quaternion_from_euler(euler[0], degrees=True)  # unit quaternion
+        ba0 = np.zeros(3)  # accelerometer bias [m/s^2]
+        bg0 = np.zeros(3)  # gyroscope bias [rad/s]
+        x0 = np.concatenate((p0, v0, q0, ba0, bg0))
+        P0 = np.eye(12) * 1e-3
+        err_acc = sf.constants.ERR_ACC_MOTION2  # m/s^2
+        err_gyro = sf.constants.ERR_GYRO_MOTION2  # rad/s
+        ains = sf.VRU(fs, x0, P0, err_acc, err_gyro)
+
+        smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True)
+
+        euler_ains = []
+        for f_i, w_i in zip(acc_imu, gyro_imu):
+            smoother.update(
+                f_i,
+                w_i,
+                degrees=True,
+            )
+
+            euler_ains.append(smoother.ains.euler(degrees=True))
+
+        # Forward filter state estimates
+        euler_ains = np.array(euler_ains)
+
+        # Smoothed state estimates
+        euler_smth = smoother.euler(degrees=True)
 
         # Half-sample shift
         # # (compensates for the delay introduced by Euler integration)
         euler_ains = resample_poly(euler_ains, 2, 1)[1:-1:2]
         euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2]
         euler = euler[:-1, :]
 
+        # Drop yaw
+        euler = euler[:, :2]
+        euler_ains = euler_ains[:, :2]
+        euler_smth = euler_smth[:, :2]
+
         warmup = int(fs * 600.0)  # truncate 600 seconds from the beginning
 
         euler_err_smth = np.std((euler_smth - euler)[warmup:], axis=0)