四元数转欧拉角matlab

function [ angle] = fromQToE（ q ）
 %  z   y  x
 q0=q（1）;
 q1=q（2）;
 q2=q（3）;
 q3=q（4）;
 m13=2*（q1*q3-q0*q2）;
 m23=2*（q2*q3+q0*q1）;
 m33=1-2*（q1^2+q2^2）;
 m12=2*（q1*q2+q0*q3）; 
 m11=1-2*（q2^2+q3^2）;
 m21=2*（q1*q2-q0*q3）;
 m22=1-2*（q1^2+q3^2）;

     
 if （-m13）>=1.0
     angle（2）=1.57;
 elseif （-m13）<=-1.0
     angle（2）=-1.57;
 else
     angle（2）=asin（-m13）;    
 end
 if abs（m13）>0.9999
     angle（1）=atan2（-m21m22）;
     angle（3）=0;
 else
     angle（1）=atan2（m12m11）;
     angle（3）=atan2（m23m33）;
 end
%   %  y   x  z
%  q0=q（1）;
%  q1=q（2）;
%  q2=q（3）;