FIX unstable QUEST estimation

- All vectors to compare are now normalized, so that the recursive methods find a solution.
- Remove function to set reference gravity vector.
- Add reference magnetic vector as global variable.

ahrs/filters/quest.py

@@ -207,6 +207,8 @@
 from ..utils.wgs84 import WGS
 # Reference Observations in Munich, Germany
 GRAVITY = WGS().normal_gravity(MUNICH_LATITUDE, MUNICH_HEIGHT)
+wmm = WMM(latitude=MUNICH_LATITUDE, longitude=MUNICH_LONGITUDE, height=MUNICH_HEIGHT)
+REFERENCE_MAGNETIC_VECTOR = np.array([wmm.X, wmm.Y, wmm.Z])
 
 def _assert_iterables(item, item_name: str = 'iterable'):
     if not isinstance(item, (list, tuple, np.ndarray)):
@@ -218,49 +220,22 @@ def _set_magnetic_field_vector(self, magnetic_dip: Union[int, float, list, tuple
 
     Parameters
     ----------
-    magnetic_dip : float
-        Magnetic dip, in degrees.
+    magnetic_dip : float, array-like
+        Magnetic dip, in degrees, or local geomagnetic reference as
+        three-dimensional vector.
 
     """
     if isinstance(magnetic_dip, (float, int)):
         magnetic_field = np.array([cosd(magnetic_dip), 0., sind(magnetic_dip)])
     elif isinstance(magnetic_dip, (list, tuple, np.ndarray)):
         magnetic_field = np.copy(magnetic_dip)
     elif magnetic_dip is None:
-        MAG = WMM(latitude=MUNICH_LATITUDE, longitude=MUNICH_LONGITUDE, height=MUNICH_HEIGHT).magnetic_elements
-        magnetic_field = np.array([MAG['X'], MAG['Y'], MAG['Z']])
+        magnetic_field = REFERENCE_MAGNETIC_VECTOR
     else:
         raise TypeError(f"magnetic_dip must be given as a float, list, tuple or NumPy array. Got {type(magnetic_dip)}")
     if magnetic_field.shape != (3,):
         raise ValueError(f"magnetic_dip must be given as a float, list, tuple or NumPy array with 3 elements. Got {magnetic_field.shape}")
-    return magnetic_field
-
-def _set_gravitational_acceleration_vector(self, gravitational_acceleration: Union[int, float, list, tuple, np.ndarray]):
-    """
-    Set the gravitational acceleration vector.
-
-    Parameters
-    ----------
-    gravitational_acceleration : float or array-like
-        Gravitational acceleration, in m/s^2.
-
-    Returns
-    -------
-    gravitaty_vector : numpy.ndarray
-        Gravitational acceleration vector, in m/s^2.
-
-    """
-    if isinstance(gravitational_acceleration, (float, int)):
-        gravity_vector = np.array([0., 0., gravitational_acceleration])
-    elif isinstance(gravitational_acceleration, (list, tuple, np.ndarray)):
-        gravity_vector = np.copy(gravitational_acceleration)
-    elif gravitational_acceleration is None:
-        gravity_vector = np.array([0., 0., GRAVITY])
-    else:
-        raise TypeError(f"gravity must be given as a float, list, tuple or NumPy array. Got {type(gravitational_acceleration)}")
-    if gravity_vector.shape != (3,):
-        raise ValueError(f"gravity must be given as a float, list, tuple or NumPy array with 3 elements. Got {gravity_vector.shape}")
-    return gravity_vector
+    return magnetic_field / np.linalg.norm(magnetic_field)
 
 class QUEST:
     """
@@ -300,7 +275,7 @@ def __init__(self, acc: np.ndarray = None, mag: np.ndarray = None, **kw):
         self.w: np.ndarray = kw.get('weights', 0.5*np.ones(2))
         # Reference measurements
         self.m_q: np.ndarray = _set_magnetic_field_vector(self, kw.get('magnetic_dip'))
-        self.g_q: np.ndarray = _set_gravitational_acceleration_vector(self, kw.get('gravity'))  # Normal Gravity vector
+        self.g_q: np.ndarray = np.array([0., 0., 1.])  # Normal Gravity vector
         if self.acc is not None and self.mag is not None:
             self.Q = self._compute_all()
 
@@ -344,6 +319,8 @@ def estimate(self, acc: np.ndarray, mag: np.ndarray) -> np.ndarray:
         """
         _assert_iterables(acc, 'Gravitational acceleration vector')
         _assert_iterables(mag, 'Geomagnetic field vector')
+        acc = acc/np.linalg.norm(acc)
+        mag = mag/np.linalg.norm(mag)
         B = self.w[0]*np.outer(acc, self.g_q) + self.w[1]*np.outer(mag, self.m_q)   # Attitude profile matrix
         S = B + B.T
         z = np.array([B[1, 2]-B[2, 1], B[2, 0]-B[0, 2], B[0, 1]-B[1, 0]])   # Pseudovector (Axial vector))