Hughes’ Method

ahrs.common.orientation.hughes(C: ndarray) → ndarray

Quaternion from a Direction Cosine Matrix with trigonometric Hughes’ method [Hug86].

Defining the quaternion (reluctantly called “Euler Parameters” in Hughes’ book) as:

\[\mathbf{q} = \begin{bmatrix} \eta & \boldsymbol{\epsilon} \end{bmatrix}\]

where

\[\begin{split}\begin{array}{rcl} \eta &=& \cos \big(\frac{\phi}{2}\big) \\ \boldsymbol{\epsilon} &=& \sin \big(\frac{\phi}{2}\big) \mathbf{a} \end{array}\end{split}\]

The quaternion is subject to the constraint \(\boldsymbol{\epsilon}^T\boldsymbol{\epsilon}+\eta^2=1\), and \(\phi\) is the rotation about the unitary axis \(\mathbf{a}\).

The rotation matrix associated to the quaternion is:

\[\mathbf{C} = (\eta ^2 - \boldsymbol{\epsilon}^T\boldsymbol{\epsilon})\mathbf{I} + 2\boldsymbol{\epsilon}\boldsymbol{\epsilon}^T - 2 \eta\boldsymbol{\epsilon}^\times\]

where \(\mathbf{I}\) is the \(3\times 3\) identity matrix, and \(\boldsymbol{\epsilon}^\times\) is the skew-symmetric matrix of \(\boldsymbol{\epsilon}\).

Given \(\mathbf{C}\), the quaternion components are obtained as:

\[\begin{split}\begin{array}{rcl} \eta &=& \pm\frac{1}{2} \big(1 + c_{11} + c_{22} + c_{33}\big)^{1/2} \\ \boldsymbol{\epsilon} &=& \frac{1}{4\eta} \begin{bmatrix} c_{23} - c_{32} \\ c_{31} - c_{13} \\ c_{12} - c_{21} \end{bmatrix} \end{array}\end{split}\]

The plus sign is chosen if it is advantageous to have a unique \(\eta\); corresponding to \(\phi \in [0, \pi]\). To ensure uniqueness, the vector part is multiplied by -1 if \(\eta > 0\).

If \(\eta = 0\), then it is a pure quaternion, and its vector part is:

\[\begin{split}\boldsymbol{\epsilon} = \begin{bmatrix} \sqrt{\frac{1+c_{11}}{2}} \\ \sqrt{\frac{1+c_{22}}{2}} \\ \sqrt{\frac{1+c_{33}}{2}} \end{bmatrix}\end{split}\]

Finally, we normalize the quaternion to have unitary norm.

\[\mathbf{q} = \frac{\mathbf{q}}{\|\mathbf{q}\|}\]

Parameters:

C (numpy.ndarray) – 3-by-3 or N-by-3-by-3 Direction Cosine Matrix (or Matrices).

Returns:

q – 4-dimensional or N-by-4 Quaternion array.

Return type:

numpy.ndarray