- #1
bras0463
- 3
- 0
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:
There is clearly a problem with the maths but I can't see what it is. Does this quaternion maths look correct?
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:
Code:
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. Does this quaternion maths look correct?