check err cov

Author: vegard-solum-4ss

tests/test_smoothing.py

@@ -213,7 +213,7 @@ def test_benchmark_vru(self):
 
         smoother = sf.FixedIntervalSmoother(ains, cov_smoothing=True)
 
-        euler_ains = []
+        euler_ains, err_ains = [], []
         for f_i, w_i in zip(acc_imu, gyro_imu):
             smoother.update(
                 f_i,
@@ -222,12 +222,15 @@ def test_benchmark_vru(self):
             )
 
             euler_ains.append(smoother.ains.euler(degrees=True))
+            err_ains.append(smoother.ains.P.diagonal())
 
         # Forward filter state estimates
         euler_ains = np.array(euler_ains)
+        err_ains = np.array(err_ains)
 
         # Smoothed state estimates
         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)
@@ -245,3 +248,6 @@ def test_benchmark_vru(self):
         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)
+
+        smoother.P.shape == (len(acc_imu), 12, 12)
+        np.testing.assert_array_less(err_smth[warmup:], err_ains[warmup:] + 1e-12)