Use m_norm to normalize magnetometer values, instead of recomputing t…

…he norm.
Mayitzin · Jan 27, 2025 · e70942a · e70942a
1 parent be5ac20
commit e70942a
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/ahrs/filters/fqa.py b/ahrs/filters/fqa.py
@@ -374,7 +374,7 @@ def estimate(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
         m_norm = np.linalg.norm(mag)
         if m_norm == 0:
             return q_er.to_array()
-        mag /= np.linalg.norm(mag)
+        mag /= m_norm
         bm = Quaternion([0.0, *mag])
         em = q_e.product(q_r.product(bm.product(Quaternion(q_r.conj).product(q_e.conj))))   # (eq. 34)
         nx, ny, _ = self.m_ref