Professional Documents
Culture Documents
C:\Users\marathon\Desktop\SpinCalc.m
1 of 8
function OUTPUT=SpinCalc(CONVERSION,INPUT,tol,ichk)
%Function for the conversion of one rotation input type to desired output.
%Supported conversion input/output types are as follows:
%
1: Q
Rotation Quaternions
%
2: EV
Euler Vector and rotation angle (degrees)
%
3: DCM
Orthogonal DCM Rotation Matrix
%
4: EA###
Euler angles (12 possible sets) (degrees)
%
%Author: John Fuller
%National Institute of Aerospace
%Hampton, VA 23666
%John.Fuller@nianet.org
%
%Version 1.3
%June 30th, 2009
%
%Version 1.3 updates
%
SpinCalc now detects when input data is too close to Euler singularity, if user is
choosing
%
Euler angle output. Prohibits output if middle angle is within 0.1 degree of
singularity value.
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
OUTPUT=SpinCalc(CONVERSION,INPUT,tol,ichk)
%Inputs:
%CONVERSION - Single string value that dictates the type of desired
%
conversion. The conversion strings are listed below.
%
%
'DCMtoEA###' 'DCMtoEV'
'DCMtoQ'
**for cases that involve
%
'EA###toDCM' 'EA###toEV' 'EA###toQ'
euler angles, ### should be
%
'EVtoDCM'
'EVtoEA###' 'EVtoQ'
replaced with the proper
%
'QtoDCM'
'QtoEA###'
'QtoEV'
order desired. EA321 would
%
'EA###toEA###'
be Z(yaw)-Y(pitch)-X(roll).
%
%INPUT - matrix or vector that corresponds to the first entry in the
%
CONVERSION string, formatted as follows:
%
%
DCM - 3x3xN multidimensional matrix which pre-multiplies a coordinate
%
frame column vector to calculate its coordinates in the desired
%
new frame.
%
%
EA### - [psi,theta,phi] (Nx3) row vector list dictating to the first angle
%
rotation (psi), the second (theta), and third (phi) (DEGREES)
%
%
EV - [m1,m2,m3,MU] (Nx4) row vector list dictating the components of euler
%
rotation vector (original coordinate frame) and the Euler
%
rotation angle about that vector (MU) (DEGREES)
%
%
Q - [q1,q2,q3,q4] (Nx4) row vector list defining quaternion of
%
rotation. q4 = cos(MU/2) where MU is Euler rotation angle
%
%tol - tolerance value
%ichk - 0 disables warning flags
%
1 enables warning flags (near singularities)
%**NOTE: N corresponds to multiple orientations
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%Output:
%OUTPUT - matrix or vector corresponding to the second entry in the
%
CONVERSION input string, formatted as shown above.
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%Pre-processer to determine type of conversion from CONVERSION string input
%Types are numbered as follows:
%Q=1
EV=2
DCM=3
EA=4
i_type=strfind(lower(CONVERSION),'to');
length=size(CONVERSION,2);
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
2 of 8
if length>12 || length<4,
%no CONVERSION string can be shorter than 4 or longer than
12 chars
error('Error: Invalid entry for CONVERSION input string');
end
o_type=length-i_type;
if i_type<5,
i_type=i_type-1;
else
i_type=i_type-2;
end
if o_type<5,
o_type=o_type-1;
else
o_type=o_type-2;
end
TYPES=cell(1,4);
TYPES{1,1}='Q'; TYPES{1,2}='EV'; TYPES{1,3}='DCM'; TYPES{1,4}='EA';
INPUT_TYPE=TYPES{1,i_type};
OUTPUT_TYPE=TYPES{1,o_type};
clear TYPES
%Confirm input as compared to program interpretation
if i_type~=4 && o_type~=4, %if input/output are NOT Euler angles
CC=[INPUT_TYPE,'to',OUTPUT_TYPE];
if strcmpi(CONVERSION,CC)==0;
error('Error: Invalid entry for CONVERSION input string');
end
else
if i_type==4,
%if input type is Euler angles, determine the order of rotations
EULER_order_in=str2double(CONVERSION(1,3:5));
rot_1_in=floor(EULER_order_in/100);
%first rotation
rot_2_in=floor((EULER_order_in-rot_1_in*100)/10);
%second rotation
rot_3_in=(EULER_order_in-rot_1_in*100-rot_2_in*10);
%third rotation
if rot_1_in<1 || rot_2_in<1 || rot_3_in<1 || rot_1_in>3 || rot_2_in>3 ||
rot_3_in>3,
error('Error: Invalid input Euler angle order type (conversion string).');
%check that all orders are between 1 and 3
elseif rot_1_in==rot_2_in || rot_2_in==rot_3_in,
error('Error: Invalid input Euler angle order type (conversion string).');
%check that no 2 consecutive orders are equal (invalid)
end
%check input dimensions to be 1x3x1
if size(INPUT,2)~=3 || size(INPUT,3)~=1
error('Error: Input euler angle data vector is not Nx3')
end
%identify singularities
input_size = size(INPUT);
N = input_size(1);
% Identify singularities (second Euler angle out of range)
EA2 = INPUT(:,2); % (Nx1) 2nd Euler angle(s)
ZEROS = zeros(N,1); % (Nx1)
ONES = ones(N,1); % (Nx1)
if rot_1_in==rot_3_in % Type 2 rotation (1st and 3rd rotations about same axis)
if any(EA2>180*ONES) || any(EA2<ZEROS)
error('Second input Euler angle(s) outside 0 to 180 degree range')
elseif any(EA2>=178*ONES) || any(EA2<=2*ONES)
if ichk==1
errordlg('Warning: Second input Euler angle(s) near a singularity
(0 or 180 degrees).')
end
end
else % Type 1 rotation (rotations about three distinct axes)
if any(abs(EA2)>=90*ONES)
error('Second input Euler angle(s) outside -90 to 90 degree range')
elseif any(abs(EA2)>88*ONES)
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
3 of 8
if ichk==1
errordlg('Warning: Second input Euler angle(s) near a singularity
(-90 or 90 degrees).')
end
end
end
end
if o_type==4,
%if output type is Euler angles, determine order of rotations
EULER_order_out=str2double(CONVERSION(1,length-2:length));
rot_1_out=floor(EULER_order_out/100);
%first rotation
rot_2_out=floor((EULER_order_out-rot_1_out*100)/10);
%second rotation
rot_3_out=(EULER_order_out-rot_1_out*100-rot_2_out*10); %third rotation
if rot_1_out<1 || rot_2_out<1 || rot_3_out<1 || rot_1_out>3 || rot_2_out>3 ||
rot_3_out>3,
error('Error: Invalid output Euler angle order type (conversion string).');
%check that all orders are between 1 and 3
elseif rot_1_out==rot_2_out || rot_2_out==rot_3_out,
error('Error: Invalid output Euler angle order type (conversion string).');
%check that no 2 consecutive orders are equal
end
end
if i_type==4 && o_type~=4, %if input are euler angles but not output
CC=['EA',num2str(EULER_order_in),'to',OUTPUT_TYPE]; %construct program
conversion string for checking against user input
elseif o_type==4 && i_type~=4, %if output are euler angles but not input
CC=[INPUT_TYPE,'to','EA',num2str(EULER_order_out)]; %construct program
conversion string for checking against user input
elseif i_type==4 && o_type==4, %if both input and output are euler angles
CC=['EA',num2str(EULER_order_in),'to','EA',num2str(EULER_order_out)];
%
construct program conversion string
end
if strcmpi(CONVERSION,CC)==0; %check program conversion string against user input
to confirm the conversion command
error('Error: Invalid entry for CONVERSION input string');
end
end
clear i_type o_type CC
%From the input, determine the quaternions that uniquely describe the
%rotation prescribed by that input. The output will be calculated in the
%second portion of the code from these quaternions.
switch INPUT_TYPE
case 'DCM'
if size(INPUT,1)~=3 || size(INPUT,2)~=3 %check DCM dimensions
error('Error: DCM matrix is not 3x3xN');
end
N=size(INPUT,3);
%number of orientations
%Check if matrix is indeed orthogonal
perturbed=NaN(3,3,N);
DCM_flag=0;
for ii=1:N,
perturbed(:,:,ii)=abs(INPUT(:,:,ii)*INPUT(:,:,ii)'-eye(3)); %perturbed
array shows difference between DCM*DCM' and I
if abs(det(INPUT(:,:,ii))-1)>tol, %if determinant is off by one more than
tol, user is warned.
if ichk==1,
DCM_flag=1;
end
end
if abs(det(INPUT(:,:,ii))+1)<0.05, %if determinant is near -1, DCM is
improper
error('Error: Input DCM(s) improper');
end
if DCM_flag==1,
errordlg('Warning: Input DCM matrix determinant(s) off from 1 by more
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
4 of 8
than tolerance.')
end
end
DCM_flag=0;
if ichk==1,
for kk=1:N,
for ii=1:3,
for jj=1:3,
if perturbed(ii,jj,kk)>tol,
%if any difference is larger than
tol, user is warned.
DCM_flag=1;
end
end
end
end
if DCM_flag==1,
fprintf('Warning: Input DCM(s) matrix not orthogonal to precision
tolerance.')
end
end
clear perturbed DCM_flag
Q=NaN(4,N);
for ii=1:N,
denom=NaN(4,1);
denom(1)=0.5*sqrt(1+INPUT(1,1,ii)-INPUT(2,2,ii)-INPUT(3,3,ii));
denom(2)=0.5*sqrt(1-INPUT(1,1,ii)+INPUT(2,2,ii)-INPUT(3,3,ii));
denom(3)=0.5*sqrt(1-INPUT(1,1,ii)-INPUT(2,2,ii)+INPUT(3,3,ii));
denom(4)=0.5*sqrt(1+INPUT(1,1,ii)+INPUT(2,2,ii)+INPUT(3,3,ii));
%determine which Q equations maximize denominator
switch find(denom==max(denom),1,'first') %determines max value of qtests
to put in denominator
case 1
Q(1,ii)=denom(1);
Q(2,ii)=(INPUT(1,2,ii)+INPUT(2,1,ii))/(4*Q(1,ii));
Q(3,ii)=(INPUT(1,3,ii)+INPUT(3,1,ii))/(4*Q(1,ii));
Q(4,ii)=(INPUT(2,3,ii)-INPUT(3,2,ii))/(4*Q(1,ii));
case 2
Q(2,ii)=denom(2);
Q(1,ii)=(INPUT(1,2,ii)+INPUT(2,1,ii))/(4*Q(2,ii));
Q(3,ii)=(INPUT(2,3,ii)+INPUT(3,2,ii))/(4*Q(2,ii));
Q(4,ii)=(INPUT(3,1,ii)-INPUT(1,3,ii))/(4*Q(2,ii));
case 3
Q(3,ii)=denom(3);
Q(1,ii)=(INPUT(1,3,ii)+INPUT(3,1,ii))/(4*Q(3,ii));
Q(2,ii)=(INPUT(2,3,ii)+INPUT(3,2,ii))/(4*Q(3,ii));
Q(4,ii)=(INPUT(1,2,ii)-INPUT(2,1,ii))/(4*Q(3,ii));
case 4
Q(4,ii)=denom(4);
Q(1,ii)=(INPUT(2,3,ii)-INPUT(3,2,ii))/(4*Q(4,ii));
Q(2,ii)=(INPUT(3,1,ii)-INPUT(1,3,ii))/(4*Q(4,ii));
Q(3,ii)=(INPUT(1,2,ii)-INPUT(2,1,ii))/(4*Q(4,ii));
end
end
Q=Q';
clear denom
case 'EV' %Euler Vector Input Type
if size(INPUT,2)~=4 || size(INPUT,3)~=1
%check dimensions
error('Error: Input euler vector and rotation data matrix is not Nx4')
end
N=size(INPUT,1);
MU=INPUT(:,4)*pi/180; %assign mu name for clarity
if sqrt(INPUT(:,1).^2+INPUT(:,2).^2+INPUT(:,3).^2)-ones(N,1)>tol*ones(N,1), %
check that input m's constitute unit vector
error('Input euler vector(s) components do not constitute a unit vector')
end
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
5 of 8
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
6 of 8
12/24/13 2:58 PM
C:\Users\marathon\Desktop\SpinCalc.m
7 of 8
^2+Q(:,3).^2));
theta=asin(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)));
phi=atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).
^2-Q(:,3).^2));
Euler_type=1;
elseif EULER_order_out==231;
psi=atan2(2.*(Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).
^2-Q(:,3).^2));
theta=asin(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)));
phi=atan2(2.*(Q(:,1).*Q(:,4)-Q(:,3).*Q(:,2)),(Q(:,4).^2-Q(:,1).^2+Q(:,2).
^2-Q(:,3).^2));
Euler_type=1;
elseif EULER_order_out==312;
psi=atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),(Q(:,4).^2-Q(:,1).^2+Q(:,2).
^2-Q(:,3).^2));
theta=asin(2.*(Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3)));
phi=atan2(2.*(Q(:,2).*Q(:,4)-Q(:,3).*Q(:,1)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).
^2+Q(:,3).^2));
Euler_type=1;
elseif EULER_order_out==132;
psi=atan2(2.*(Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3)),(Q(:,4).^2-Q(:,1).^2+Q(:,2).
^2-Q(:,3).^2));
theta=asin(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)));
phi=atan2(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).
^2-Q(:,3).^2));
Euler_type=1;
elseif EULER_order_out==213;
psi=atan2(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).
^2+Q(:,3).^2));
theta=asin(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)));
phi=atan2(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)),(Q(:,4).^2-Q(:,1).^2+Q(:,2).
^2-Q(:,3).^2));
Euler_type=1;
elseif EULER_order_out==321;
psi=atan2(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).
^2-Q(:,3).^2));
theta=asin(2.*(Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3)));
phi=atan2(2.*(Q(:,1).*Q(:,4)+Q(:,3).*Q(:,2)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).
^2+Q(:,3).^2));
Euler_type=1;
else
error('Error: Invalid output Euler angle order type (conversion string).');
end
if(isreal([psi,theta,phi]))==0,
error('Error: Unreal Euler output. Input resides too close to singularity.
Please choose different output type.')
end
OUTPUT=mod([psi,theta,phi]*180/pi,360); %deg
if Euler_type==1,
sing_chk=find(abs(theta)*180/pi>89.9);
sing_chk=sort(sing_chk(sing_chk>0));
if size(sing_chk,1)>=1,
error('Error: Input rotation #%s resides too close to Type 1 Euler
singularity.\nType 1 Euler singularity occurs when second angle is -90 or 90 degrees.
\nPlease choose different output type.',num2str(sing_chk(1,1)));
end
elseif Euler_type==2,
sing_chk=[find(abs(theta*180/pi)<0.1);find(abs(theta*180/pi-180)<0.1);find
(abs(theta*180/pi-360))<0.1];
sing_chk=sort(sing_chk(sing_chk>0));
if size(sing_chk,1)>=1,
error('Error: Input rotation #%s resides too close to Type 2 Euler
singularity.\nType 2 Euler singularity occurs when second angle is 0 or 180 degrees.
\nPlease choose different output type.',num2str(sing_chk(1,1)));
end
12/24/13 2:58 PM
end
end
C:\Users\marathon\Desktop\SpinCalc.m
8 of 8