clear all;

% define roll, pitch and yaw variables which are rotations about the X, Y
% and Z body axes respectively
syms roll pitch yaw 'real'

% Define transformaton matrices for rotations about the X,Y and Z body axes
Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];

% calculate the tranformation matrix from body to navigation frame resulting from
% a rotation in yaw, roll, pitch order
Tbn_312 = transpose(Yrot*(Xrot*Zrot));
% Tbn_312 = 
%
% [ cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), -cos(roll)*sin(yaw), cos(yaw)*sin(pitch) + cos(pitch)*sin(roll)*sin(yaw)]
% [ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll),  cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)]
% [                               -cos(roll)*sin(pitch),           sin(roll),                                cos(pitch)*cos(roll)]

% define the quaternion elements
syms q0 q1 q2 q3 'real'

% calculate the tranformation matrix from body to navigation frame as a
% function of the quaternions
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
syms q0 q1 q2 q3 'real' % quaternions defining attitude of body axes relative to local NED
quat = [q0;q1;q2;q3];
Tbn = Quat2Tbn(quat);

% Express each 312 Tait-Bryan angle as a function of the quaternions
roll_312 = asin(Tbn(3,2));
yaw_312 = atan(-Tbn(1,2),Tbn(2,2));
pitch_312 = atan(-Tbn(3,1),Tbn(3,3));
G_yaw = jacobian(yaw_312,quat);