（源代碼）四元數與歐拉角間的轉換

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