

addpath('SpinCalc');
addpath('quaternions');



TC_Qtn = [TC_Qtn1' TC_Qtn2' TC_Qtn3' TC_Qtn4'];
TC_Qtn =qmult(TC_Qtn,[sqrt(.5) 0 -sqrt(.5) 0]);  %Rotate sensor 90 degrees about y-axis to make z point up.



    EA = SpinCalc('QtoEA321',TC_Qtn,.001,1);
    
    idx = find(EA(:,1) >180);
    EA(idx,1) = EA(idx,1)-360;
    
    idx = find(EA(:,2) >180);
    EA(idx,2) = EA(idx,2)-360;
    
    idx = find(EA(:,3) > 180);
    EA(idx,3) = EA(idx,3)-360; 
    
    figure
    ah1 = subplot(3,1,1)
    plot(EA(:,1));
    
     ah2 = subplot(3,1,2)
    plot(EA(:,2));
    
     ah2 = subplot(3,1,3)
    plot(EA(:,3));
    