Update Quaternion and ReadMe

- Add Quaternion class. - Add easy versioning. - Add tests in __main__ guards of submodules. - Remove R2q from orientation.py to advocate for methods of class Quaternion. - Update readme with quaternion and DCM examples.
Mayitzin · Dec 27, 2019 · 5815fcf · 5815fcf
1 parent 826a36a
commit 5815fcf
Show file tree

Hide file tree

Showing 6 changed files with 352 additions and 122 deletions.
diff --git a/README.md b/README.md
@@ -17,51 +17,69 @@ AHRS is compatible with __Python 3.6__ and above.
 
 AHRS may be installed using [pip](https://pip.pypa.io):
 
-```sh
+```
 pip install ahrs
 ```
 
 Or directly from the repository:
 
-```sh
+```
 git clone https://github.com/Mayitzin/ahrs.git
 cd ahrs
 python setup.py install
 ```
 
-AHRS depends on the most distributed packages of Python. If you don't have them, they will be automatically downloaded and installed.
+AHRS depends on the most distributed packages of scientifc Python environments ([NumPy](https://numpy.org/), [SciPy](https://www.scipy.org/) and [matplotlib](https://matplotlib.org/)). If you don't have them, they will be automatically downloaded and installed.
 
 ## Using AHRS
 
 To play with orientations, for example, we can use the `orientation` module.
 
 ```py
->>> import ahrs
->>> # Rotation matrix of 30.0 degrees around X-axis
-... ahrs.common.orientation.rotation('x', 30.0)
-array([[ 1.       ,  0.       ,  0.       ],
-       [ 0.       ,  0.8660254, -0.5      ],
-       [ 0.       ,  0.5      ,  0.8660254]])
->>> # Rotation sequence of the form: R_y(10.0)@R_x(20.0)@R_y(30.0)
-... ahrs.common.orientation.rot_seq('yXy', [10.0, 20.0, 30.0])
-array([[ 0.77128058,  0.05939117,  0.63371836],
-       [ 0.17101007,  0.93969262, -0.29619813],
-       [-0.61309202,  0.33682409,  0.71461018]])
+>>> from ahrs.common import orientation
+>>> # Rotation product: R_y(10.0) @ R_x(20.0) @ R_y(30.0)
+... Rx = orientation.rotation('x', 10.0)
+>>> Ry = orientation.rotation('y', 20.0)
+>>> Rz = orientation.rotation('z', 30.0)
+>>> Rx@Ry@Rz
+array([[ 0.81379768 -0.46984631  0.34202014]
+       [ 0.54383814  0.82317294 -0.16317591]
+       [-0.20487413  0.31879578  0.92541658]])
+>>> # Same rotation sequence but with single call to rot_seq()
+... orientation.rot_seq('xyz', [10.0, 20.0, 30.0])
+array([[ 0.81379768 -0.46984631  0.34202014]
+       [ 0.54383814  0.82317294 -0.16317591]
+       [-0.20487413  0.31879578  0.92541658]])
 ```
 
-It also works nicely with Quaternions.
+It now includes the class `Quaternion` to easily handle the orientation estimation with quaternions.
 
 ```py
->>> import numpy as np
->>> q = np.random.random(4)
->>> # It automatically normalizes any given vector
-... ahrs.common.orientation.q2R(q)
-array([[ 0.76811067,  0.3546719 ,  0.53311709],
-       [ 0.55044928,  0.05960693, -0.83273802],
-       [-0.32712625,  0.93308888, -0.14944417]])
+>>> from ahrs import Quaternion
+>>> q1 = Quaternion()
+>>> str(q1)          # Empty quaternions default to identity quaternion
+'(1.0000 +0.0000i +0.0000j +0.0000k)'
+>>> q2 = Quaternion([1.0, 2.0, 3.0])
+>>> str(q2)          # 3-element vectors build pure quaternions
+'(0.0000 +0.2673i +0.5345j +0.8018k)'
+>>> q3 = Quaternion([1., 2., 3., 4.])
+>>> str(q3)          # All quaternions are normalized
+'(0.1826 +0.3651i +0.5477j +0.7303k)'
+>>> str(q2+q3)       # Use normal arithmetic operators to perform operations on quaternions
+'(0.0918 +0.3181i +0.5444j +0.7707k)'
+>>> q2.product(q3)   # Quaternion products are supported
+array([-0.97590007,  0.        ,  0.19518001,  0.09759001])
+>>> str(q2*q3)
+'(-0.9759 +0.0000i +0.1952j +0.0976k)'
+>>> q2.to_DCM()      # Conversions between representations are also implemented
+array([[-0.85714286,  0.28571429,  0.42857143],
+       [ 0.28571429, -0.42857143,  0.85714286],
+       [ 0.42857143,  0.85714286,  0.28571429]])
 ```
 
-`ahrs` also includes a module that simplifies data loading and visualization using `matplotlib`.
+And many other quaternion operations, properties and methods are also available.
+
+`ahrs` includes a sub-module that simplifies data loading and visualization using `matplotlib` as plot engine.
 
 ```py
 >>> data = ahrs.utils.io.load("ExampleData.mat")

diff --git a/ahrs/__init__.py b/ahrs/__init__.py
@@ -11,6 +11,7 @@
 from . import common
 from . import filters
 from . import utils
+from .common.quaternion import Quaternion
 from .versioning import get_version
 
 __version__ = get_version()
diff --git a/ahrs/common/orientation.py b/ahrs/common/orientation.py
@@ -7,11 +7,7 @@
 """
 
 import numpy as np
-from .mathfuncs import *
-
-__all__ = ['q_conj', 'q_random', 'q_norm', 'q_prod', 'q_mult_L', 'q_mult_R',
-'q_rot', 'axang2quat', 'quat2axang', 'q_correct', 'q2R', 'q2euler', 'rotation', 'rot_seq',
-'R2q', 'dcm2quat', 'cardan2q', 'q2cardan', 'am2q', 'acc2q', 'slerp']
+from ahrs.common.mathfuncs import cosd, sind, DEG2RAD, RAD2DEG
 
 def q_conj(q):
     """
@@ -654,67 +650,6 @@ def rot_seq(axes=None, angles=None):
             R = rotation(axes[i], angles[i])@R
     return R
 
-def R2q(R=None, eta=0.0):
-    """
-    Compute a Quaternion from a rotation matrix
-
-    Use Shepperd's voting scheme to compute the corresponding Quaternion q from
-    a given rotation matrix R. Optimized by Sarabandi et al.
-
-    References
-    ----------
-    .. [1] Sarabandi, S. et al. (2018) Accurate Computation of Quaternions
-           from Rotation Matrices.
-           (http://www.iri.upc.edu/files/scidoc/2068-Accurate-Computation-of-Quaternions-from-Rotation-Matrices.pdf)
-
-    """
-    if R is None:
-        R = np.identity(3)
-    # Get elements of R
-    r11, r12, r13 = R[0][0], R[0][1], R[0][2]
-    r21, r22, r23 = R[1][0], R[1][1], R[1][2]
-    r31, r32, r33 = R[2][0], R[2][1], R[2][2]
-    # Compute qw
-    d_w = r11+r22+r33
-    if d_w > eta:
-        q_w = 0.5*np.sqrt(1.0+d_w)
-    else:
-        nom = (r32-r23)**2+(r13-r31)**2+(r21-r12)**2
-        q_w = 0.5*np.sqrt(nom/(3.0-d_w))
-    # Compute qx
-    d_x = r11-r22-r33
-    if d_x > eta:
-        q_x = 0.5*np.sqrt(1.0+d_x)
-    else:
-        nom = (r32-r23)**2+(r12+r21)**2+(r31+r13)**2
-        q_x = 0.5*np.sqrt(nom/(3.0-d_x))
-    # Compute qy
-    d_y = -r11+r22-r33
-    if d_y > eta:
-        q_y = 0.5*np.sqrt(1.0+d_y)
-    else:
-        nom = (r13-r31)**2+(r12+r21)**2+(r23+r32)**2
-        q_y = 0.5*np.sqrt(nom/(3.0-d_y))
-    # Compute qz
-    d_z = -r11-r22+r33
-    if d_z > eta:
-        q_z = 0.5*np.sqrt(1.0+d_z)
-    else:
-        nom = (r21-r12)**2+(r31+r13)**2+(r23+r32)**2
-        q_z = 0.5*np.sqrt(nom/(3.0-d_z))
-    # Assign signs
-    if q_w >= 0.0:
-        q_x *= np.sign(r32-r23)
-        q_y *= np.sign(r13-r31)
-        q_z *= np.sign(r21-r12)
-    else:
-        q_w *= -1.0
-        q_x *= -np.sign(r32-r23)
-        q_y *= -np.sign(r13-r31)
-        q_z *= -np.sign(r21-r12)
-    # Return values of quaternion
-    return np.asarray([q_w, q_x, q_y, q_z])
-
 def dcm2quat(R):
     """
     Return a unit quaternion from a given Direct Cosine Matrix.
@@ -1155,3 +1090,13 @@ def slerp(q0, q1, t_array, **kwgars):
     s0 = np.cos(theta) - qdot*sin_theta/sin_theta_0
     s1 = sin_theta/sin_theta_0
     return s0[:,np.newaxis]*v0[np.newaxis,:] + s1[:,np.newaxis]*v1[np.newaxis,:]
+
+if __name__ == '__main__':
+    Rx = rotation('x', 10.0)
+    Ry = rotation('y', 20.0)
+    Rz = rotation('z', 30.0)
+    Rzyx = Rx@Ry@Rz
+    print(Rzyx)
+    R = rot_seq('xyz', [10.0, 20.0, 30.0])
+    print(R)
+    # print(np.isclose(Rzyx, R))