clear angles
flag = 1;
psi1=cpsiu;
theta1=cthetau;
phi1=cphiu;
Infr=rotsyaxis{1};
Supr=rotsyaxis{6};
it=1;
i=1;
angles(it,:) = [psi1, theta1, phi1];
while flag
    R = rotmat(0,-theta1,-phi1);
    Supr = (Supr'*R)';
    it=it+1;
    [psi1, theta1, phi1] = getAngle(Infr,Supr);
    angles(it,:) = [psi1, theta1, phi1];
    
    if (abs(angles(it,2))<1e-10) & (abs(angles(it,3))<1e-10)
        flag = 0;
        %IVDAngle(i,1) = angles(1);
        it
    end
end
figure
plot(angles(:,1))
hold on
plot(angles(:,2),'r')
plot(angles(:,3),'g')


close all
clear angles
flag = 1;
psi1=cpsiu;
theta1=cthetau;
phi1=cphiu;
Infr=rotsyaxis{1};
Supr=rotiyaxis{3};
vectorplotter3(Infr,Supr)
it=1;
i=1;
R=rotmat(0,-theta1,0);
Supr=Supr*R;
vectorplotter3(Infr,Supr)
[psi1, theta1, phi1] = getAngle(Infr,Supr);
R=rotmat(0,0,-phi1);
Supr=Supr*R;
vectorplotter3(Infr,Supr)
[psi1, theta1, phi1] = getAngle(Infr,Supr);
psi1
cpsiu

