Add Documentation

- Add autogenerated sphinx documentation. - Update information in README. Add table of algorithms and remove old examples. - Improve docstrings of filters. - Add descriptions in main docstrings of WMM and WGS84. - Change main characteristics of Quaternion class as properties. Improves their use. - Move DCM to quaternion methods to submodule orientation. So it can be widely used. - Add methods `average` and `remove_jumps` to class QuaternionArray. - Add method `to_angles` to class DCM. - Add angle and quaternion metrics to submodule metrics. - Fix reading coefficient files in class WMM. - Update contact email.
Mayitzin · Aug 5, 2020 · 61271a3 · 61271a3
1 parent 4bcf329
commit 61271a3
Show file tree

Hide file tree

Showing 58 changed files with 4,021 additions and 2,132 deletions.
diff --git a/.gitignore b/.gitignore
@@ -22,5 +22,4 @@ dist/*
 .vscode
 
 # Documentation
-docs/*
 doctest/*
diff --git a/README.md b/README.md
@@ -13,7 +13,7 @@ Orginally, an [AHRS](https://en.wikipedia.org/wiki/Attitude_and_heading_referenc
 
 This package's focus is **fast prototyping**, **education**, **testing** and **easy modularity**. Performance is _NOT_ the main goal. For optimized implementations there are endless resources in C/C++.
 
-AHRS is compatible with __Python 3.6__ and newer.
+AHRS is compatible with **Python 3.6** and newer.
 
 ## Installation
 
@@ -33,36 +33,6 @@ python setup.py install
 
 AHRS depends on common packages [NumPy](https://numpy.org/) and [SciPy](https://www.scipy.org/). Further packages are avoided, to reduce its third-party dependency.
 
-## Using AHRS
-
-To play with orientations, for example, we can use the `orientation` module.
-
-```python
->>> from ahrs.common.orientation import rotation
->>> Rx = rotation('x', 10.0)
->>> Ry = rotation('y', 20.0)
->>> Rz = rotation('z', 30.0)
->>> Rx@Ry@Rz                # Rotation product: R_y(10.0) @ R_x(20.0) @ R_y(30.0)
-array([[ 0.81379768 -0.46984631  0.34202014],
-       [ 0.54383814  0.82317294 -0.16317591],
-       [-0.20487413  0.31879578  0.92541658]])
->>> from ahrs.common.orientation import rot_seq
->>> rot_seq('xyz', [10.0, 20.0, 30.0])      # Same rotation calling rot_seq()
-array([[ 0.81379768 -0.46984631  0.34202014],
-       [ 0.54383814  0.82317294 -0.16317591],
-       [-0.20487413  0.31879578  0.92541658]])
-```
-
-Or the newly implemented class `DCM`
-
-```python
->>> from ahrs import DCM
->>> DCM(x=10.0, y=20.0, z=30.0)
-DCM([[ 0.81379768 -0.46984631  0.34202014],
-     [ 0.54383814  0.82317294 -0.16317591],
-     [-0.20487413  0.31879578  0.92541658]])
-```
-
 ## New in 0.3
 
 - [Type hints](https://www.python.org/dev/peps/pep-0484/) are added.
@@ -86,7 +56,7 @@ DCM([[ 0.81379768 -0.46984631  0.34202014],
 ['a', 'arithmetic_mean_radius', 'aspect_ratio', 'atmosphere_gravitational_constant', 'authalic_sphere_radius', 'curvature_polar_radius', 'dynamic_inertial_moment_about_X', 'dynamic_inertial_moment_about_Y', 'dynamic_inertial_moment_about_Z', 'dynamical_form_factor', 'equatorial_normal_gravity', 'equivolumetric_sphere_radius', 'f', 'first_eccentricity_squared', 'geometric_dynamic_ellipticity', 'geometric_inertial_moment', 'geometric_inertial_moment_about_Z', 'gm', 'gravitational_constant_without_atmosphere', 'is_geodetic', 'linear_eccentricity', 'mass', 'mean_normal_gravity', 'normal_gravity', 'normal_gravity_constant', 'normal_gravity_potential', 'polar_normal_gravity', 'second_degree_zonal_harmonic', 'second_eccentricity_squared', 'semi_minor_axis', 'w']
 ```
 
-It can be used, for example, to estimate the normal gravity acceleration (in $m/s^2$) at any location on Earth.
+It can be used, for example, to estimate the normal gravity acceleration (in m/s^2) at any location on Earth.
 
 ```python
 >>> g = wgs.normal_gravity(50.0, h=500.0)        # Latitude = 50° N,   height above sea = 500 m
@@ -105,13 +75,13 @@ Setting the fundamental parameters (`a`, `f`, `gm`, `w`) yields a different elli
 - The [International Gravity Formula](http://earth.geology.yale.edu/~ajs/1945A/360.pdf) and the EU's [WELMEC](https://www.welmec.org/documents/guides/2/) normal gravity reference system are also implemented.
 
 ```python
->>> ahrs.utils.international_gravity(50.0)       # Works only on surface
+>>> ahrs.utils.international_gravity(50.0)       # Works only at sea level
 9.810786421572386
->>> ahrs.utils.welmec_gravity(50.0, 500.0)
+>>> ahrs.utils.welmec_gravity(50.0, 500.0)       # This can be also above sea level (here 500 m)
 9.809152687885897
 ```
 
-- New class `DCM` (derived from `numpy.ndarray` to use its advantages) for orientation/rotation representations as $3\times 3$ Direction Cosine Matrices.
+- New class `DCM` (derived from `numpy.ndarray` to use its attributes and methods) for orientation/rotation representations as 3x3 Direction Cosine Matrices.
 
 ```python
 >>> from ahrs import DCM
@@ -122,16 +92,18 @@ Setting the fundamental parameters (`a`, `f`, `gm`, `w`) yields a different elli
 DCM([[ 0.81379768 -0.46984631  0.34202014],
      [ 0.54383814  0.82317294 -0.16317591],
      [-0.20487413  0.31879578  0.92541658]])
->>> R.inv
+>>> R.inv     # or R.I
 array([[ 0.81379768  0.54383814 -0.20487413]
        [-0.46984631  0.82317294  0.31879578]
        [ 0.34202014 -0.16317591  0.92541658]])
 >>> R.log
 array([0.26026043, 0.29531805, 0.5473806 ])
+>>> R.to_axisangle()        # Axis as 3D NumPy array, and angle as radians
+(array([0.38601658, 0.43801381, 0.81187135]), 0.6742208510527136)
 >>> R.to_quaternion()
 array([0.94371436, 0.12767944, 0.14487813, 0.26853582])
->>> R.to_axisangle()
-(array([0.38601658, 0.43801381, 0.81187135]), 0.6742208510527136)
+>>> R.to_quaternion(method='itzhack', version=2)
+array([ 0.94371436, -0.12767944, -0.14487813, -0.26853582])
 ```
 
 - A whole bunch of new constant values (mainly for Geodesy) accessed from the top level of the package.
@@ -155,12 +127,12 @@ Some algorithms allow a finer tuning of its estimation with different parameters
 
 Implemented attitude estimators are checked:
 
-- [x] Angular velocity integration
 - [x] AQUA (Algebraic Quaternion Algorithm)
 - [x] Complementary Filter with Quaternions
 - [x] Davenport's q-method
 - [ ] ESOQ (Estimator of the Optimal Quaternion)
 - [ ] ESOQ-2 (New Estimator of the Optimal Quaternion)
+- [x] FAMC (Fast Accelerometer-Magnetometer Combination)
 - [ ] FCF (Fast Complementary Filter)
 - [ ] FKF (Fast Kalman Filter)
 - [x] FLAE (Fast Linear Attitude Estimator)
@@ -171,21 +143,43 @@ Implemented attitude estimators are checked:
 - [x] Madgwick (Gradient-Descent Estimator)
 - [ ] MAGYQ (Magnetic, Acceleration Fields and Gyroscope Quaternion)
 - [x] Mahony (Nonlinear Explicit Complementary Filter)
+- [x] Naïve Angular velocity integration
 - [ ] OLAE (Optimal Linear Attitude Estimator)
 - [ ] OLEQ (Optimal Linear Estimator of Quaternion)
 - [x] QUEST (QUaternion ESTimator)
 - [ ] REQUEST (Recursive QUEST)
 - [ ] ROLEQ (Recursive Optimal Linear Estimator of Quaternion)
-- [ ] Sabatini
+- [x] SAAM (Super-fast Attitude of Accelerometer and Magnetometer)
+- [ ] Sabatini (EKF-Based Quaternion Estimator)
 - [ ] SOLEQ (Sub-Optimal Linear Estimator of Quaternion)
-- [x] TRIAD
+- [x] Tilt (Inclination and Heading)
+- [x] TRIAD (Tri-Axial Attitude Determination)
+
+| Algorithm      | Gyroscope | Accelerometer | Magnetometer |
+|----------------|:---------:|:-------------:|:------------:|
+| AQUA           | YES       | Optional      | Optional     |
+| Complementary  | YES       | YES           | YES          |
+| Davenport's    | NO        | YES           | YES          |
+| FAMC           | NO        | YES           | YES          |
+| FLAE           | NO        | YES           | YES          |
+| Fourati        | YES       | YES           | YES          |
+| FQA            | NO        | YES           | Optional     |
+| Integration    | YES       | NO            | NO           |
+| Madgwick       | YES       | YES           | Optional     |
+| Mahony         | YES       | YES           | Optional     |
+| OLEQ           | NO        | YES           | YES          |
+| QUEST          | NO        | YES           | YES          |
+| SAAM           | NO        | YES           | YES          |
+| ROLEQ          | YES       | YES           | YES          |
+| Tilt           | NO        | YES           | YES          |
+| TRIAD          | NO        | YES           | YES          |
 
 ## Notes for future versions
 
 `ahrs` is still moving away from plotting and data parsing submodules to better focus in the algorithmic parts. Submodules `io` and `plot` will not be further developed and, eventually, will be removed.
 
-This way you can also choose your favorite libraries for data loading and visualization. This also means, we get rid of its dependency on `matplotlib`.
+That way you can also choose your favorite libraries for data loading and visualization. This also means, getting rid of its dependency on `matplotlib`.
 
 ## Documentation
 
-A comprehensive documentation, with examples, will soon come to [Read the Docs](https://docs.readthedocs.io/).
+A comprehensive documentation, with examples, will soon come to [Read the Docs](https://docs.readthedocs.io/). In the meantime you can easily access the fully documented code by calling `help()` over your desired object.
diff --git a/ahrs/__init__.py b/ahrs/__init__.py
@@ -11,7 +11,9 @@
 from . import common
 from . import filters
 from . import utils
+from .common.constants import *
 from .common.quaternion import Quaternion
+from .common.dcm import DCM
 from .versioning import get_version
 
 __version__ = get_version()
diff --git a/ahrs/common/__init__.py b/ahrs/common/__init__.py
@@ -8,7 +8,7 @@
 from . import orientation
 from . import geometry
 from . import frames
-from .constants import *
 
 from .mathfuncs import M_PI, DEG2RAD, RAD2DEG
 from .quaternion import Quaternion
+from .dcm import DCM
diff --git a/ahrs/common/dcm.py b/ahrs/common/dcm.py
@@ -3,36 +3,54 @@
 Direction Cosine Matrix
 =======================
 
-Rotations are linear transformations in Euclidean three-dimensional spaces
-about the origin. They have several representations. The most common to use and
-easier to understand are the Direction Cosine Matrices, or Rotation Matrices.
-
 An orientation (attitude) is the difference between two coordinate frames. This
-difference is determined by the coordinates of its orthonormal vectors. For
+difference is determined by the coordinates of its _orthonormal_ vectors. For
 three dimensions the discrepancy between any given frame and the base
 coordinate frame is the orientation.
 
 Three vectors :math:`r_1`, :math:`r_2` and :math:`r_3` are the unit vectors
-along the principal axes :math:`X`, :math:`Y` and :math:`Z`. An orientation is
-defined by a :math:`3\times 3` matrix:
+along the principal axes :math:`X`, :math:`Y` and :math:`Z`. The orientation is
+formally defined by a :math:`3\\times 3` matrix:
 
 .. math::
 
-    R = \\begin{bmatrix}r_1 & r_2 & r_3\\end{bmatrix} \\in \\mathbb{R}^{3\times 3}
+    R = \\begin{bmatrix}r_1 & r_2 & r_3\\end{bmatrix} \\in \\mathbb{R}^{3\\times 3}
 
 where :math:`r_1`, :math:`r_2` and :math:`r_3` are unit vectors stacked as
-columns and represent an orthonormal frame. Thus, :math:`RR^{-1}=RR^T=R^TR=I`,
-indicating that the inverse of :math:`R` is its transpose. All matrices
-satisfying this condition are called _orthogonal matrices_ .
+columns and represent an orthonormal frame. Thanks to this orthonormality,
+:math:`RR^{-1}=RR^T=R^TR=I`, indicating that the inverse of :math:`R` is its
+transpose. All matrices satisfying this condition are called _orthogonal matrices_ .
+
+Its determinant is always :math:`+1`, so its product with any vector will leave the
+vector's length unchanged. Matrices conforming to this properties belong to the
+special orthogonal group :math:`SO(3)`.
 
-Its determinant is always +1, so its product with any vector will leave the
-vector's length unchanged.
+Even better, the product of two or more rotation matrices yields another
+rotation matrix in :math:`SO(3)`
 
 Direction cosines are cosines of angles between a vector and a base coordinate
 frame. In this case, the difference between orthogonal vectors :math:`r_i` and
 the base frame are describing the Direction Cosines. This orientation matrix is
 commonly named the "Direction Cosine Matrix."
 
+DCMs are, therefore, the most common representation of rotations, especially in
+real applications of spacecraft tracking and location.
+
+Rotational Motion
+-----------------
+
+Rotations are linear transformations in Euclidean three-dimensional spaces
+about the origin. They have several representations. The most common to use and
+easier to understand are the Direction Cosine Matrices, or Rotation Matrices.
+
+Strictly speaking, a **rotation matrix** is the matrix that when pre-multiplied
+by a vector expressed in the world coordinates yields the same vector expressed
+in the body-fixed coordinates.
+
+A rotation matrix can also be referred to as a _direction cosine matrix_,
+because its elements are the cosines of the unsigned angles between body-fixed
+axes and the world axes.
+
 References
 ----------
 .. [1] Wikipedia: Direction Cosine.
@@ -47,14 +65,20 @@
        Imaging Vis 35, 155–164 (2009).
 .. [5] Howard D Curtis. Orbital Mechanics for Engineering Students (Third
        Edition) Butterworth-Heinemann. 2014.
+.. [6] Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with
+       Applications to Orbits, Aerospace and Virtual Reality. Princeton;
+       Oxford: Princeton University Press, 1999.
+.. [7] Diebel, James. Representing Attitude; Euler Angles, Unit Quaternions,
+       and Rotation. Stanford University. 20 October 2006.
 
 """
 
-from typing import NoReturn, Tuple
+from typing import Tuple
 import numpy as np
 from .mathfuncs import *
 from .orientation import rotation
 from .orientation import rot_seq
+# Functions to convert DCM to quaternion representation
 from .orientation import shepperd
 from .orientation import hughes
 from .orientation import chiaverini
@@ -65,7 +89,7 @@ class DCM(np.ndarray):
     """Direction Cosine Matrix in SO(3)
 
     """
-    def __new__(subtype, array=None, *args, **kwargs):
+    def __new__(subtype, array: np.ndarray = None, *args, **kwargs):
         if array is None:
             array = np.identity(3)
             if 'q' in kwargs:
@@ -95,10 +119,6 @@ def __new__(subtype, array=None, *args, **kwargs):
         obj.A = array
         return obj
 
-    # def __array_finalize__(self, obj):
-    #     if obj is None: return
-    #     self.A = getattr(obj, 'A', None)
-
     @property
     def I(self) -> np.ndarray:
         return self.A.T
@@ -241,7 +261,7 @@ def from_q(self, q: np.ndarray) -> np.ndarray:
         """
         return self.from_quaternion(self, q)
 
-    def to_quaternion(self, method: str = 'shepperd', **kw) -> np.ndarray:
+    def to_quaternion(self, method: str = 'chiaverini', **kw) -> np.ndarray:
         """
         Set quaternion from given Direction Cosine Matrix.
 
@@ -265,9 +285,20 @@ def to_quaternion(self, method: str = 'shepperd', **kw) -> np.ndarray:
             q = sarabandi(self.A, eta=kw.get('threshold', 0.0))
         return q/np.linalg.norm(q)
 
-    def to_q(self, method: str = 'shepperd', **kw) -> np.ndarray:
+    def to_q(self, method: str = 'chiaverini', **kw) -> np.ndarray:
         """Synonym of method `to_quaternion`
         """
         return self.to_quaternion(method=method, **kw)
 
+    def to_angles(self) -> np.ndarray:
+        """Euler Angles from DCM
 
+        Returns
+        -------
+        e : numpy.ndarray
+            Euler angles
+        """
+        phi = np.arctan2(self.A[1, 2], self.A[2, 2])    # Bank Angle
+        theta = -np.sin(self.A[0, 2])                   # Elevation Angle
+        psi = np.arctan2(self.A[0, 1], self.A[0, 0])    # Heading Angle
+        return np.array([phi, theta, psi])