Chiaverini’s Method

ahrs.common.orientation.chiaverini(dcm: ndarray) → ndarray

Quaternion from a Direction Cosine Matrix with Chiaverini’s algebraic method [Chiaverini].

Defining the unit quaternion as:

\[\mathbf{q} = \begin{bmatrix} q_w & \mathbf{q}_v \end{bmatrix}\]

where

\[\begin{split}\begin{array}{rcl} q_w &=& \cos \big(\frac{\theta}{2}\big) \\ \mathbf{q}_v &=& \sin \big(\frac{\theta}{2}\big) \mathbf{r} \end{array}\end{split}\]

with \(q_w \geq 0\) for \(\theta \in [-\pi, \pi]\); \(q_w\) is the scalar part, while \(\mathbf{q}_v\) is the vector part, and \(\theta\) is the rotation about the axis \(\mathbf{r}\).

We know the rotation matrix of a given unit quaternion is:

\[\mathbf{R}(\mathbf{q}) = (q_w^2 - \mathbf{q}_v^T\mathbf{q}_v)\mathbf{I}_3 + 2\mathbf{q}_v\mathbf{q}_v^T + 2 q_w\lfloor\mathbf{q}_v\rfloor_\times\]

where \(\mathbf{I}_3\) is the \(3\times 3\) identity matrix, and \(\lfloor\mathbf{q}_v\rfloor_\times\) is the skew-symmetric matrix of the vector part. Solving the equation above for the scalar and vector parts we get the unitary quaternion:

\[\begin{split}\begin{array}{rcl} q_w &=& \frac{1}{2} \sqrt{R_{00} + R_{11} + R_{22} + 1} \\ \mathbf{q}_v &=& \begin{bmatrix} \frac{1}{2} \mathrm{sgn}(R_{21} - R_{12}) \sqrt{R_{00} - R_{11} - R_{22} + 1} \\ \frac{1}{2} \mathrm{sgn}(R_{02} - R_{20}) \sqrt{R_{11} - R_{22} - R_{00} + 1} \\ \frac{1}{2} \mathrm{sgn}(R_{10} - R_{01}) \sqrt{R_{22} - R_{00} - R_{11} + 1} \end{bmatrix} \end{array}\end{split}\]

where \(\mathrm{sgn}\) is the sign function:

\[\begin{split}\mathrm{sgn}(x) = \left\{ \begin{array}{ll} \mathrm{-1} & \: x < 0 \\ \mathrm{1} & \: \mathrm{otherwise} \end{array} \right.\end{split}\]

Parameters:

dcm (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