Add Error if geomagnetic field is invalid in EKF. Remove redundant no…

…rmalization of magnetic measurement vector in FQA.

ahrs/filters/ekf.py

@@ -1013,7 +1013,7 @@ def _set_measurement_noise_covariance(self, **kw) -> np.ndarray:
 
     def _set_reference_frames(self, mref: float, frame: str = 'NED') -> None:
         if frame.upper() not in ['NED', 'ENU']:
-            raise ValueError(f"Invalid frame '{frame}'. Try 'NED' or ENU'")
+            raise ValueError(f"Invalid frame '{frame}'. Try 'NED' or 'ENU'")
         # Magnetic Reference Vector
         if mref is None:
             # Local magnetic reference of Munich, Germany
@@ -1310,8 +1310,9 @@ def update(self, q: np.ndarray, gyr: np.ndarray, acc: np.ndarray, mag: np.ndarra
         self.z = np.copy(a)
         if mag is not None:
             m_norm = np.linalg.norm(mag)
-            if m_norm > 0:
-                self.z = np.r_[a, mag/m_norm]
+            if m_norm == 0:
+                raise ValueError(f"Invalid geomagnetic field. Its magnitude must be greater than zero.")
+            self.z = np.r_[a, mag/m_norm]
         self.R = np.diag(np.repeat(self.noises[1:] if mag is not None else self.noises[1], 3))
         # ----- Prediction -----
         q_t = self.f(q, g)                  # Predicted State

ahrs/filters/fqa.py

@@ -342,7 +342,6 @@ def estimate(self, acc: np.ndarray = None, mag: np.ndarray = None) -> np.ndarray
         if not m_norm>0:
             return q_er
         q_a = np.array([1., 0., 0., 0.])
-        m_norm = np.linalg.norm(mag)
         if m_norm>0:
             m = mag/m_norm
             bm = np.array([0.0, *m])