% For the yaw-pitch-roll sequence calculate
clc;
clear all;
yaw   = 50;  % [deg]
pitch = 90;  % [deg]
roll  = 120; % [deg]

The quaternion

Now lets optain direction cosine matrix from Yaw,Pitch and Roll angles

 R1_roll  =  [1            0           0;
             0            cosd(roll)  sind(roll);
             0           -sind(roll)  cosd(roll);
             ]
 R2_pitch = [cosd(pitch)  0     -sind(pitch);
             0            1       0;
             sind(pitch)  0      cosd(pitch)]

 R3_yaw   =   [ cosd(yaw)   sind(yaw)   0;
             -sind(yaw)   cosd(yaw)   0;
             0            0          1]
 QXx      = R1_roll*R2_pitch*R3_yaw

 K =1/3*[ QXx(1,1)-QXx(2,2)-QXx(3,3), QXx(2,1)+QXx(1,2), QXx(3,1)+QXx(1,3),...
     QXx(2,3)-QXx(3,2);
     QXx(2,1)+QXx(1,2), -QXx(1,1)+QXx(2,2)-QXx(3,3), QXx(3,2)+QXx(2,3),...
     QXx(3,1) - QXx(1,3);
     QXx(3,1)+QXx(1,3), QXx(3,2)+QXx(2,3), -QXx(1,1)-QXx(2,2)-QXx(3,3),...
     QXx(1,2)-QXx(2,1);
     QXx(2,3) - QXx(3,2), QXx(3,1)- QXx(1,3), QXx(1,2)- QXx(2,1),...
     QXx(1,1)+QXx(2,2)+QXx(3,3)
     ]
% The eigenvalues and eigenvectors of this matrix are
 [V,D] = eig(K);
% Finding the largest eigenvalue
eigv = [D(1,1),D(2,2),D(3,3),D(4,4)]
[max_eig,i] = max(eigv);
% The quaternion is the eigenvector coresponding to the maximum eigenvalue
q = V(:,i)
% Checking the norm of quaternion.Should be equal to 1
q_norm = norm(q)

R1_roll =

    1.0000         0         0
         0   -0.5000    0.8660
         0   -0.8660   -0.5000

R2_pitch =

     0     0    -1
     0     1     0
     1     0     0

R3_yaw =

    0.6428    0.7660         0
   -0.7660    0.6428         0
         0         0    1.0000

QXx =

         0         0   -1.0000
    0.9397    0.3420         0
    0.3420   -0.9397         0

K =

   -0.1140    0.3132   -0.2193    0.3132
    0.3132    0.1140   -0.3132    0.4473
   -0.2193   -0.3132   -0.1140   -0.3132
    0.3132    0.4473   -0.3132    0.1140

eigv =

   -0.3333   -0.3333   -0.3333    1.0000

q =

   -0.4056
   -0.5792
    0.4056
   -0.5792

q_norm =

    1.0000

Lets now calculate quaternion using another approach.This procedure is easy but fails when q4 = 0.

q4 = 0.5*sqrt(1 + QXx(1,1) + QXx(2,2) + QXx(3,3));
q1 = (QXx(2,3) - QXx(3,2))/(4*q4);
q2 = (QXx(3,1) - QXx(1,3))/(4*q4);
q3 = (QXx(1,2) - QXx(2,1))/(4*q4);

q = [q1;q2;q3;q4]
q_norm = norm(q)

q =

    0.4056
    0.5792
   -0.4056
    0.5792

q_norm =

     1

Euler principal rotation angle and Euler axis of rotation.

Euler principal rotation angle [deg]

theta = 2*acos(q(4))*180/pi
% The unit vector along the Euler axis around which the inertial reference
% frame is rotated into the body fixed frame

u = [q1 q2 q3]/(sind(theta/2))

theta =

  109.2075

u =

    0.4975    0.7106   -0.4975

Published with MATLAB® 7.10