Add Unit Test to Fourati Estimator.

Mayitzin · Jan 28, 2022 · 87d2788 · 87d2788
1 parent 24f548f
commit 87d2788
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/tests/test_estimators.py b/tests/test_estimators.py
@@ -293,5 +293,24 @@ def test_marg(self):
         orientation = ahrs.filters.Mahony(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)
 
+class TestFourati(unittest.TestCase):
+    def setUp(self) -> None:
+        # Create random attitudes
+        num_samples = 1000
+        a_ref = REFERENCE_GRAVITY_VECTOR
+        m_ref = ahrs.common.frames.ned2enu(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 @ a_ref for R in rotations]) + np.random.standard_normal((num_samples, 3)) * self.noise_sigma
+        self.Rm = np.array([R @ m_ref for R in rotations]) + np.random.standard_normal((num_samples, 3)) * self.noise_sigma
+
+    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)
+
 if __name__ == '__main__':
     unittest.main()