Hughes’ Method

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

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

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}\]

Provided \(\eta\neq 0\). If \(\eta =0\), then \(\boldsymbol{\epsilon}\) is simply \(\mathbf{a}\).

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