How to calculate roll, pitch and yaw?

General Usage
, ,
19

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.