Merge branch 'master' to reconcile divergent branches

ahrs/filters/angular.py

@@ -267,6 +267,9 @@
 """
 
 import numpy as np
+from ..utils.core import _assert_numerical_iterable
+from ..common.quaternion import QuaternionArray
+from ..common.dcm import DCM
 
 class AngularRate:
     """
@@ -284,9 +287,14 @@ class AngularRate:
         Sampling step in seconds. Inverse of sampling frequency. Not required
         if ``frequency`` value is given.
     method : str, default: ``'closed'``
-        Estimation method to use. Options are: ``'series'`` or ``'closed'``.
+        Estimation method to use. Options are: ``'series'``, ``'closed'``, or
+        ``'integration'``. The option ``'integration'`` is a simple numerical
+        integration of the angular velocity as roll-pitch-yaw angles.
     order : int
         Truncation order, if method ``'series'`` is used.
+    representation : str, default: ``'quaternion'``
+        Attitude representation. Options are ``'quaternion'``, ``'angles'`` or
+        ``'rotmat'``.
 
     Attributes
     ----------
@@ -358,19 +366,64 @@ def __init__(self, gyr: np.ndarray = None, q0: np.ndarray = None, frequency: flo
         self.frequency: float = frequency
         self.order: int = order
         self.method: str = kw.get('method', 'closed')
+        self.representation: str = kw.get('representation', 'quaternion')
         self.Dt: float = kw.get('Dt', 1.0/self.frequency)
         if self.gyr is not None:
+            if self.method.lower() == 'integration':
+                self.W = self.integrate_angular_positions(self.gyr, dt=self.Dt)
             self.Q = self._compute_all()
 
     def _compute_all(self):
         """Estimate all quaternions with given sensor values."""
+        if self.method.lower() == 'integration':
+            return self.integrate_angular_positions(self.gyr, dt=self.Dt)
         num_samples = len(self.gyr)
         Q = np.zeros((num_samples, 4))
         Q[0] = self.q0
         for t in range(1, num_samples):
             Q[t] = self.update(Q[t-1], self.gyr[t], method=self.method, order=self.order)
         return Q
 
+    def integrate_angular_positions(self, gyr: np.ndarray, dt: float = None) -> np.ndarray:
+        """
+        Integrate angular positions from angular rates.
+
+        Integrate angular positions :math:`\\mathbf{\\theta}` from angular
+        rates :math:`\\mathbf{\\omega}` with a given time step :math:`\\Delta t`:
+
+        .. math::
+            \\mathbf{\\theta}_{t+1} = \\mathbf{\\theta}_t + \\mathbf{\\omega}_t \\Delta t
+
+        Parameters
+        ----------
+        gyr : array_like
+            Angular rates, in rad/s.
+        dt : float, optional
+            Time step, in seconds. If not given, the time step is set to
+            :math:`1/f_s`, where :math:`f_s` is the sampling frequency, which
+            is set to 100 Hz by default.
+
+        Returns
+        -------
+        theta : numpy.ndarray
+            Tri-axial angular positions, in rad.
+        """
+        _assert_numerical_iterable(gyr, 'gyr')
+        if self.representation.lower() not in ['quaternion', 'angles', 'rotmat']:
+            raise ValueError(f"Representation must be 'quaternion', 'angles' or 'rotmat'. Got '{self.representation}' instead.")
+        if dt is None:
+            dt = self.Dt
+        if not isinstance(dt, (int, float)):
+            raise TypeError(f"dt must be a float or an integer. Got {type(dt)} instead.")
+        # Angular velocity integration --> Angular position
+        angular_positions = np.cumsum(gyr * dt, axis=0)
+        # Return angular positions in the desired representation
+        if self.representation.lower() == 'quaternion':
+            return QuaternionArray().from_rpy(angular_positions)
+        if self.representation.lower() == 'rotmat':
+            return DCM().from_quaternion(QuaternionArray().from_rpy(angular_positions))
+        return np.unwrap(angular_positions, axis=0)
+
     def update(self, q: np.ndarray, gyr: np.ndarray, method: str = 'closed', order: int = 1, dt: float = None) -> np.ndarray:
         """
         Update the quaternion estimation

ahrs/filters/complementary.py

@@ -210,6 +210,8 @@ def _compute_all(self) -> np.ndarray:
             samples.
 
         """
+        if self.gyr.ndim < 2:
+            raise ValueError(f"All inputs must have at least two observations.")
         if self.acc.shape != self.gyr.shape:
             raise ValueError(f"Could not operate on acc array of shape {self.acc.shape} and gyr array of shape {self.gyr.shape}.")
         W = np.zeros_like(self.acc)
@@ -282,7 +284,10 @@ def am_estimation(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
         angles[:, 1] = np.arctan2(-a[:, 0], np.sqrt(a[:, 1]**2 + a[:, 2]**2))
         if mag is not None:
             # Estimate heading angle
-            m = mag/np.linalg.norm(mag, axis=1)[:, None]
+            m_norm = np.linalg.norm(mag, axis=1)[:, None]
+            if np.where(m_norm == 0)[0].size > 0:
+                raise ValueError("All Magnetic field measurements must be non-zero.")
+            m = mag/m_norm
             by = m[:, 1]*np.cos(angles[:, 0]) - m[:, 2]*np.sin(angles[:, 0])
             bx = m[:, 0]*np.cos(angles[:, 1]) + np.sin(angles[:, 1])*(m[:, 1]*np.sin(angles[:, 0]) + m[:, 2]*np.cos(angles[:, 0]))
             angles[:, 2] = np.arctan2(-by, bx)
@@ -291,6 +296,8 @@ def am_estimation(self, acc: np.ndarray, mag: np.ndarray = None) -> np.ndarray:
     @property
     def Q(self) -> np.ndarray:
         """
+        Attitudes as quaternions.
+
         Returns
         -------
         Q : numpy.ndarray

ahrs/filters/madgwick.py

@@ -3,6 +3,10 @@
 Madgwick Orientation Filter
 ===========================
 
+.. contents:: Table of Contents
+    :local:
+    :depth: 2
+
 This is an orientation filter applicable to IMUs consisting of tri-axial
 gyroscopes and accelerometers, and MARG arrays, which also include tri-axial
 magnetometers, proposed by Sebastian Madgwick [Madgwick]_.
@@ -446,17 +450,17 @@ class Madgwick:
     >>> type(madgwick.Q), madgwick.Q.shape
     (<class 'numpy.ndarray'>, (1000, 4))
 
-    If we desire to estimate each sample independently, we call its *update*
-    method.
+    If we desire to estimate each sample independently, we call the
+    corresponding ``update`` method.
 
     >>> madgwick = Madgwick()
     >>> Q = np.tile([1., 0., 0., 0.], (num_samples, 1)) # Allocate for quaternions
     >>> for t in range(1, num_samples):
     ...     Q[t] = madgwick.updateIMU(Q[t-1], gyr=gyro_data[t], acc=acc_data[t])
 
 
-    This algorithm requires a valid initial attitude. This can be set with the
-    parameter ``q0``:
+    This algorithm requires a valid initial attitude, as a versor. This can be
+    set with the parameter ``q0``:
 
     >>> madgwick = Madgwick(gyr=gyro_data, acc=acc_data, q0=[0.7071, 0.0, 0.7071, 0.0])
 
@@ -466,8 +470,8 @@ class Madgwick:
         equal to ``1.0``.
 
     If no initial orientation is given, an attitude is estimated using the
-    first samples. This attitude is computed assuming the sensors are straped
-    to a body in a quasi-static state.
+    first sample of each sensor. This initial attitude is computed assuming the
+    sensors are straped to a body in a quasi-static state.
 
     Further on, we can also use magnetometer data.
 

docs/source/nomenclature.rst

@@ -116,6 +116,7 @@ CAN     Controller Area Network
 CAS     Collision Avoidance System
 CD      Centered Dipole
 CDI     Course Deviation Indicator
+CG      Center of Gravity
 CGM     Corrected Geomagnetic Coordinates
 CIRAS   Coriolis Inertial Rate and Acceleration Sensor
 CIRS    Conventional Inertial Reference System
@@ -183,6 +184,7 @@ IERS    International Earth Rotation and Reference Systems Service
 IFR     Instrument Flight Rules
 IGRF    International Geomagnetic Reference Field
 ILS     Instrument landing system
+IMC     Intermodule Communication
 IMU     Inertial Measurement Unit
 INS     Inertial Navigation System
 INU     Inertial Navigation Unit
@@ -200,6 +202,7 @@ lon     Longitude
 LORAN   Long-range radio navigation
 LPF     Low-Pass Filter
 LPV     Localizer Performance with Vertical Guidance
+LQR     Linear Quadratic Regulator
 MagCal  Magnetic Calibration
 MANET   Mobile ad hoc Network
 MARG    Magnetism, Angular Rate, and Gravity
@@ -234,11 +237,13 @@ RMS     Root Mean Square
 RNAV    Area Navigation
 RTU     Remote Terminal Unit
 SBAS    Satellite-Based Augmentation System
+SBC     Single Board Computer
 SCADA   System Control and Data Acquisition
 SCI     Serial Communications Interface
 SI      Système International d'unités
 SLERP   Spherical Linear Interpolation
 SOA     Silicon Oscillating Accelerometer
+SPI     Serial Peripheral Interface
 SVD     Singular Value Decomposition
 TACAN   Tactical Air Navigation System
 TAWS    Terrain Awareness and Warning System
@@ -256,6 +261,7 @@ VNAV    Vertical Navigation
 VOR     Very High Frequency Omnidirectional Radio Range
 WAAS    Wide Area Augmentation System
 WGS     World Geodetic System
+WLAN    Wireless Local Area Network
 WMM     World Magnetic Model
 ZUPT    Zero Velocity Update
 ======  =========