Programming Euler angles in MATLAB
Programming Euler angles in MATLAB
(OP)
Hello all!
I am having difficulty extracting Euler angles from a rotated coordinate system in MATLAB. The problem comes in when the angles go outside the bounds of [-pi/2,pi/2] because the asin() function only outputs within those bounds. Does anyone have any insight into how I might be able to extract the proper angles without getting the sharp discontinuities in the plot as a function of time? I've really been banging my head on the wall for this one. I appreciate any help!
Thanks,
Andy
I am having difficulty extracting Euler angles from a rotated coordinate system in MATLAB. The problem comes in when the angles go outside the bounds of [-pi/2,pi/2] because the asin() function only outputs within those bounds. Does anyone have any insight into how I might be able to extract the proper angles without getting the sharp discontinuities in the plot as a function of time? I've really been banging my head on the wall for this one. I appreciate any help!
Thanks,
Andy





RE: Programming Euler angles in MATLAB
Cheers
Greg Locock
Please see FAQ731-376 for tips on how to make the best use of Eng-Tips.
RE: Programming Euler angles in MATLAB
RE: Programming Euler angles in MATLAB
function [PHI, THETA, PSI] = FindEulerAngles(i_SEG, k_SEG, L_SEG)
% This function finds the Euler angles of a rotated coordinate system based
% on the i, k, and L (line of nodes) values for a given segment. This code
% is supplement to the Vaughan GaitBook. The reason the formulas below do
% not match the ones given in the text is that he uses the asin function,
% which only reports values between [-pi/2, pi/2], so there were
% discontinuities and errors when the Euler angles were greater than pi/2.
% Therefore, the code below contains the formulas for several cases, which
% are explained at each angle calculation.
% IMPORTANT NOTE: This code will give incorrect results unless the vectors
% i_SEG, k_SEG, and L_SEG are normalized to 1 in each row!
I = [1 0 0]; % Define global X-direction
K = [0 0 1]; % Define global Z-direction
for count = 1:size(i_SEG,1)
% Phi is positive if the Line of nodes (K x k) contains a positive
% y-value
if L_SEG(count,2) >= 0
PHI(count,:) = acos(dot(I,L_SEG(count,:)));
% Phi is negative if the Line of nodes (K x k) contains a negative
% y-value
elseif L_SEG(count,2) < 0
PHI(count,:) = -acos(dot(I,L_SEG(count,:)));
end
% Theta is positive if the xy-plane is rotated positively about the
% Line of nodes (according to right-hand rule). Therefore, there are
% two cases that include this:
% 1) phi > 0 and the x-component of k_seg > 0
% 2) phi < 0 and the x-component of k_seg < 0
if (PHI(count,:)>=0 & k_SEG(count,1)>0) | (PHI(count,:)<0 & k_SEG(count,1)<0)
THETA(count,:) = acos(dot(K,k_SEG(count,:)));
% The two cases in which phi is negative are:
% 1) phi > 0 and the x-component of k_seg < 0
% 2) phi < 0 and the x-component of k_seg > 0
elseif (PHI(count,:)>=0 & k_SEG(count,1)<0) | (PHI(count,:)<0 & k_SEG(count,1)>0)
THETA(count,:) = -acos(dot(K,k_SEG(count,:)));
end
% Lastly, the third angle is positive if theta and the z-component of
% the i_seg > 0, as represented by the two cases:
% 1) theta > 0 and i_seg > 0
% 2) theta < 0 and i_seg < 0
if (THETA(count,:)>=0 & i_SEG(count,3)>0) | (THETA(count,:)<0 & i_SEG(count,3)<0)
PSI(count,:) = acos(dot(L_SEG(count,:),i_SEG(count,:)));
% The other cases are:
% 1) theta < 0 and i_seg > 0
% 2) theta > 0 and i_seg < 0
elseif (THETA(count,:)<0 & i_SEG(count,3)>0) | (THETA(count,:)>=0 & i_SEG(count,3)<0)
PSI(count,:) = -acos(dot(L_SEG(count,:),i_SEG(count,:)));
end
end