Direction Cosine Matrix

The difference, in three dimensions, between any given orthogonal frame and a base coordinate frame is the orientation or attitude.

Rotations are linear operations preserving vector lenght and relative vector orientation, and a rotation operator acting on a vector \(\mathbf{v}\in\mathbb{R}^3\) can be defined in the Special Orthogonal group \(SO(3)\), also known as the rotation group.

The rotation operator is a linear transformation represented by a \(3\times 3\) matrix:

\[\begin{split}\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in \mathbb{R}^{3\times 3}\end{split}\]

where

\[\begin{split}\begin{array}{lcr} \mathbf{r}_1 = \begin{bmatrix}r_{11}\\ r_{12} \\ r_{13} \end{bmatrix} \; , & \mathbf{r}_2 = \begin{bmatrix}r_{21}\\ r_{22} \\ r_{32} \end{bmatrix} \; , & \mathbf{r}_3 = \begin{bmatrix}r_{31}\\ r_{23} \\ r_{33} \end{bmatrix} \end{array}\end{split}\]

are unit vectors orthogonal to each other. All matrices satisfying this orthogonality are called orthogonal matrices.

The transformation matrix \(\mathbf{R}\) rotates any vector \(\mathbf{v}\in\mathbb{R}^3\) through the matrix product,

\[\mathbf{v}' = \mathbf{Rv}\]

We observe that \(\mathbf{RR}^{-1}=\mathbf{RR}^T=\mathbf{R}^T\mathbf{R}=\mathbf{I}\), indicating that the inverse of \(\mathbf{R}\) is its transpose. So,

\[\mathbf{v} = \mathbf{R}^T\mathbf{v}'\]

The determinant of a rotation matrix is always equal to \(+1\). This means, its product with any vector will leave the vector’s lenght unchanged.

Matrices conforming to both properties belong to the special orthogonal group \(SO(3)\). Even better, the product of two or more rotation matrices yields another rotation matrix in \(SO(3)\).

Direction cosines are cosines of angles between a vector and a base coordinate frame [WikipediaDCM]. In this case, the difference between orthogonal vectors \(\mathbf{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 [WikipediaRotMat], especially in real applications of spacecraft tracking and location.

References

[WikipediaDCM]Wikipedia: Direction Cosine. (https://en.wikipedia.org/wiki/Direction_cosine)
[WikipediaRotMat]Wikipedia: Rotation Matrix (https://mathworld.wolfram.com/RotationMatrix.html)
[Ma]Yi Ma, Stefano Soatto, Jana Kosecka, and S. Shankar Sastry. An Invitation to 3-D Vision: From Images to Geometric Models. Springer Verlag. 2003. (https://www.eecis.udel.edu/~cer/arv/readings/old_mkss.pdf)
[Huyhn]Huynh, D.Q. Metrics for 3D Rotations: Comparison and Analysis. J Math Imaging Vis 35, 155–164 (2009).
[Curtis]Howard D Curtis. Orbital Mechanics for Engineering Students (Third Edition) Butterworth-Heinemann. 2014.
[Kuipers]Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality. Princeton; Oxford: Princeton University Press, 1999.
[Diebel]Diebel, James. Representing Attitude; Euler Angles, Unit Quaternions, and Rotation. Stanford University. 20 October 2006.
class ahrs.common.dcm.DCM

Direction Cosine Matrix in SO(3)

Class to represent a Direction Cosine Matrix. It is built from a 3-by-3 array, but it can also be built from 3-dimensional vectors representing the roll-pitch-yaw angles, a quaternion, or an axis-angle pair representation.

Parameters:
  • array (array-like, default: None) – Array to build the DCM with.
  • q (array-like, default: None) – Quaternion to convert to DCM.
  • rpy (array-like, default: None) – Array with roll->pitch->yaw angles.
  • euler (tuple, default: None) – Dictionary with a set of angles as a pair of string and array.
  • axang (tuple, default: None) – Tuple with an array and a float of the axis and the angle representation.
Variables:

A (numpy.ndarray) – Array with the 3-by-3 direction cosine matrix.

Examples

All DCM are created as an identity matrix, which means no rotation.

>>> from ahrs import DCM
>>> DCM()
DCM([[1., 0., 0.],
     [0., 1., 0.],
     [0., 0., 1.]])

A rotation around a single axis can be defined by giving the desired axis and its value, in degrees.

>>> DCM(x=10.0)
DCM([[ 1.        ,  0.        ,  0.        ],
     [ 0.        ,  0.98480775, -0.17364818],
     [ 0.        ,  0.17364818,  0.98480775]])
>>> DCM(y=20.0)
DCM([[ 0.93969262,  0.        ,  0.34202014],
     [ 0.        ,  1.        ,  0.        ],
     [-0.34202014,  0.        ,  0.93969262]])
>>> DCM(z=30.0)
DCM([[ 0.8660254, -0.5      ,  0.       ],
     [ 0.5      ,  0.8660254,  0.       ],
     [ 0.       ,  0.       ,  1.       ]])

If we want a rotation conforming the roll-pitch-yaw sequence, we can give the corresponding angles.

>>> DCM(rpy=[30.0, 20.0, 10.0])
DCM([[ 0.81379768, -0.44096961,  0.37852231],
     [ 0.46984631,  0.88256412,  0.01802831],
     [-0.34202014,  0.16317591,  0.92541658]])

Note

Notice the angles are given in reverse order, as it is the way the matrices are multiplied.

>>> DCM(z=30.0) @ DCM(y=20.0) @ DCM(x=10.0)
DCM([[ 0.81379768, -0.44096961,  0.37852231],
     [ 0.46984631,  0.88256412,  0.01802831],
     [-0.34202014,  0.16317591,  0.92541658]])

But also a different sequence can be defined, if given as a tuple with two elements: the order of the axis to rotate about, and the value of the rotation angles (again in reverse order)

>>> DCM(euler=('zyz', [40.0, 50.0, 60.0]))
DCM([[-0.31046846, -0.74782807,  0.58682409],
     [ 0.8700019 ,  0.02520139,  0.49240388],
     [-0.38302222,  0.66341395,  0.64278761]])
>>> DCM(z=40.0) @ DCM(y=50.0) @ DCM(z=60.0)
DCM([[-0.31046846, -0.74782807,  0.58682409],
     [ 0.8700019 ,  0.02520139,  0.49240388],
     [-0.38302222,  0.66341395,  0.64278761]])

Another option is to build the rotation matrix from a quaternion:

>>> DCM(q=[1., 2., 3., 4.])
DCM([[-0.66666667,  0.13333333,  0.73333333],
     [ 0.66666667, -0.33333333,  0.66666667],
     [ 0.33333333,  0.93333333,  0.13333333]])

The quaternions are automatically normalized to make them versors and be used as rotation operators.

Finally, we can also build the rotation matrix from an axis-angle representation:

>>> DCM(axang=([1., 2., 3.], 60.0))
DCM([[-0.81295491,  0.52330834,  0.25544608],
     [ 0.03452394, -0.3945807 ,  0.91821249],
     [ 0.58130234,  0.75528436,  0.30270965]])

The axis of rotation is also normalized to be used as part of the rotation operator.

I

synonym of property inv().

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.I
array([[ 0.92541658,  0.16317591,  0.34202014],
       [-0.31879578,  0.82317294,  0.46984631],
       [-0.20487413, -0.54383814,  0.81379768]])
Returns:Inverse of the DCM.
Return type:np.ndarray
adj

Synonym of property adjugate().

Returns:Adjugate of the DCM.
Return type:np.ndarray

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.adj
array([[ 0.92541658,  0.16317591,  0.34202014],
       [-0.31879578,  0.82317294,  0.46984631],
       [-0.20487413, -0.54383814,  0.81379768]])
adjugate

Return the adjugate of the DCM.

The adjugate, a.k.a. classical adjoint, of a matrix \(\mathbf{A}\) is the transpose of its cofactor matrix. For orthogonal matrices, it simplifies to:

\[\mathrm{adj}(\mathbf{A}) = \mathrm{det}(\mathbf{A})\mathbf{A}^T\]
Returns:Adjugate of the DCM.
Return type:np.ndarray

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.adjugate
array([[ 0.92541658,  0.16317591,  0.34202014],
       [-0.31879578,  0.82317294,  0.46984631],
       [-0.20487413, -0.54383814,  0.81379768]])
det

Synonym of property determinant().

Returns:Determinant of the DCM.
Return type:float

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.det
1.0000000000000002
determinant

Determinant of the DCM.

Given a direction cosine matrix \(\mathbf{R}\), its determinant \(|\mathbf{R}|\) is found as:

\[\begin{split}|\mathbf{R}| = \begin{vmatrix}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{vmatrix}= r_{11}\begin{vmatrix}r_{22} & r_{23}\\r_{32} & r_{33}\end{vmatrix} - r_{12}\begin{vmatrix}r_{21} & r_{23}\\r_{31} & r_{33}\end{vmatrix} + r_{13}\begin{vmatrix}r_{21} & r_{22}\\r_{31} & r_{32}\end{vmatrix}\end{split}\]

where the determinant of \(\mathbf{B}\in\mathbb{R}^{2\times 2}\) is:

\[\begin{split}|\mathbf{B}|=\begin{vmatrix}b_{11}&b_{12}\\b_{21}&b_{22}\end{vmatrix}=b_{11}b_{22}-b_{12}b_{21}\end{split}\]

All matrices in SO(3), to which direction cosine matrices belong, have a determinant equal to \(+1\).

Returns:Determinant of the DCM.
Return type:float

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.determinant
1.0000000000000002
fro

Synonym of property frobenius().

Returns:Frobenius norm of the DCM.
Return type:float

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.fro
1.7320508075688774
frobenius

Frobenius norm of the DCM.

The Frobenius norm of a matrix \(\mathbf{A}\) is defined as:

\[\|\mathbf{A}\|_F = \sqrt{\sum_{i=1}^m\sum_{j=1}^n|a_{ij}|^2}\]
Returns:Frobenius norm of the DCM.
Return type:float

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.frobenius
1.7320508075688774
from_axang(axis: numpy.ndarray, angle: float) → numpy.ndarray

Synonym of method from_axisangle().

Parameters:
  • axis (numpy.ndarray) – Axis of rotation.
  • angle (float) – Angle of rotation, in radians.
Returns:

R – 3-by-3 direction cosine matrix

Return type:

numpy.ndarray

Examples

>>> R = DCM()
>>> R.view()
DCM([[1., 0., 0.],
     [0., 1., 0.],
     [0., 0., 1.]])
>>> R.from_axang([0.81187135, -0.43801381, 0.38601658], 0.6742208510527136)
array([[ 0.92541658, -0.31879578, -0.20487413],
       [ 0.16317591,  0.82317294, -0.54383814],
       [ 0.34202014,  0.46984631,  0.81379768]])
from_axisangle(axis: numpy.ndarray, angle: float) → numpy.ndarray

DCM from axis-angle representation

Use Rodrigue’s formula to obtain the DCM from the axis-angle representation.

\[\mathbf{R} = \mathbf{I}_3 - (\sin\theta)\mathbf{K} + (1-\cos\theta)\mathbf{K}^2\]

where \(\mathbf{R}\) is the DCM, which rotates through an angle \(\theta\) counterclockwise about the axis \(\mathbf{k}\), \(\mathbf{I}_3\) is the \(3\times 3\) identity matrix, and \(\mathbf{K}\) is the skew-symmetric matrix of \(\mathbf{k}\).

Parameters:
  • axis (numpy.ndarray) – Axis of rotation.
  • angle (float) – Angle of rotation, in radians.
Returns:

R – 3-by-3 direction cosine matrix

Return type:

numpy.ndarray

Examples

>>> R = DCM()
>>> R.view()
DCM([[1., 0., 0.],
     [0., 1., 0.],
     [0., 0., 1.]])
>>> R.from_axisangle([0.81187135, -0.43801381, 0.38601658], 0.6742208510527136)
array([[ 0.92541658, -0.31879578, -0.20487413],
       [ 0.16317591,  0.82317294, -0.54383814],
       [ 0.34202014,  0.46984631,  0.81379768]])
from_q(q: numpy.ndarray) → numpy.ndarray

Synonym of method from_quaternion().

Parameters:q (numpy.ndarray) – Quaternion
Returns:R – 3-by-3 direction cosine matrix
Return type:numpy.ndarray

Examples

>>> R = DCM()
>>> R.from_q([0.70710678, 0.0, 0.70710678, 0.0])
array([[-2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00, -2.22044605e-16]])

Non-normalized quaternions will be normalized and transformed too.

>>> R.from_q([1, 0.0, 1, 0.0])
array([[ 2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00,  2.22044605e-16]])

A list (or a Numpy array) with N quaternions will return an N-by-3-by-3 array with the corresponding DCMs.

from_quaternion(q: numpy.ndarray) → numpy.ndarray

DCM from given quaternion

The quaternion \(\mathbf{q}\) has the form \(\mathbf{q} = (q_w, q_x, q_y, q_z)\), where \(\mathbf{q}_v = (q_x, q_y, q_z)\) is the vector part, and \(q_w\) is the scalar part.

The resulting matrix \(\mathbf{R}\) has the form:

\[\begin{split}\mathbf{R}(\mathbf{q}) = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_xq_y - q_wq_z) & 2(q_xq_z + q_wq_y) \\ 2(q_xq_y + q_wq_z) & 1 - 2(q_x^2 + q_z^2) & 2(q_yq_z - q_wq_x) \\ 2(q_xq_z - q_wq_y) & 2(q_wq_x + q_yq_z) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}\end{split}\]

The identity Quaternion \(\mathbf{q} = \begin{pmatrix}1 & 0 & 0 & 0\end{pmatrix}\), produces a \(3 \times 3\) Identity matrix \(\mathbf{I}_3\).

Parameters:q (numpy.ndarray) – Quaternion
Returns:R – 3-by-3 direction cosine matrix
Return type:numpy.ndarray

Examples

>>> R = DCM()
>>> R.from_quaternion([0.70710678, 0.0, 0.70710678, 0.0])
array([[-2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00, -2.22044605e-16]])

Non-normalized quaternions will be normalized and transformed too.

>>> R.from_quaternion([1, 0.0, 1, 0.0])
array([[ 2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00,  2.22044605e-16]])

A list (or a Numpy array) with N quaternions will return an N-by-3-by-3 array with the corresponding DCMs.

inv

Inverse of the DCM.

The direction cosine matrix belongs to the Special Orthogonal group SO(3), where its transpose is equal to its inverse:

\[\mathbf{R}^T\mathbf{R} = \mathbf{R}^{-1}\mathbf{R} = \mathbf{I}_3\]

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.inv
array([[ 0.92541658,  0.16317591,  0.34202014],
       [-0.31879578,  0.82317294,  0.46984631],
       [-0.20487413, -0.54383814,  0.81379768]])
Returns:Inverse of the DCM.
Return type:np.ndarray
log

Logarithm of DCM.

The logarithmic map is defined as the inverse of the exponential map. It corresponds to the logarithm given by the Rodrigues rotation formula:

\[\log(\mathbf{R}) = \frac{\theta(\mathbf{R}-\mathbf{R}^T)}{2\sin\theta}\]

with \(\theta=\arccos\Big(\frac{\mathrm{tr}(\mathbf{R}-1)}{2}\Big)\).

Returns:log – Logarithm of DCM
Return type:numpy.ndarray

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.log
array([[ 0.        , -0.26026043, -0.29531805],
       [ 0.26026043,  0.        , -0.5473806 ],
       [ 0.29531805,  0.5473806 ,  0.        ]])
ode(w: numpy.ndarray) → numpy.ndarray

Ordinary Differential Equation of the DCM.

Parameters:w (numpy.ndarray) – Angular velocity, in rad/s, about X-, Y- and Z-axis.
Returns:dR/dt – Derivative of DCM
Return type:numpy.ndarray
to_angles() → numpy.ndarray

Synonym of method to_rpy().

Returns:a – roll-pitch-yaw angles
Return type:numpy.ndarray
to_axang() → Tuple[numpy.ndarray, float]

Synonym of method to_axisangle().

Returns:
  • axis (numpy.ndarray) – Axis of rotation.
  • angle (float) – Angle of rotation, in radians.

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.to_axang()
(array([ 0.81187135, -0.43801381,  0.38601658]), 0.6742208510527136)
to_axisangle() → Tuple[numpy.ndarray, float]

Return axis-angle representation of the DCM.

Defining a rotation matrix \(\mathbf{R}\):

\[\begin{split}\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}\end{split}\]

The axis-angle representation of \(\mathbf{R}\) is obtained with:

\[\theta = \arccos\Big(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\Big)\]

for the rotation angle, and:

\[\begin{split}\mathbf{k} = \frac{1}{2\sin\theta} \begin{bmatrix}r_{32} - r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12}\end{bmatrix}\end{split}\]

for the rotation vector.

Note

The axis-angle representation is not unique since a rotation of \(−\theta\) about \(−\mathbf{k}\) is the same as a rotation of \(\theta\) about \(\mathbf{k}\).

Returns:
  • axis (numpy.ndarray) – Axis of rotation.
  • angle (float) – Angle of rotation, in radians.

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.to_axisangle()
(array([ 0.81187135, -0.43801381,  0.38601658]), 0.6742208510527136)
to_q(method: str = 'chiaverini', **kw) → numpy.ndarray

Synonym of method to_quaternion().

Parameters:method (str, default: 'chiaverini') – Method to use. Options are: 'chiaverini', 'hughes', 'itzhack', 'sarabandi', and 'shepperd'.

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.to_q()   # Uses method 'chiaverini' by default
array([ 0.94371436,  0.26853582, -0.14487813,  0.12767944])
>>> R.to_q('shepperd')
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_q('hughes')
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_q('itzhack', version=2)
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_q('sarabandi', threshold=0.5)
array([0.94371436, 0.26853582, 0.14487813, 0.12767944])
to_quaternion(method: str = 'chiaverini', **kw) → numpy.ndarray

Quaternion from Direction Cosine Matrix.

There are five methods available to obtain a quaternion from a Direction Cosine Matrix:

  • 'chiaverini' as described in [Chiaverini].
  • 'hughes' as described in [Hughes].
  • 'itzhack' as described in [Bar-Itzhack] using version 3 by default. Possible options are integers 1, 2 or 3.
  • 'sarabandi' as described in [Sarabandi] with a threshold equal to 0.0 by default. Possible threshold values are floats between -3.0 and 3.0.
  • 'shepperd' as described in [Shepperd].
Parameters:method (str, default: 'chiaverini') – Method to use. Options are: 'chiaverini', 'hughes', 'itzhack', 'sarabandi', and 'shepperd'.

Examples

>>> R = DCM(rpy=[10.0, -20.0, 30.0])
>>> R.view()
DCM([[ 0.92541658, -0.31879578, -0.20487413],
     [ 0.16317591,  0.82317294, -0.54383814],
     [ 0.34202014,  0.46984631,  0.81379768]])
>>> R.to_quaternion()   # Uses method 'chiaverini' by default
array([ 0.94371436,  0.26853582, -0.14487813,  0.12767944])
>>> R.to_quaternion('shepperd')
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_quaternion('hughes')
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_quaternion('itzhack', version=2)
array([ 0.94371436, -0.26853582,  0.14487813, -0.12767944])
>>> R.to_quaternion('sarabandi', threshold=0.5)
array([0.94371436, 0.26853582, 0.14487813, 0.12767944])
to_rpy() → numpy.ndarray

Roll-Pitch-Yaw Angles from DCM

A set of Roll-Pitch-Yaw angles may be written according to:

\[\begin{split}\mathbf{a} = \begin{bmatrix}\phi \\ \theta \\ \psi\end{bmatrix} = \begin{bmatrix}\mathrm{arctan2}(r_{23}, r_{33}) \\ -\arcsin(r_{13}) \\ \mathrm{arctan2}(r_{12}, r_{11})\end{bmatrix}\end{split}\]
Returns:a – roll-pitch-yaw angles.
Return type:numpy.ndarray