（源代码）四元数与欧拉角间的转换

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')