Scribd
Upload
30 views13 pages

AE5337 Homework 1 Liu

Uploaded by

Edward
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
30 views13 pages

AE5337 Homework 1 Liu

Uploaded by

Edward
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 13

Solution:

Solution:
Solution:
Solution:
Matlab code:
gamma = 0;
beta = 0;
alpha = 0;

fprintf('Orientation (gamma, beta, alpha) = (%f, %f, %f) \n', gamma, beta, alpha)

ca = cos(alpha); sa = sin(alpha);
cb = cos(beta); sb = sin(beta);
cr = cos(gamma); sr = sin(gamma);

% Use Z-Y-X Euker Angles


fprintf('For roll, pitch, yaw angles (Z-Y-X Euker Angles), the rotation matrix is')
RM = [ca*cb ca*sb*sr-sa*cr ca*sb*cr+sa*sr ;
sa*cb sa*sb*sr+ca*cr sa*sb*cr-ca*sr ;
-sb cb*sr cb*cr ]

fprintf('Equivalent angle-axis representation:')


[theta, K] = RMtoAA(RM)

fprintf('Rotation matrix obtained from the equivalent angle and rotation axis:')
[RK] = AAtoRM(theta, K)

function [theta, K] = RMtoAA(RM)


% From rotation-matrix form to equivalent angle-axis form
theta = acos( (RM(1,1)+RM(2,2)+RM(3,3)-1)/2 );
if theta == 0
K = [1; 0; 0]; % can be arbitrary, take (1,0,0)
elseif theta == pi
kx = sqrt( (RM(1,1)+1)/2 );
if kx ~= 0
ky = RM(1,2)/(2*kx);
kz = RM(1,3)/(2*kx);
else
ky = sqrt( (RM(2,2)+1)/2 );
if ky ~= 0
kx = RM(1,2)/(2*ky);
kz = RM(2,3)/(2*ky);
else
kz = sqrt( (RM(3,3)+1)/2 );
kx = RM(1,3)/(2*kz);
ky = RM(2,3)/(2*kz);
end
end
K = [kx; ky; kz];
else
K = 1/(2*sin(theta))*[RM(3,2)-RM(2,3); RM(1,3)-RM(3,1); RM(2,1)-RM(1,2)];
end
end

function [RM] = AAtoRM(theta, K)


% From equivalent angle-axis representation to rotation-matrix representation
vt = 1-cos(theta);
ct = cos(theta); st = sin(theta);
kx = K(1); ky = K(2); kz = K(3);
RM = [ kx*kx*vt+ct kx*ky*vt-kz*st kx*kz*vt+ky*st ;
kx*ky*vt+kz*st ky*ky*vt+ct ky*kz*vt-kx*st ;
kx*kz*vt-ky*st ky*kz*vt+kx*st kz*kz*vt+ct ];
end

Test results:
For case of no rotation (𝛾, 𝛽, 𝛼) = (0,0,0), arbitrary rotation axis is taken as
(1,0,0)𝑇 , and rotation matrices from each representation match up.
>>
Orientation (gamma, beta, alpha) = (0.000000, 0.000000, 0.000000)
For roll, pitch, yaw angles (Z-Y-X Euler Angles), the rotation matrix is
RM =
1 0 0
0 1 0
0 0 1

Equivalent angle-axis representation:


theta =
0

K =
1
0
0

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
1 0 0
0 1 0
0 0 1

>>
For case of 180∘roll angle rotation (𝛾, 𝛽, 𝛼) = (𝜋, 0,0), equivalent rotation axis is
obtained correctly as (1,0,0)𝑇 , and rotation matrices from each representation
match up.
>>
Orientation (gamma, beta, alpha) = (3.141593, 0.000000, 0.000000)
For roll, pitch, yaw angles (Z-Y-X Euler Angles), the rotation matrix is
RM =
1.0000 0 0
0 -1.0000 -0.0000
0 0.0000 -1.0000

Equivalent angle-axis representation:


theta =
3.1416

K =
1
0
0

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
1.0000 0 0
0 -1.0000 -0.0000
0 0.0000 -1.0000

>>
For case of 180∘pitch angle rotation (𝛾, 𝛽, 𝛼) = (0, 𝜋, 0), equivalent rotation axis is
obtained correctly as (0,1,0)𝑇 , and rotation matrices from each representation
match up.
>>
Orientation (gamma, beta, alpha) = (0.000000, 3.141593, 0.000000)
For roll, pitch, yaw angles (Z-Y-X Euler Angles), the rotation matrix is
RM =
-1.0000 0 0.0000
0 1.0000 0
-0.0000 0 -1.0000

Equivalent angle-axis representation:


theta =
3.1416

K =
0
1
0

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
-1.0000 0 0.0000
0 1.0000 0
-0.0000 0 -1.0000
>>
For case of arbitrary rotation (𝛾, 𝛽, 𝛼) = (1,2,3), equivalent rotation axis and angle
are obtained correctly, and rotation matrices from each representation match up.
>>
Orientation (gamma, beta, alpha) = (1.000000, 2.000000, 3.000000)
For roll, pitch, yaw angles (Z-Y-X Euler Angles), the rotation matrix is
RM =
0.4120 -0.8337 -0.3676
-0.0587 -0.4269 0.9024
-0.9093 -0.3502 -0.2248

Equivalent angle-axis representation:


theta =
2.2394

K =
-0.7981
0.3451
0.4938

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
0.4120 -0.8337 -0.3676
-0.0587 -0.4269 0.9024
-0.9093 -0.3502 -0.2248

>>

Solution:
Matlab code:
gamma = 0;
beta = 0;
alpha = 0;

fprintf('Orientation (gamma, beta, alpha) = (%f, %f, %f) \n',gamma, beta, alpha)

ca = cos(alpha); sa = sin(alpha);
cb = cos(beta); sb = sin(beta);
cr = cos(gamma); sr = sin(gamma);

% Use Z-Y-Z Euker Angles


fprintf('For Z-Y-Z Euler angles, the rotation matrix is')
RM = [ca*cb*cr-sa*sr -ca*cb*sr-sa*cr ca*sb ;
sa*cb*cr+ca*sr -sa*cb*sr+ca*cr sa*sb ;
-sb*cr sb*sr cb ]

fprintf('Equivalent angle-axis representation:')


[theta, K] = RMtoAA(RM)

fprintf('Rotation matrix obtained from the equivalent angle and rotation axis:')
[RK] = AAtoRM(theta, K)

function [theta, K] = RMtoAA(RM)


% From rotation-matrix form to equivalent angle-axis form
theta = acos( (RM(1,1)+RM(2,2)+RM(3,3)-1)/2 );
if theta == 0
K = [1; 0; 0]; % can be arbitrary, take (1,0,0)
elseif theta == pi
kx = sqrt( (RM(1,1)+1)/2 );
if kx ~= 0
ky = RM(1,2)/(2*kx);
kz = RM(1,3)/(2*kx);
else
ky = sqrt( (RM(2,2)+1)/2 );
if ky ~= 0
kx = RM(1,2)/(2*ky);
kz = RM(2,3)/(2*ky);
else
kz = sqrt( (RM(3,3)+1)/2 );
kx = RM(1,3)/(2*kz);
ky = RM(2,3)/(2*kz);
end
end
K = [kx; ky; kz];
else
K = 1/(2*sin(theta))*[RM(3,2)-RM(2,3); RM(1,3)-RM(3,1); RM(2,1)-RM(1,2)];
end
end

function [RM] = AAtoRM(theta, K)


% From equivalent angle-axis representation to rotation-matrix representation
vt = 1-cos(theta);
ct = cos(theta); st = sin(theta);
kx = K(1); ky = K(2); kz = K(3);
RM = [ kx*kx*vt+ct kx*ky*vt-kz*st kx*kz*vt+ky*st ;
kx*ky*vt+kz*st ky*ky*vt+ct ky*kz*vt-kx*st ;
kx*kz*vt-ky*st ky*kz*vt+kx*st kz*kz*vt+ct ];
end

Test results:
For case of 180∘first angle rotation (𝛾, 𝛽, 𝛼) = (𝜋, 0,0), equivalent rotation axis is
obtained correctly as (0,0,1)𝑇 , and rotation matrices from each representation
match up.
Orientation (gamma, beta, alpha) = (3.141593, 0.000000, 0.000000)
For Z-Y-Z Euler angles, the rotation matrix is
RM =
-1.0000 -0.0000 0
0.0000 -1.0000 0
0 0 1.0000

Equivalent angle-axis representation:


theta =
3.1416

K =
0
0
1

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
-1.0000 -0.0000 0
0.0000 -1.0000 0
0 0 1.0000
>>
For case of 180∘second angle rotation (𝛾, 𝛽, 𝛼) = (0, 𝜋, 0), equivalent rotation axis
is obtained correctly as (0,1,0)𝑇 , and rotation matrices from each representation
match up.
Orientation (gamma, beta, alpha) = (0.000000, 3.141593, 0.000000)
For Z-Y-Z Euler angles, the rotation matrix is
RM =
-1.0000 0 0.0000
0 1.0000 0
-0.0000 0 -1.0000

Equivalent angle-axis representation:


theta =
3.1416

K =
0
1
0

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
-1.0000 0 0.0000
0 1.0000 0
-0.0000 0 -1.0000
>>
For case of arbitrary rotation (𝛾, 𝛽, 𝛼) = (1,2,3), equivalent rotation axis and angle
are obtained correctly, and rotation matrices from each representation match up.
Note that the equivalent rotation axis, angle and rotation matrix obtained here are
different than those in the last test case in last exercise despite the rotation angles
are the same.
Orientation (gamma, beta, alpha) = (1.000000, 2.000000, 3.000000)
For Z-Y-Z Euler angles, the rotation matrix is
RM =
0.1038 -0.4229 -0.9002
-0.8648 -0.4855 0.1283
-0.4913 0.7651 -0.4161

Equivalent angle-axis representation:


theta =
2.6880

K =
0.7267
-0.4666
-0.5042

Rotation matrix obtained from the equivalent angle and rotation axis:
RK =
0.1038 -0.4229 -0.9002
-0.8648 -0.4855 0.1283
-0.4913 0.7651 -0.4161
>>
Solution:

Solution:
Solution:

Solution:
Solution:

You might also like