check err cov ains

Author: vegard-solum-4ss

tests/test_smoothing.py

@@ -66,7 +66,7 @@ def test_benchmark_ains(self):
 
         smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True)
 
-        pos_ains, vel_ains, euler_ains = [], [], []
+        pos_ains, vel_ains, euler_ains, err_ains = [], [], [], []
         for f_i, w_i, p_i, h_i in zip(acc_imu, gyro_imu, pos_aid, head_aid):
             smoother.update(
                 f_i,
@@ -82,16 +82,19 @@ def test_benchmark_ains(self):
             pos_ains.append(smoother.ains.position())
             vel_ains.append(smoother.ains.velocity())
             euler_ains.append(smoother.ains.euler(degrees=True))
+            err_ains.append(smoother.ains.P.diagonal())
 
         # Forward filter state estimates
         pos_ains = np.array(pos_ains)
         vel_ains = np.array(vel_ains)
         euler_ains = np.array(euler_ains)
+        err_ains = np.array(err_ains)
 
         # Smoothed state estimates
         pos_smth = smoother.position()
         vel_smth = smoother.velocity()
         euler_smth = smoother.euler(degrees=True)
+        err_smth = np.array([P_i.diagonal() for P_i in smoother.P])
 
         # Half-sample shift
         # # (compensates for the delay introduced by Euler integration)
@@ -119,6 +122,9 @@ def test_benchmark_ains(self):
         euler_err_ains = np.std((euler_ains - euler_ref)[warmup:], axis=0)
         np.testing.assert_array_less(euler_err_smth, euler_err_ains)
 
+        smoother.P.shape == (len(acc_imu), 12, 12)
+        np.testing.assert_array_less(err_smth[warmup:], err_ains[warmup:] + 1e-12)
+
     def test_benchmark_ahrs(self):
         # Reference signal
         fs = 10.24  # sampling rate in Hz