From e70942aa50776f80676a232c2e5d2559b8bc165e Mon Sep 17 00:00:00 2001 From: Mayitzin Date: Mon, 27 Jan 2025 20:25:17 +0100 Subject: [PATCH] Use m_norm to normalize magnetometer values, instead of recomputing the norm. --- ahrs/filters/fqa.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ahrs/filters/fqa.py b/ahrs/filters/fqa.py index b28b8e8..77e6195 100644 --- 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