Merge branch 'develop_testing'

ahrs/common/dcm.py

@@ -94,6 +94,10 @@
 from .orientation import itzhack
 from .orientation import sarabandi
 
+def _assert_iterables(item, item_name: str = 'iterable'):
+    if not isinstance(item, (list, tuple, np.ndarray)):
+        raise TypeError(f"{item_name} must be given as an array, got {type(item)}")
+
 class DCM(np.ndarray):
     """
     Direction Cosine Matrix in SO(3)
@@ -210,15 +214,20 @@ def __new__(subtype, array: np.ndarray = None, **kwargs):
                 array = array@rotation('y', kwargs.pop('y', 0.0))
                 array = array@rotation('z', kwargs.pop('z', 0.0))
             if 'rpy' in kwargs:
-                array = rot_seq('zyx', kwargs.pop('rpy'))
+                angles = kwargs.pop('rpy')
+                _assert_iterables(angles, "Roll-Pitch-Yaw angles")
+                if len(angles) != 3:
+                    raise ValueError("roll-pitch-yaw angles must be an array with 3 rotations in degrees.")
+                array = rot_seq('zyx', angles)
             if 'euler' in kwargs:
                 seq, angs = kwargs.pop('euler')
                 array = rot_seq(seq, angs)
             if 'axang' in kwargs:
                 ax, ang = kwargs.pop('axang')
                 array = DCM.from_axisangle(DCM, np.array(ax), ang)
-        if array.shape[-2:]!=(3, 3):
-            raise ValueError("Direction Cosine Matrix must have shape (3, 3) or (N, 3, 3), got {}.".format(array.shape))
+        _assert_iterables(array, "Direction Cosine Matrix")
+        if array.shape[-2:] != (3, 3):
+            raise ValueError(f"Direction Cosine Matrix must have shape (3, 3) or (N, 3, 3), got {array.shape}.")
         in_SO3 = np.isclose(np.linalg.det(array), 1.0)
         in_SO3 &= np.allclose(array@array.T, np.identity(3))
         if not in_SO3:
@@ -610,6 +619,9 @@ def from_axisangle(self, axis: np.ndarray, angle: float) -> np.ndarray:
                [ 0.34202014,  0.46984631,  0.81379768]])
 
         """
+        _assert_iterables(axis)
+        if not isinstance(angle, (int, float)):
+            raise ValueError(f"`angle` must be a float value. Got {type(angle)}")
         axis /= np.linalg.norm(axis)
         K = skew(axis)
         return np.identity(3) + np.sin(angle)*K + (1-np.cos(angle))*K@K
@@ -713,9 +725,10 @@ def from_quaternion(self, q: np.ndarray) -> np.ndarray:
         """
         if q is None:
             return np.identity(3)
+        _assert_iterables(q, "Quaternion")
         q = np.copy(q)
         if q.shape[-1] != 4 or q.ndim > 2:
-            raise ValueError("Quaternion must be of the form (4,) or (N, 4)")
+            raise ValueError(f"Quaternion must be of the form (4,) or (N, 4). Got {q.shape}")
         if q.ndim > 1:
             q /= np.linalg.norm(q, axis=1)[:, None]     # Normalize
             R = np.zeros((q.shape[0], 3, 3))
@@ -784,7 +797,7 @@ def from_q(self, q: np.ndarray) -> np.ndarray:
         """
         return self.from_quaternion(q)
 
-    def to_quaternion(self, method: str = 'chiaverini', **kw) -> np.ndarray:
+    def to_quaternion(self, method: str='chiaverini', **kw) -> np.ndarray:
         """
         Quaternion from Direction Cosine Matrix.
 
@@ -826,19 +839,19 @@ def to_quaternion(self, method: str = 'chiaverini', **kw) -> np.ndarray:
 
         """
         q = np.array([1., 0., 0., 0.])
-        if method.lower()=='hughes':
+        if method.lower() == 'hughes':
             q = hughes(self.A)
-        if method.lower()=='chiaverini':
+        if method.lower() == 'chiaverini':
             q = chiaverini(self.A)
-        if method.lower()=='shepperd':
+        if method.lower() == 'shepperd':
             q = shepperd(self.A)
-        if method.lower()=='itzhack':
+        if method.lower() == 'itzhack':
             q = itzhack(self.A, version=kw.get('version', 3))
-        if method.lower()=='sarabandi':
+        if method.lower() == 'sarabandi':
             q = sarabandi(self.A, eta=kw.get('threshold', 0.0))
         return q/np.linalg.norm(q)
 
-    def to_q(self, method: str = 'chiaverini', **kw) -> np.ndarray:
+    def to_q(self, method: str='chiaverini', **kw) -> np.ndarray:
         """
         Synonym of method :meth:`to_quaternion`.
 
@@ -907,7 +920,7 @@ def ode(self, w: np.ndarray) -> np.ndarray:
         Parameters
         ----------
         w : numpy.ndarray
-            Angular velocity, in rad/s, about X-, Y- and Z-axis.
+            Instantaneous angular velocity, in rad/s, about X-, Y- and Z-axis.
 
         Returns
         -------

ahrs/common/orientation.py

@@ -662,6 +662,9 @@ def rot_seq(axes: Union[list, str] = None, angles: Union[list, float] = None) ->
         return R
     if angles is None:
         angles = np.random.uniform(low=-180.0, high=180.0, size=num_rotations)
+    for x in angles:
+        if not isinstance(x, (float, int)):
+            raise TypeError(f"Angles must be float or int numbers. Got {type(x)}")
     if set(axes).issubset(set(accepted_axes)):
         # Perform the matrix multiplications
         for i in range(num_rotations-1, -1, -1):

ahrs/common/quaternion.py

@@ -333,14 +333,18 @@
 """
 
 import numpy as np
-from typing import Type, Union, Any, Tuple, List
+from typing import Union, Any, Tuple
 # Functions to convert DCM to quaternion representation
 from .orientation import shepperd
 from .orientation import hughes
 from .orientation import chiaverini
 from .orientation import itzhack
 from .orientation import sarabandi
 
+def _assert_iterables(item, item_name: str = 'iterable'):
+    if not isinstance(item, (list, tuple, np.ndarray)):
+        raise TypeError(f"{item_name} must be given as an array, got {type(item)}")
+
 def slerp(q0: np.ndarray, q1: np.ndarray, t_array: np.ndarray, threshold: float = 0.9995) -> np.ndarray:
     """Spherical Linear Interpolation between two quaternions.
 
@@ -483,19 +487,23 @@ class Quaternion(np.ndarray):
     '(0.0000 +0.2673i +0.5345j +0.8018k)'
 
     """
-    def __new__(subtype, q: np.ndarray = None, versor: bool = True, **kwargs):
+    def __new__(subtype, q: Union[list, np.ndarray] = None, versor: bool = True, **kwargs):
         if q is None:
             q = np.array([1.0, 0.0, 0.0, 0.0])
             if "dcm" in kwargs:
-                q = Quaternion.from_DCM(Quaternion, np.array(kwargs.pop("dcm")))
+                q = Quaternion.from_DCM(Quaternion, kwargs.pop("dcm"), **kwargs)
             if "rpy" in kwargs:
-                q = Quaternion.from_rpy(Quaternion, np.array(kwargs.pop("rpy")))
+                q = Quaternion.from_rpy(Quaternion, kwargs.pop("rpy"))
             if "angles" in kwargs:  # Older call to rpy
-                q = Quaternion.from_angles(Quaternion, np.array(kwargs.pop("angles")))
+                q = Quaternion.from_angles(Quaternion, kwargs.pop("angles"))
+        if not isinstance(q, (list, np.ndarray)):
+            raise TypeError(f"Expected `q` must be a list or a numpy array. Got {type(q)}")
         q = np.array(q, dtype=float)
-        if q.ndim!=1 or q.shape[-1] not in [3, 4]:
-            raise ValueError("Expected `q` to have shape (4,) or (3,), got {}.".format(q.shape))
-        if q.shape[-1]==3:
+        if q.ndim != 1 or q.shape[-1] not in [3, 4]:
+            raise ValueError(f"Expected `q` to have shape (4,) or (3,), got {q.shape}.")
+        if q.shape[-1] == 3:
+            if not np.any(q):
+                raise ValueError("Expected `q` to be a non-zero vector.")
             q = np.array([0.0, *q])
         if versor:
             q /= np.linalg.norm(q)
@@ -931,12 +939,12 @@ def log(self) -> np.ndarray:
         Examples
         --------
         >>> q = Quaternion([1.0, -2.0, 3.0, -4.0])
-        >>> q.view()
+        >>> q
         Quaternion([ 0.18257419, -0.36514837,  0.54772256, -0.73029674])
         >>> q.log
         array([ 0.        , -0.51519029,  0.77278544, -1.03038059])
         >>> q = Quaternion([0.0, 1.0, -2.0, 3.0])
-        >>> q.view()
+        >>> q
         Quaternion([ 0.        ,  0.26726124, -0.53452248,  0.80178373])
         >>> q.log
         array([ 0.        ,  0.41981298, -0.83962595,  1.25943893])
@@ -958,7 +966,7 @@ def __str__(self) -> str:
         >>> str(q)
         '(0.5575 +0.1296i +0.5737j +0.5859k)'
         """
-        return "({:-.4f} {:+.4f}i {:+.4f}j {:+.4f}k)".format(self.w, self.x, self.y, self.z)
+        return f"({self.w:-.4f} {self.x:+.4f}i {self.y:+.4f}j {self.z:+.4f}k)"
 
     def __add__(self, p: Any):
         """
@@ -1437,7 +1445,7 @@ def rotate(self, a: np.ndarray) -> np.ndarray:
         """
         a = np.array(a)
         if a.shape[0] != 3:
-            raise ValueError("Expected `a` to have shape (3, N) or (3,), got {}.".format(a.shape))
+            raise ValueError(f"Expected `a` to have shape (3, N) or (3,), got {a.shape}.")
         return self.to_DCM()@a
 
     def to_array(self) -> np.ndarray:
@@ -1615,22 +1623,32 @@ def from_DCM(self, dcm: np.ndarray, method: str = 'chiaverini', **kw) -> np.ndar
             Method to use. Options are: 'chiaverini', 'hughes', 'itzhack',
             'sarabandi', and 'shepperd'.
 
+        Notes
+        -----
+        The selection can be simplified with Structural Pattern Matching if
+        using Python 3.10 or later.
+
         """
+        _assert_iterables(dcm, 'Direction Cosine Matrix')
+        in_SO3 = np.isclose(np.linalg.det(np.atleast_2d(dcm)), 1.0)
+        in_SO3 &= np.allclose(dcm@dcm.T, np.identity(3))
+        if not in_SO3:
+            raise ValueError("Given Direction Cosine Matrix is not in SO(3).")
+        dcm = np.copy(dcm)
         if dcm.shape != (3, 3):
-            raise TypeError("Expected matrix of size (3, 3). Got {}".format(dcm.shape))
-        q = None
-        if method.lower()=='hughes':
+            raise TypeError(f"Expected matrix of size (3, 3). Got {dcm.shape}")
+        if method.lower() == 'hughes':
             q = hughes(dcm)
-        if method.lower()=='chiaverini':
+        elif method.lower() == 'chiaverini':
             q = chiaverini(dcm)
-        if method.lower()=='shepperd':
+        elif method.lower() == 'shepperd':
             q = shepperd(dcm)
-        if method.lower()=='itzhack':
+        elif method.lower() == 'itzhack':
             q = itzhack(dcm, version=kw.get('version', 3))
-        if method.lower()=='sarabandi':
+        elif method.lower() == 'sarabandi':
             q = sarabandi(dcm, eta=kw.get('threshold', 0.0))
-        if q is None:
-            raise KeyError("Given method '{}' is not implemented.".format(method))
+        else:
+            raise KeyError(f"Given method '{method}' is not implemented.")
         q /= np.linalg.norm(q)
         return q
 
@@ -1704,9 +1722,13 @@ def from_rpy(self, angles: np.ndarray) -> np.ndarray:
         With both approaches the same quaternion is obtained.
 
         """
+        _assert_iterables(angles, 'Roll-Pitch-Yaw angles')
         angles = np.array(angles)
         if angles.ndim != 1 or angles.shape[0] != 3:
-            raise ValueError("Expected `angles` to have shape (3,), got {}.".format(angles.shape))
+            raise ValueError(f"Expected `angles` must have shape (3,), got {angles.shape}.")
+        for angle in angles:
+            if angle < -2.0* np.pi or angle > 2.0 * np.pi:
+                raise ValueError(f"Expected `angles` must be in the range [-2pi, 2pi], got {angles}.")
         yaw, pitch, roll = angles
         cy = np.cos(0.5*yaw)
         sy = np.sin(0.5*yaw)
@@ -1774,8 +1796,9 @@ def ode(self, w: np.ndarray) -> np.ndarray:
             Derivative of quaternion
 
         """
+        _assert_iterables(w, 'Angular velocity')
         if w.ndim != 1 or w.shape[0] != 3:
-            raise ValueError("Expected `w` to have shape (3,), got {}.".format(w.shape))
+            raise ValueError(f"Expected `w` to have shape (3,), got {w.shape}")
         F = np.array([
             [0.0, -w[0], -w[1], -w[2]],
             [w[0], 0.0, -w[2], w[1]],
@@ -1927,9 +1950,10 @@ class QuaternionArray(np.ndarray):
     def __new__(subtype, q: np.ndarray = None, versors: bool = True):
         if q is None:
             q = np.array([[1.0, 0.0, 0.0, 0.0]])
+        _assert_iterables(q, 'Quaternion Array')
         q = np.array(q, dtype=float)
-        if q.ndim!=2 or q.shape[-1] not in [3, 4]:
-            raise ValueError("Expected array to have shape (N, 4) or (N, 3), got {}.".format(q.shape))
+        if q.ndim != 2 or q.shape[-1] not in [3, 4]:
+            raise ValueError(f"Expected array to have shape (N, 4) or (N, 3), got {q.shape}.")
         if q.shape[-1] == 3:
             q = np.c_[np.zeros(q.shape[0]), q]
         if versors:
@@ -2543,7 +2567,7 @@ def average(self, span: Tuple[int, int] = None, weights: np.ndarray = None) -> n
             else:
                 raise ValueError("span must be a pair of integers indicating the indices of the data.")
         if weights is not None:
-            if weights.ndim>1:
+            if weights.ndim > 1:
                 raise ValueError("The weights must be in a one-dimensional array.")
             if weights.size != q.shape[0]:
                 raise ValueError("The number of weights do not match the number of quaternions.")
@@ -2624,6 +2648,7 @@ def rotate_by(self, q: np.ndarray) -> np.ndarray:
                [-0.00284403 -0.29514739]])
 
         """
+        _assert_iterables(q, 'Quaternion')
         q = np.copy(q)
         if q.size != 4:
             raise ValueError("Given quaternion to rotate about must have 4 elements.")

ahrs/filters/triad.py

@@ -318,20 +318,40 @@ def __init__(self,
         v2: np.ndarray = None,
         representation: str = 'rotmat',
         frame: str = 'NED'):
-        self.w1: np.ndarray = np.copy(w1)
-        self.w2: np.ndarray = np.copy(w2)
+        self._guard_clauses_parameters(representation, frame)
+        self._guard_clauses_vectors(w1, w2, v1, v2)
         self.representation: str = representation
-        if representation.lower() not in ['rotmat', 'quaternion']:
-            raise ValueError("Wrong representation type. Try 'rotmat', or 'quaternion'")
-        if frame.upper() not in ['NED', 'ENU']:
-            raise ValueError(f"Given frame {frame} is NOT valid. Try 'NED' or 'ENU'")
+        self.frame: str = frame
+        # Input values
+        self.w1: np.ndarray = np.copy(w1) if isinstance(w1, (list, np.ndarray)) else None
+        self.w2: np.ndarray = np.copy(w2) if isinstance(w2, (list, np.ndarray)) else None
         # Reference frames
         self.v1 = self._set_first_triad_reference(v1, frame)
         self.v2 = self._set_second_triad_reference(v2, frame)
         # Compute values if samples given
-        if all([self.w1, self.w2]):
+        if self.w1 is not None and self.w2 is not None:
             self.A = self._compute_all(self.representation)
 
+    def _guard_clauses_parameters(self, representation, frame):
+        for item in [representation, frame]:
+            if not isinstance(item, str):
+                raise TypeError(f"{item} must be a string")
+        if representation.lower() not in ['rotmat', 'quaternion']:
+            raise ValueError(f"Given representation '{representation}' is NOT valid. Try 'rotmat', or 'quaternion'")
+        if frame.upper() not in ['NED', 'ENU']:
+            raise ValueError(f"Given frame '{frame}' is NOT valid. Try 'NED' or 'ENU'")
+
+    def _guard_clauses_vectors(self, *vectors):
+        for item in vectors:
+            if not isinstance(item, (list, np.ndarray, type(None))):
+                raise TypeError(f"{item} must be a list or numpy.ndarray. It is {type(item)}")
+        w1, w2, *_ = vectors
+        if w1 is None and w2 is None:
+            return None
+        w1, w2 = np.copy(w1), np.copy(w2)
+        if w1.shape != w2.shape:
+            raise ValueError(f"Vectors must have the same shape. w1: {w1.shape}, w2: {w2.shape}")
+
     def _set_first_triad_reference(self, value, frame):
         if value is None:
             ref = np.array([0.0, 0.0, 1.0]) if frame.upper == 'NED' else np.array([0.0, 0.0, -1.0])
@@ -341,16 +361,17 @@ def _set_first_triad_reference(self, value, frame):
         return ref
 
     def _set_second_triad_reference(self, value, frame):
-        ref = np.array([MAG['X'], MAG['Y'], MAG['Z']])
         if isinstance(value, float):
-            if abs(value)>90:
+            if abs(value) > 90:
                 raise ValueError(f"Dip Angle must be within range [-90, 90]. Got {value}")
             ref = np.array([cosd(value), 0.0, sind(value)]) if frame.upper() == 'NED' else np.array([0.0, cosd(value), -sind(value)])
-        if isinstance(value, (np.ndarray, list)):
+        elif isinstance(value, (np.ndarray, list)):
             ref = np.copy(value)
+        else:
+            ref = np.array([MAG['X'], MAG['Y'], MAG['Z']])
         return ref/np.linalg.norm(ref)
 
-    def _compute_all(self, representation) -> np.ndarray:
+    def _compute_all(self, representation: str) -> np.ndarray:
         """
         Estimate the attitude given all data.
 
@@ -369,7 +390,7 @@ def _compute_all(self, representation) -> np.ndarray:
             if ``representation`` is set to ``'quaternion'``.
 
         """
-        if self.w1.shape!=self.w2.shape:
+        if self.w1.shape != self.w2.shape:
             raise ValueError("w1 and w2 are not the same size")
         if self.w1.ndim == 1:
             return self.estimate(self.w1, self.w2, representation)