from_rpy

Quaternion.from_rpy(angles: ndarray) → ndarray

Quaternion from given RPY angles.

The quaternion can be constructed from the Aerospace cardanian angle sequence that follows the order \(\phi\to\theta\to\psi\), where \(\phi\) is the roll (or bank) angle, \(\theta\) is the pitch (or elevation) angle, and \(\psi\) is the yaw (or heading) angle.

The composing quaternions are:

\[\begin{split}\begin{array}{rcl} \mathbf{q}_X &=& \begin{pmatrix}\cos\frac{\phi}{2} & \sin\frac{\phi}{2} & 0 & 0\end{pmatrix} \\ && \\ \mathbf{q}_Y &=& \begin{pmatrix}\cos\frac{\theta}{2} & 0 & \sin\frac{\theta}{2} & 0\end{pmatrix} \\ && \\ \mathbf{q}_Z &=& \begin{pmatrix}\cos\frac{\psi}{2} & 0 & 0 & \sin\frac{\psi}{2}\end{pmatrix} \end{array}\end{split}\]

The elements of the final quaternion \(\mathbf{q}=\mathbf{q}_Z\mathbf{q}_Y\mathbf{q}_X = q_w+q_xi+q_yj+q_zk\) are obtained as:

\[\begin{split}\begin{array}{rcl} q_w &=& \cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} + \sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\ && \\ q_x &=& \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} - \sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} \\ && \\ q_y &=& \cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} + \sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} \\ && \\ q_z &=& \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} - \cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \end{array}\end{split}\]

Warning

The Aerospace sequence \(\phi\to\theta\to\psi\) is only one of the twelve possible rotation sequences around the main axes. Other sequences might be more suitable for other applications, but this one is the most common in practice.

Parameters:

angles (numpy.ndarray) – 3 cardanian angles, in radians, following the order: roll -> pitch -> yaw.

Returns:

q – Quaternion from roll-pitch-yaw angles.

Return type:

numpy.ndarray

Examples

>>> from ahrs import DEG2RAD    # Helper variable to convert angles to radians
>>> q = Quaternion()
>>> q.from_rpy(np.array([10.0, 20.0, 30.0])*DEG2RAD)    # Give roll-pitch-yaw angles as radians.
array([0.95154852, 0.23929834, 0.18930786, 0.03813458])

It can be corroborated with the class DCM, which represents a Direction Cosine Matrix, and can also be built with roll-pitch-yaw angles.

>>> from ahrs import DCM
>>> R = DCM(rpy=[10.0, 20.0, 30.0])     # Here you give the angles as degrees
>>> R
DCM([[ 0.92541658,  0.01802831,  0.37852231],
     [ 0.16317591,  0.88256412, -0.44096961],
     [-0.34202014,  0.46984631,  0.81379768]])
>>> q.from_DCM(R)
array([0.95154852, 0.23929834, 0.18930786, 0.03813458])

With both approaches the same quaternion is obtained.