Problem with quaternion rotation maths

Hi, I am trying to work out the maths to keep a camera connected to a balloon pointing in a fixed direction. The camera can roll, pitch and yaw relative to the balloon which itself can roll, pitch and yaw relative to an inertial frame. Sensors provide the camera roll, pitch and yaw relative to the balloon and the balloon roll, pitch and yaw relative to the reference frame.

Here is the matlab code I'm using to try and find the desired camera roll, pitch and yaw angles of rotation:

clear all;
clc;

% Setup target
initial_camera_relative_to_balloon_yaw      = 0;
initial_camera_relative_to_balloon_pitch    = 0;
initial_camera_relative_to_balloon_roll     = 0;

initial_balloon_relative_to_reference_yaw        = -90;
initial_balloon_relative_to_reference_pitch      = 0;
initial_balloon_relative_to_reference_roll       = 0;

initial_camera_relative_to_balloon_quat     = angle2quat(deg2rad(initial_camera_relative_to_balloon_yaw), deg2rad(initial_camera_relative_to_balloon_pitch), deg2rad(initial_camera_relative_to_balloon_roll));
initial_balloon_relative_to_reference_quat  = angle2quat(deg2rad(initial_balloon_relative_to_reference_yaw), deg2rad(initial_balloon_relative_to_reference_pitch), deg2rad(initial_balloon_relative_to_reference_roll));
target_relative_to_reference_quat           = quatmultiply(initial_camera_relative_to_balloon_quat, initial_balloon_relative_to_reference_quat)

% Normal running
balloon_relative_to_reference_yaw    = -90;
balloon_relative_to_reference_pitch  = 45;
balloon_relative_to_reference_roll   = 0;

balloon_relative_to_reference_quat   = angle2quat(deg2rad(balloon_relative_to_reference_yaw), deg2rad(balloon_relative_to_reference_pitch), deg2rad(balloon_relative_to_reference_roll));
reference_relative_to_balloon_quat   = quatinv(balloon_relative_to_reference_quat);
camera_relative_to_balloon_quat      = quatmultiply(target_relative_to_reference_quat, reference_relative_to_balloon_quat);

[c_yaw, c_pitch, c_roll] = quat2angle(camera_relative_to_balloon_quat);
camera_yaw      = rad2deg(c_yaw);
camera_pitch    = rad2deg(c_pitch);
camera_roll     = rad2deg(c_roll);
% If the maths was right this should be [0 -45 0] not [0 0 -45]
[camera_yaw, camera_pitch, camera_roll]

There is clearly a problem with the maths but I can't see what it is. Can someone help with this?

Answer 1

[Paul]

Apparently the quatmultiply() function operates the other way around from that which I had thought. The solution was to change the following lines as shown:

target_relative_to_reference_quat =
  quatmultiply(initial_balloon_relative_to_reference_quat,
               initial_camera_relative_to_balloon_quat)

...

camera_relative_to_balloon_quat =
  quatmultiply(reference_relative_to_balloon_quat,
               target_relative_to_reference_quat);

Answer 2

[Andrei Bobrov]

try

[c_yaw, c_pitch, c_roll] = quat2angle(camera_relative_to_balloon_quat,'ZXY');

Answer 3

[Paul]

Hi Andrei,

That fixes the answer in the specific case that I posted but it is not a general solution. See below for a different example that doesn't work with your proposed change:

clear all;
clc;

% Setup target
initial_camera_relative_to_balloon_yaw      = 0;
initial_camera_relative_to_balloon_pitch    = 0;
initial_camera_relative_to_balloon_roll     = 0;

initial_balloon_relative_to_reference_yaw        = 0;
initial_balloon_relative_to_reference_pitch      = 0;
initial_balloon_relative_to_reference_roll       = 0;

initial_camera_relative_to_balloon_quat     = angle2quat(deg2rad(initial_camera_relative_to_balloon_yaw), deg2rad(initial_camera_relative_to_balloon_pitch), deg2rad(initial_camera_relative_to_balloon_roll));
initial_balloon_relative_to_reference_quat  = angle2quat(deg2rad(initial_balloon_relative_to_reference_yaw), deg2rad(initial_balloon_relative_to_reference_pitch), deg2rad(initial_balloon_relative_to_reference_roll));
target_relative_to_reference_quat           = quatmultiply(initial_camera_relative_to_balloon_quat, initial_balloon_relative_to_reference_quat)

% Normal running
balloon_relative_to_reference_yaw    = 0;
balloon_relative_to_reference_pitch  = 45;
balloon_relative_to_reference_roll   = 0;

balloon_relative_to_reference_quat   = angle2quat(deg2rad(balloon_relative_to_reference_yaw), deg2rad(balloon_relative_to_reference_pitch), deg2rad(balloon_relative_to_reference_roll));
reference_relative_to_balloon_quat   = quatinv(balloon_relative_to_reference_quat);
camera_relative_to_balloon_quat      = quatmultiply(target_relative_to_reference_quat, reference_relative_to_balloon_quat);

[c_yaw, c_pitch, c_roll] = quat2angle(camera_relative_to_balloon_quat, 'ZXY');
camera_yaw      = rad2deg(c_yaw);
camera_pitch    = rad2deg(c_pitch);
camera_roll     = rad2deg(c_roll);
% If the maths was right this should be [0 -45 0] not [0 0 -45]
[camera_yaw, camera_pitch, camera_roll]