Add Unit Test for EKF.

Mayitzin · Jan 28, 2022 · 8078536 · 8078536
1 parent e70dd19
commit 8078536
Showing 1 changed file with 23 additions and 0 deletions.
diff --git a/tests/test_estimators.py b/tests/test_estimators.py
@@ -312,5 +312,28 @@ def test_estimation(self):
         orientation = ahrs.filters.Fourati(gyr=self.gyr, acc=self.Rg, mag=self.Rm)
         self.assertLess(np.nanmean(ahrs.utils.metrics.qad(self.Qts, orientation.Q)), self.noise_sigma*20)
 
+class TestEKF(unittest.TestCase):
+    def setUp(self) -> None:
+        # Create random attitudes
+        num_samples = 1000
+        a_ref = REFERENCE_GRAVITY_VECTOR
+        m_ref = REFERENCE_MAGNETIC_VECTOR
+        gyros = random_angvel(num_samples=num_samples, span=(-np.pi, np.pi))
+        self.Qts = ahrs.QuaternionArray(ahrs.filters.AngularRate(gyros).Q)
+        rotations = self.Qts.to_DCM()
+        # Add noise to reference vectors and rotate them by the random attitudes
+        self.noise_sigma = 1e-2
+        self.gyr = gyros + np.random.standard_normal((num_samples, 3)) * self.noise_sigma
+        self.Rg = np.array([R.T @ a_ref for R in rotations]) + np.random.standard_normal((num_samples, 3)) * self.noise_sigma
+        self.Rm = np.array([R.T @ m_ref for R in rotations]) + np.random.standard_normal((num_samples, 3)) * self.noise_sigma
+
+    def test_gyr_acc(self):
+        orientation = ahrs.filters.EKF(gyr=self.gyr, acc=self.Rg)
+        self.assertLess(np.nanmean(ahrs.utils.metrics.qad(self.Qts, orientation.Q)), self.noise_sigma*10.0)
+
+    def test_gyr_acc_mag(self):
+        orientation = ahrs.filters.EKF(gyr=self.gyr, acc=self.Rg, mag=self.Rm)
+        self.assertLess(np.nanmean(ahrs.utils.metrics.qad(self.Qts, orientation.Q)), self.noise_sigma*10.0)
+
 if __name__ == '__main__':
     unittest.main()