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!