How to calculate roll, pitch and yaw?

I am trying to calculate the orientation of a wing, expressed as roll, pitch and yaw angle based on the three unit vectors, that form the kite reference frame.

All coordinates are in ENU (East, North, Up) reference frame

The orientation shall be calculated with respect to the NED (North, East, Down) reference frame.

I have the following code:

# unit tests for calculation of the orientation
using LinearAlgebra, Rotations, Test, StaticArrays

# Kite reference frame
# x: from trailing edge to leading edge
# y: to the right looking in flight direction
# z: down

# all coordinates are in ENU (East, North, Up) reference frame
# the orientation is calculated with respect to the NED (North, East, Down) reference frame

"""
    is_right_handed_orthonormal(x, y, z)

Returns `true` if the vectors `x`, `y` and `z` form a right-handed orthonormal basis.
"""
function is_right_handed_orthonormal(x, y, z)
    R = [x y z]
    R*R' β‰ˆ I && det(R) β‰ˆ 1
end

"""
    rot3d(ax, ay, az, bx, by, bz)

Calculate the rotation matrix that needs to be applied on the reference frame (ax, ay, az) to match 
the reference frame (bx, by, bz).
All parameters must be 3-element vectors. Both refrence frames must be orthogonal,
all vectors must already be normalized.

Source: [TRIAD_Algorithm](http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm)
"""
function rot3d(ax, ay, az, bx, by, bz)
    @assert is_right_handed_orthonormal(ax, ay, az)
    @assert is_right_handed_orthonormal(bx, by, bz)    
    R_ai = [ax az ay]
    R_bi = [bx bz by]
    return R_bi * R_ai'
end

function calc_orient_rot(x, y, z)
    # reference frame for the orientation: NED
    ax = @SVector[0, 1, 0] # in ENU reference frame this is pointing to the north
    ay = @SVector[1, 0, 0] # in ENU reference frame this is pointing to the east
    az = @SVector[0, 0,-1] # in ENU reference frame this is pointing down
    rot = rot3d(x, y, z, ax, ay, az)
    return rot
end

quat2euler(q::AbstractVector) = quat2euler(QuatRotation(q))
function quat2euler(q::QuatRotation)
    # Convert quaternion to RotXYZ
    rot = RotXYZ(q)
    
    # Extract roll, pitch, and yaw from RotXYZ
    roll = rot.theta1
    pitch = rot.theta2
    yaw = rot.theta3

    return roll, pitch, yaw
end

@testset "calc_orientation, kite pointing to the north and is at zenith" begin
    # If kite (x axis) is pointing to the north, and is at zenith, then in ENUs reference frame:
    # - x = 0, 1, 0
    # - y = 1, 0, 0
    # - z = 0, 0,-1
    # This would be equal to the NED reference frame.
    x = [0, 1, 0]
    y = [1, 0, 0]
    z = [0, 0,-1]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ 0
    @test pitch β‰ˆ 0
    @test yaw β‰ˆ 0
end
@testset "calc_orientation, kite pointing to the west and is at zenith " begin
    x = [-1, 0, 0]
    y = [ 0, 1, 0]
    z = [ 0, 0,-1]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ 0
    @test pitch β‰ˆ 0
    @test yaw β‰ˆ -90
end
@testset "calc_orientation, kite pointing to the north, right tip up   " begin
    # x = [0, 1, 0] y = [0, 0, 1] z = [1, 0, 0] should give -90 degrees roll
    x = [ 0, 1, 0]
    y = [ 0, 0, 1]
    z = [ 1, 0, 0]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test_broken roll β‰ˆ -90
    @test_broken pitch β‰ˆ 0
    @test yaw β‰ˆ 0
end
@testset "calc_orientation, kite pointing upwards, right tip eastwards " begin
    # If x, y and z are given in ENU
    # x = [0, 0, 1] y = [1, 0, 0] z = [0, 1, 0] should give 90 degrees pitch
    x = [ 0, 0, 1]  # nose pointing up
    y = [ 1, 0, 0]  # right tip pointing east
    z = [ 0, 1, 0]  # z axis pointing to the north
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test_broken roll β‰ˆ 0
    @test_broken pitch β‰ˆ 90
    @test yaw β‰ˆ 0
end
nothing

When I run this code I get:

julia> include("test/test_orientation.jl")
Test Summary:                                                 | Pass  Total  Time
calc_orientation, kite pointing to the north and is at zenith |    3      3  0.0s
Test Summary:                                                 | Pass  Total  Time
calc_orientation, kite pointing to the west and is at zenith  |    3      3  0.0s
Test Summary:                                                 | Pass  Broken  Total  Time
calc_orientation, kite pointing to the north, right tip up    |    1       2      3  0.0s
Test Summary:                                                 | Pass  Broken  Total  Time
calc_orientation, kite pointing upwards, right tip eastwards  |    1       2      3  0.0s

So four of the 12 tests fail.

I already tried:

  • to use RotZYX instead of RotXYZ
  • to swap the reference frames when calling rot3d

But I do not manage to make all tests pass.

Any help welcome!

I have a version now where the tests pass, but I do not understand why.

# unit tests for calculation of the orientation
using LinearAlgebra, Rotations, Test, StaticArrays

# Kite reference frame
# x: from trailing edge to leading edge
# y: to the right looking in flight direction
# z: down

# all coordinates are in ENU (East, North, Up) reference frame
# the orientation is calculated with respect to the NED (North, East, Down) reference frame

"""
    is_right_handed_orthonormal(x, y, z)

Returns `true` if the vectors `x`, `y` and `z` form a right-handed orthonormal basis.
"""
function is_right_handed_orthonormal(x, y, z)
    R = [x y z]
    R*R' β‰ˆ I && det(R) β‰ˆ 1
end

"""
    rot3d(ax, ay, az, bx, by, bz)

Calculate the rotation matrix that needs to be applied on the reference frame (ax, ay, az) to match 
the reference frame (bx, by, bz).
All parameters must be 3-element vectors. Both refrence frames must be orthogonal,
all vectors must already be normalized.

Source: [TRIAD_Algorithm](http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm)
"""
function rot3d(ax, ay, az, bx, by, bz)
    @assert is_right_handed_orthonormal(ax, ay, az)
    @assert is_right_handed_orthonormal(bx, by, bz)    
    R_ai = [ax az ay]
    R_bi = [bx bz by]
    return R_bi * R_ai'
end

function calc_orient_rot(x, y, z)
    # reference frame for the orientation: NED
    ax = @SVector[0, 1, 0] # in ENU reference frame this is pointing to the north
    ay = @SVector[1, 0, 0] # in ENU reference frame this is pointing to the east
    az = @SVector[0, 0,-1] # in ENU reference frame this is pointing down
    rot = rot3d(ax, ay, az, x, y, z)
    return rot
end

quat2euler(q::AbstractVector) = quat2euler(QuatRotation(q))
function quat2euler(q::QuatRotation)
    # Convert quaternion to RotXYZ
    rot = RotXYZ(q)
    
    # Extract roll, pitch, and yaw from RotXYZ
    roll = rot.theta2
    pitch = rot.theta1
    yaw = -rot.theta3

    return roll, pitch, yaw
end

@testset "calc_orientation, kite pointing to the north and is at zenith" begin
    # If kite (x axis) is pointing to the north, and is at zenith, then in ENUs reference frame:
    # - x = 0, 1, 0
    # - y = 1, 0, 0
    # - z = 0, 0,-1
    # This would be equal to the NED reference frame.
    x = [0, 1, 0]
    y = [1, 0, 0]
    z = [0, 0,-1]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ 0
    @test pitch β‰ˆ 0
    @test yaw β‰ˆ 0
end
@testset "calc_orientation, kite pointing to the west and is at zenith " begin
    x = [-1, 0, 0]
    y = [ 0, 1, 0]
    z = [ 0, 0,-1]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ 0
    @test pitch β‰ˆ 0
    @test yaw β‰ˆ -90
end
@testset "calc_orientation, kite pointing to the north, right tip up   " begin
    # x = [0, 1, 0] y = [0, 0, 1] z = [1, 0, 0] should give -90 degrees roll
    x = [ 0, 1, 0]
    y = [ 0, 0, 1]
    z = [ 1, 0, 0]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ -90
    @test pitch β‰ˆ 0
    @test yaw β‰ˆ 0
end
@testset "calc_orientation, kite pointing upwards, right tip eastwards " begin
    # If x, y and z are given in ENU
    # x = [0, 0, 1] y = [1, 0, 0] z = [0, 1, 0] should give 90 degrees pitch
    x = [ 0, 0, 1]  # nose pointing up
    y = [ 1, 0, 0]  # right tip pointing east
    z = [ 0, 1, 0]  # z axis pointing to the north
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test roll β‰ˆ 0
    @test pitch β‰ˆ 90
    @test yaw β‰ˆ 0
end
nothing

I swapped the reference frame when calling rot3d and modified the function quat2euler to look like this:

function quat2euler(q::QuatRotation)
    # Convert quaternion to RotXYZ
    rot = RotXYZ(q)
    
    # Extract roll, pitch, and yaw from RotXYZ
    roll = rot.theta2
    pitch = rot.theta1
    yaw = -rot.theta3

    return roll, pitch, yaw
end

So I changed the sign of yaw and swapped roll and pitch.

Is this correct? If so, why is that needed?

Linking this site that looks quite useful.

1 Like

This test case fails also with the new version:

@testset "calc_orientation, all angles positive                        " begin
    # x, y and z are given in ENU
    x = [ 0.61237244,  0.61237244,  0.5       ]
    y = [ 0.65973961, -0.04736717, -0.75      ]
    z = [-0.43559574,  0.78914913, -0.4330127 ]
    @assert is_right_handed_orthonormal(x, y, z)
    rot = calc_orient_rot(x, y, z)
    q = QuatRotation(rot)
    roll, pitch, yaw = rad2deg.(quat2euler(q))
    @test_broken roll β‰ˆ 60
    @test_broken pitch β‰ˆ 30
    @test_broken yaw β‰ˆ 45
    println("roll: $roll, pitch: $pitch, yaw: $yaw")
end

It would already help if someone could explain how to create a test case (a kite reference frame) for given roll, pitch and yaw angles.

One thing that you must take into consideration is that Rotations.jl considers the rotations as active (you are rotating vectors). Normally, for attitude computation, you want the rotations to be passive (you are rotating reference frames). There is nothing different regarding the matrices and quaternions themselves, but the interpretation is the opposite.

That’s why I created ReferenceFrameRotations.jl. Using this package, if you have a direction cosine matrix, you can obtain the roll, pitch, yaw angles by:

julia> D = DCM([1. 0. 0.; 0. 0. -1; 0. -1 0.]);

julia> dcm_to_angle(D, :ZYX)
EulerAngles{Float64}:
  R(Z) :  0.0    rad  ( 0.0Β°)
  R(Y) : -0.0    rad  (-0.0Β°)
  R(X) : -1.5708 rad  (-90.0Β°)
3 Likes

QuatRotation associates to a unit quaternion q=(cos(\psi), u\sin(\psi)), with u a unit vector, the rotation of axis/direction u, and angle 2\psi. In your code above you considered that QuatRotation associates to the rotation rot a quaternion q, but this is not the case.

This already works better, but I still need to do more testing:

function quat2euler(q::QuatRotation)
    # Convert quaternion to RotXYZ
    rot = RotXYZ(q)
    
    # # Extract roll, pitch, and yaw from RotXYZ
    # roll = rot1.theta2
    # pitch = rot1.theta1
    # yaw = -rot1.theta3
    yaw   = -atan(rot[2, 1], rot[1, 1])
    roll = atan(-rot[3, 1], sqrt(rot[3, 2]^2 + rot[3, 3]^2))
    pitch  = atan(rot[3, 2], rot[3, 3])

    return roll, pitch, yaw
end

You need to check for singularities to improve numerical accuracy.

Because your comments referred to quaternions I thought you wanted to get the 3 angles from the quaternion associated to a rotation.
So I defined the function that determines the roll, pitch, and yaw angles, from a unit quaternion and checked that these angles are the same as the ones calculated from the rotation matrix:

using Rotations, Quaternions, LinearAlgebra

Basis{T} = Tuple{Vector{T}, Vector{T}, Vector{T}} 

function is_right_handed_orthonormal(B::Basis{T}) where T<:Real
    R = hcat(B...)
    R*R' β‰ˆ I && det(R) β‰ˆ 1
end


function rot3d(B1::Basis{T}, B2::Basis{T}) where T<: Real
    @assert is_right_handed_orthonormal(B1)
    @assert is_right_handed_orthonormal(B2)  
    R1 = hcat(B1...)
    R2 = hcat(B2...)
    R = RotMatrix{3, T}(R2 * R1')
end

function  quat2euler(q::Quaternion) 

    roll= atan(2 * (q.s * q.v1 + q.v2 * q.v3), 
               1 - 2 * (q.v1 * q.v1 + q.v2 * q.v2))
    pitch = 2 * atan(sqrt(1 + 2 * (q.s * q.v2 - q.v1 * q.v3)), 
                     sqrt(1 - 2 * (q.s * q.v2 - q.v1 * q.v3))) - pi/2

    yaw = atan(2 * (q.s * q.v3 + q.v1 * q.v2), 
               1 - 2 * (q.v2 * q.v2 + q.v3 * q.v3))

    (roll, pitch, yaw)
end

function rot2euler(R::RotMatrix{3, T}) where T<:Real
    roll = atan(R[3,2], R[3,3])
    pitch = atan(-R[3,1], sqrt(R[3,2]^2+R[3,3]^2))
    yaw = atan(R[2,1], R[1,1])
    (roll, pitch, yaw)
end    
#Example:
B1 = ([1/3, βˆ’2/3, 2/3], [2/3, 2/3, 1/3], [-2/3, 1/3, 2/3])
B2 = ([1, 2, 4]/sqrt(21),  [2, -3, 1]/sqrt(14), [2,1,-1]/sqrt(6))
R = rot3d(B1, B2)
q = [Base.getproperty(QuatRotation(R), s) for s in (:w, :x, :y,:z)]
r, p, y = quat2euler(quat(q...))
rr, pp, yy = rot2euler(R)

The last two lines give the same angles.

1 Like

I am trying your approach, but I am not yet there. Code:

# generate test cases for the calculation of roll, pitch and yaw
using LinearAlgebra, Rotations
import ReferenceFrameRotations as RFR

# z-yβ€²-xβ€³ (intrinsic rotations) or x-y-z (extrinsic rotations): 
# the intrinsic rotations are known as: yaw, pitch and roll

# x: from trailing edge to leading edge
# y: to the right looking in flight direction
# z: down

# If kite (x axis) is pointing to the north, and is at zenith, then in ENUs reference frame:
# - x = 0, 1, 0
# - y = 1, 0, 0
# - z = 0, 0,-1
# This would be equal to the NED reference frame.
x = [0, 1, 0]
y = [1, 0, 0]
z = [0, 0,-1]

# R = Yaw * Pitch * Roll

yaw = deg2rad(20)
Yaw = AngleAxis(yaw, z[1], z[2], z[3])

pitch = deg2rad(30)
Pitch = AngleAxis(pitch, y[1], y[2], y[3])

roll = deg2rad(40)
Roll = AngleAxis(roll, x[1], x[2], x[3])

D= RFR.DCM(Yaw)
euler = RFR.dcm_to_angle(D, :ZYX)
yaw = euler.a1
println("yaw: ", rad2deg(yaw))

D= RFR.DCM(Pitch)
euler = RFR.dcm_to_angle(D, :ZYX)
pitch = -euler.a3
println("pitch: ", rad2deg(pitch))

D= RFR.DCM(Roll)
euler = RFR.dcm_to_angle(D, :ZYX)
roll = -euler.a2
println("roll: ", rad2deg(roll))

Output:

julia> include("mwes/mwe_21.jl")
yaw: 20.0
pitch: 29.999999999999993
roll: 39.99999999999999

What I do:

  • create the NED reference frame
  • create a rotation by yaw, pitch and roll
  • convert these rotations back to euler angles

While this very simple example works, the signs of pitch and roll are wrong. Why?

And why does the pitch angle, which is a rotation around the y axis appears as euler.a3 and not as euler.a2 ?

The pitch rotation you created is a 30 deg rotation about the X axis. However, a pitch rotation is, by definition, a rotation about the Y axis. I think you are not understanding the concept of coordinate frame transformations and Euler angle representation.

Euler angles are only one way to represent a transformation between two reference frames in a more geometric visualization, with the caveat that we might encounter some singularities.

Hence, first you need to obtain the DCM (or quaternion) that rotates one reference frame into alignment with the other. After that, the Euler angle representation (yaw, pitch, and roll) of such a rotation is obtained by dcm_to_angle(D, :ZYX).

I think this is a bit of a strong statement. I am electrical engineer and therefore flight dynamics is not so easy for me, but I work since 2010 at the faculty at aerospace engineering (with some breaks).

But I define the pitch as rotation around the Y axis of the Kite reference frame.

pitch = deg2rad(30)
Pitch = AngleAxis(pitch, y[1], y[2], y[3])

What is wrong about that?

I want to create a test case for this function, so I need to construct the body fixed frame by rotating the reference frame based on given roll, pitch and yaw angles in the first place.

How would you create such a test case (the NED reference frame (three vectors) and roll, pitch and yaw angles are given and you want to create a DCM based on this information)?

I think what makes this case problematic is the fact that we are using two different reference frames, ENU for the coordinates and define the orientation with respect to the NED reference frame. This is a definition typical for airborne wind energy systems and rarely used otherwise.

You defined y = [1, 0, 0], then you asked for a rotation of 30 deg about this vector that is aligned with the X-axis.

Let’s say you want to represent the body attitude with respect to NED using the following angles:

  • Roll: 15 deg.
  • Pitch: 30 deg.
  • Yaw: 45 deg.

Then, angle_to_dcm(deg2rad(45), deg2rad(30), deg2rad(15), :ZYX) will provide that DCM that rotates NED to the body reference frame using the yaw-pitch-roll rotation sequence.

of the earth-fixed reference frame. But pitching of an airplane is defined around the Y-axis of the body fixed reference frame, which - in this case - happens to be the x-axis of the earth-fixed reference frame.

See: Aircraft flight dynamics - Wikipedia

That’s why I said you are misunderstanding some concepts. Yaw-pitch-roll assumes you perform three rotations: one about the Z axis, another about the new Y axis, and the other about the new X axis. The angles are selected to align the two desired references.

What you did was to rotate the pitch angle about the representation of the local frame Y-axis in the body reference frame.

OK, this seams to work:

# generate test cases for the calculation of roll, pitch and yaw
using LinearAlgebra
import ReferenceFrameRotations as RFR

# z-yβ€²-xβ€³ (intrinsic rotations) or x-y-z (extrinsic rotations): 
# the intrinsic rotations are known as: yaw, pitch and roll

# x: from trailing edge to leading edge
# y: to the right looking in flight direction
# z: down

# If kite (x axis) is pointing to the north, and is at zenith, then in ENUs reference frame:
# - x = 0, 1, 0
# - y = 1, 0, 0
# - z = 0, 0,-1
# This would be equal to the NED reference frame.
x = [0, 1, 0]
y = [1, 0, 0]
z = [0, 0,-1]

yaw = deg2rad(20)
pitch = deg2rad(30)
roll = deg2rad(40)

D1 = RFR.angle_to_dcm(yaw, pitch, roll, :ZYX)
x4 = D1 * x
y4 = D1 * y
z4 = D1 * z
println("Yaw: ", rad2deg(yaw), ", Pitch: ", rad2deg(pitch), ", Roll: ", rad2deg(roll), 
        "\nx = ", x4, "\ny = ", y4, "\nz = ", z4)

euler = RFR.dcm_to_angle(D1, :ZYX)
yaw = euler.a1
pitch = euler.a2
roll = euler.a3
println("Yaw: ", rad2deg(yaw), ", Pitch: ", rad2deg(pitch), ", Roll: ", rad2deg(roll))

Output:

julia> include("mwes/mwe_22.jl")
Yaw: 20.0, Pitch: 29.999999999999996, Roll: 40.0
x = [0.29619813272602386, 0.8297694655894312, -0.47302145844036114]
y = [0.8137976813493738, 0.040008756548141844, 0.5797694655894312]
z = [0.49999999999999994, -0.5566703992264194, -0.6634139481689384]
Yaw: 20.0, Pitch: 29.999999999999996, Roll: 40.00000000000001

I use now:

function quat2euler(q::QuatRotation)  
    D = RFR.DCM(q)
    euler = RFR.dcm_to_angle(D, :ZYX)
    yaw = euler.a1
    pitch = euler.a2
    roll = euler.a3
    return roll, pitch, yaw
end

I will check on Monday if my colleagues agree with this approach.

By the way, is it possible to check if we have a gimbal lock?

Just as a follow up:

The main problem was not yet mentioned in this discussion yet. The main problem was that the vectors of the kite reference frame are defined in in the ENU reference frame, but I need the orientation in the NED reference frame. The solution is simple: Convert the vectors of the kite reference frame to NED first. This results in the following code:

"""
    enu2ned(vec::AbstractVector)

Convert a vector from ENU (east, north, up) to NED (north, east, down) reference frame.
"""
function enu2ned(vec::AbstractVector)  
    R = @SMatrix[0 1 0; 1 0 0; 0 0 -1]
    R*vec
end
"""
    calc_orient_quat(x, y, z)

Convert the the kite reference frame, defined by the three ortho-normal vectors x, y and z in ENU reference frame 
to the quaternion defining the orientation in NED reference frame.
"""
function calc_orient_quat(x, y, z)
    x = enu2ned(x)
    y = enu2ned(y)
    z = enu2ned(z)
    # reference frame for the orientation: NED (north, east, down)
    ax = @SVector [1, 0, 0]
    ay = @SVector [0, 1, 0]
    az = @SVector [0, 0, 1]
    rotation = rot3d(ax, ay, az, x, y, z)
    q = QuatRotation(rotation)
    return Rotations.params(q)
end

For converting the quaternion to Euler angles I use now this function:

"""
    quat2euler(q::QuatRotation)
    quat2euler(q::AbstractVector)

Convert a quaternion to roll, pitch, and yaw angles in radian.
The quaternion can be a 4-element vector (w, i, j, k) or a QuatRotation object.
"""
quat2euler(q::AbstractVector) = quat2euler(QuatRotation(q))
function quat2euler(q::QuatRotation)  
    D = RFR.DCM(q)
    pitch = asin(βˆ’D[3,1])
    roll  = atan(D[3,2], D[3,3])
    yaw   = atan(D[2,1], D[1,1])
    return roll, pitch, yaw
end

All works fine now, and Daan van Wolffelaar contributed 40 unit tests and we used geogebra to verify the results.

@Ronis_BR For unknown reasons the function dcm_to_angle did not work for me, I use now the following functions:

"""
    quat2euler(q::QuatRotation)
    quat2euler(q::AbstractVector)

Convert a quaternion to roll, pitch, and yaw angles in radian.
The quaternion can be a 4-element vector (w, i, j, k) or a QuatRotation object.
"""
quat2euler(q::AbstractVector) = quat2euler(QuatRotation(q))
function quat2euler(q::QuatRotation)  
    D = RFR.DCM(q)
    pitch = asin(βˆ’D[3,1])
    roll  = atan(D[3,2], D[3,3])
    yaw   = atan(D[2,1], D[1,1])
    return roll, pitch, yaw
end

and

"""
    euler2rot(roll, pitch, yaw)

Calculate the rotation matrix based on the roll, pitch, and yaw angles in radian.
"""
function euler2rot(roll, pitch, yaw)
    Ο†      = roll
    R_x = [1    0       0;
              0  cos(Ο†) -sin(Ο†);
              0  sin(Ο†)  cos(Ο†)]
    ΞΈ      = pitch          
    R_y = [ cos(ΞΈ)  0  sin(ΞΈ);
                 0     1     0;
              -sin(ΞΈ)  0  cos(ΞΈ)]
    ψ      = yaw
    R_z = [cos(ψ) -sin(ψ) 0;
              sin(ψ)  cos(ψ) 0;
                 0       0   1]
    R   = R_z * R_y * R_x
    return R
end