clear

clc

vartheta_c=0.1;

fea_c=0.1;

gamma_c=0;

q1=sin(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q2=sin(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

q3=cos(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q4=cos(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

Dt=0.001;

n=1;

t=0;

for i=1:5000

if t<1.5

vartheta_c=vartheta_c;

fea_c=fea_c;

gamma_c=gamma_c;

else

vartheta_c=vartheta_c-Dt*pi/180;

fea_c=fea_c+Dt*pi/180;

gamma_c=gamma_c+Dt*pi/180;

end

q1_c=sin(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q2_c=sin(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

q3_c=cos(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q4_c=cos(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

vartheta=asin(2*(q1_c*q2_c+q3_c*q4_c));

fea=-atan(2*(q1_c*q3_c-q4_c*q2_c)/(q1_c^2-q2_c^2-q3_c^2+q4_c^2));

gamma=-atan(2*(q2_c*q3_c-q4_c*q1_c)/(-q1_c^2+q2_c^2-q3_c^2+q4_c^2));

q1=sin(fea/2)*sin(vartheta/2)*cos(gamma/2)+cos(fea/2)*cos(vartheta/2)*sin(gamma/2);

q2=sin(fea/2)*cos(vartheta/2)*cos(gamma/2)+cos(fea/2)*sin(vartheta/2)*sin(gamma/2);

q3=cos(fea/2)*sin(vartheta/2)*cos(gamma/2)-sin(fea/2)*cos(vartheta/2)*sin(gamma/2);

q4=cos(fea/2)*cos(vartheta/2)*cos(gamma/2)-sin(fea/2)*sin(vartheta/2)*sin(gamma/2);

q=[q1;q2;q3;q4];

vartheta_store(:,n)=[vartheta_c;vartheta];

fea_store(:,n)=[fea_c;fea];

gamma_store(:,n)=[gamma_c;gamma];

q_store(:,n)=[q1_c;q2_c;q3_c;q4_c;q];

n=n+1;

t=t+Dt;

end

figure(1)

plot((1:n-1)*Dt,gamma_store(1,:)*180/pi,(1:300:n-1)*Dt,gamma_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\gamma[Deg]')

figure(2)

plot((1:n-1)*Dt,vartheta_store(1,:)*180/pi,(1:300:n-1)*Dt,vartheta_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\theta[Deg]')

figure(3)

plot((1:n-1)*Dt,fea_store(1,:)*180/pi,(1:300:n-1)*Dt,fea_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\psi[Deg]')

figure(4)

plot((1:n-1)*Dt,q_store(1,:),(1:300:n-1)*Dt,q_store(5,1:300:end),'r+')

legend('q_1_Ori','q_1_Tran')

xlabel('时间/s')

ylabel('q_1')

figure(5)

plot((1:n-1)*Dt,q_store(2,:),(1:300:n-1)*Dt,q_store(6,1:300:end),'r+')

legend('q_2_Ori','q_2_Tran')

xlabel('时间/s')

ylabel('q_2')

figure(6)

plot((1:n-1)*Dt,q_store(3,:),(1:300:n-1)*Dt,q_store(7,1:300:end),'r+')

legend('q_3__Ori','q_3_Tran')

xlabel('时间/s')

ylabel('q_3')

figure(7)

plot((1:n-1)*Dt,q_store(4,:),(1:300:n-1)*Dt,q_store(8,1:300:end),'r+')

legend('q_4__Ori','q_4_Tran')

xlabel('时间/s')

ylabel('q_4')