test ahrs

Author: vegard-solum-4ss

tests/test_smoothing.py

@@ -314,3 +314,68 @@ def test_benchmark_ains(self):
         euler_err_smth = np.std((euler_smth - euler_ref)[warmup:], axis=0)
         euler_err_ains = np.std((euler_ains - euler_ref)[warmup:], axis=0)
         np.testing.assert_array_less(euler_err_smth, euler_err_ains)
+
+    def test_benchmark_ahrs(self):
+        # Reference signal
+        fs = 10.24  # sampling rate in Hz
+        _, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs)
+        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 + imu_noise[:, 3:]
+
+        # Aiding measurements
+        pos_noise_std = 0.1  # m
+        head_noise_std = 0.01  # rad
+        rng = np.random.default_rng(0)
+        pos_aid = pos + pos_noise_std * rng.standard_normal(pos.shape)
+        head_aid = head + head_noise_std * rng.standard_normal(head.shape)
+
+        # AINS
+        p0 = pos[0]  # position [m]
+        v0 = vel[0]  # velocity [m/s]
+        q0 = sf.quaternion_from_euler(euler[0], degrees=False)  # 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.AHRS(fs, x0, P0, err_acc, err_gyro)
+
+        smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True)
+
+        euler_ains = []
+        for f_i, w_i, p_i, h_i in zip(acc_imu, gyro_imu, pos_aid, head_aid):
+            smoother.update(
+                f_i,
+                w_i,
+                degrees=False,
+                head=h_i,
+                head_var=head_noise_std**2,
+                head_degrees=False,
+            )
+
+            euler_ains.append(smoother.ains.euler(degrees=False))
+
+        # Forward filter state estimates
+        euler_ains = np.array(euler_ains)
+
+        # Smoothed state estimates
+        euler_smth = smoother.euler(degrees=False)
+
+        # 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)