Merge branch 'master' of https://github.com/Mayitzin/ahrs

ahrs/common/quaternion.py

@@ -333,6 +333,7 @@
 .. [WikiConversions] https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
 .. [WikiQuaternion] https://en.wikipedia.org/wiki/Quaternion
 .. [Wiki_SLERP] https://en.wikipedia.org/wiki/Slerp
+.. [MarioGC1] https://mariogc.com/post/angular-velocity-quaternions/
 
 """
 
@@ -2846,7 +2847,23 @@ def angular_velocities(self, dt: float) -> np.ndarray:
         Compute the angular velocity between N Quaternions.
 
         It assumes a constant sampling rate of ``dt`` seconds, and returns the
-        angular velocity around the X-, Y- and Z-axis, in radians per second.
+        angular velocity around the X-, Y- and Z-axis (roll-pitch-yaw angles),
+        in radians per second.
+
+        The angular velocities :math:`\\omega_x`, :math:`\\omega_y`, and
+        :math:`\\omega_z` are computed from quaternions :math:`\\mathbf{q}_t=\\Big(q_w(t), q_x(t), q_y(t), q_z(t)\\Big)`
+        and :math:`\\mathbf{q}_{t+\\Delta t}=\\Big(q_w(t+\\Delta t), q_x(t+\\Delta t), q_y(t+\\Delta t), q_z(t+\\Delta t)\\Big)`
+        as:
+
+        .. math::
+            \\begin{array}{rcl}
+            \\omega_x &=& \\frac{2}{\\Delta t}\\Big(q_w(t) q_x(t+\\Delta t) - q_x(t) q_w(t+\\Delta t) - q_y(t) q_z(t+\\Delta t) + q_z(t) q_y(t+\\Delta t)\\Big) \\\\ \\\\
+            \\omega_y &=& \\frac{2}{\\Delta t}\\Big(q_w(t) q_y(t+\\Delta t) + q_x(t) q_z(t+\\Delta t) - q_y(t) q_w(t+\\Delta t) - q_z(t) q_x(t+\\Delta t)\\Big) \\\\ \\\\
+            \\omega_z &=& \\frac{2}{\\Delta t}\\Big(q_w(t) q_z(t+\\Delta t) - q_x(t) q_y(t+\\Delta t) + q_y(t) q_x(t+\\Delta t) - q_z(t) q_w(t+\\Delta t)\\Big)
+            \\end{array}
+
+        where :math:`\\Delta t` is the time step between consecutive
+        quaternions [MarioGC1]_.
 
         Parameters
         ----------

ahrs/filters/complementary.py

@@ -3,16 +3,16 @@
 Complementary Filter
 ====================
 
-Attitude quaternion obtained with gyroscope and accelerometer-magnetometer
-measurements, via complementary filter.
+Attitude obtained with gyroscope and accelerometer-magnetometer measurements,
+via complementary filter.
 
 First, the current orientation is estimated at time :math:`t`, from a previous
 orientation at time :math:`t-1`, and a given angular velocity,
 :math:`\\omega`, in rad/s.
 
 This orientation is computed by numerically integrating the angular velocity
-and adding it to the previous orientation, which is known as an **attitude
-propagation**.
+and adding it to the previous orientation, with a process known as an
+**attitude propagation**.
 
 .. math::
     \\begin{array}{rcl}
@@ -141,7 +141,8 @@ class Complementary:
     Raises
     ------
     ValueError
-        When dimension of input arrays ``acc``, ``gyr``, or ``mag`` are not equal.
+        When dimension of input arrays ``acc``, ``gyr``, or ``mag`` are not
+        equal.
 
     """
     def __init__(self,
@@ -249,12 +250,15 @@ def am_estimation(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
             Estimated attitude.
         """
         acc = np.copy(acc)
+        if acc.ndim < 1:
+            raise ValueError("Input 'acc' must be a one- or two-dimensional array. Got shape {acc.shape}.")
         if acc.ndim < 2:
+            # Estimation with one sample of a tri-axial accelerometer
             a_norm = np.linalg.norm(acc)
             if not a_norm > 0:
                 raise ValueError("Gravitational acceleration must be non-zero")
             ax, ay, az = acc/a_norm
-            ### Tilt from Accelerometer
+            # Tilt from Accelerometer
             ex = np.arctan2( ay, az)                        # Roll
             ey = np.arctan2(-ax, np.sqrt(ay**2 + az**2))    # Pitch
             ez = 0.0                                        # Yaw

ahrs/utils/core.py

@@ -2,20 +2,24 @@
 Contains the core utilities for the proper use of AHRS: assertions, data
 handling, etc.
 
+These functions have no other goal in this package than to be used by other
+modules. They are not meant to be used by the user.
+
 This module is private. All functions and objects are available in the main
 ``ahrs`` namespace, or its corresponding submodule - use that instead.
 """
 
 import numpy as np
 
 def _assert_valid_array_type(item, item_name: str = 'iterable'):
-    """Assert it is an iterable"""
+    """Assert it is a list, tuple, or numpy array"""
     # NOTE: This could be changed to a more pythonic solution looking for the
     # dunder method __iter__(), but that yields strings too.
     if not isinstance(item, (list, tuple, np.ndarray)):
         raise TypeError(f"{item_name} must be given as an array. Got {type(item)}")
 
 def _assert_numerical_iterable(item, item_name: str = 'iterable'):
+    """Assert it is a list, tuple, or numpy array, and that it has numerical values"""
     _assert_valid_array_type(item, item_name)
     item_copy = np.copy(item)
     if not(item_copy.dtype == np.dtype(int) or item_copy.dtype == np.dtype(float)):

tests/test_estimators.py

@@ -306,7 +306,7 @@ def setUp(self) -> None:
         # Add noise to reference vectors and rotate them by the random attitudes
         self.Rg = np.array([R @ REFERENCE_GRAVITY_VECTOR for R in REFERENCE_ROTATIONS]) + np.random.standard_normal((NUM_SAMPLES, 3)) * ACC_NOISE_STD_DEVIATION
         self.Rm = np.array([R @ REFERENCE_MAGNETIC_VECTOR for R in REFERENCE_ROTATIONS]) + np.random.standard_normal((NUM_SAMPLES, 3)) * MAG_NOISE_STD_DEVIATION
-        self.threshold = 7.5e-2
+        self.threshold = 8e-2
 
     def test_acc_mag(self):
         orientation = ahrs.filters.AQUA(acc=self.Rg, mag=self.Rm)
@@ -953,7 +953,6 @@ def test_wrong_input_gain(self):
         self.assertRaises(TypeError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=[0.01])
         self.assertRaises(TypeError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=(0.01,))
         self.assertRaises(TypeError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=True)
-        self.assertRaises(ValueError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=0.0)
         self.assertRaises(ValueError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=-0.01)
         self.assertRaises(ValueError, ahrs.filters.Complementary, gyr=self.gyr, acc=self.Rg, mag=self.Rm, gain=1.01)