%quaternions to euler angles for the aerospace sequence


function [phi,theta,psi]=euler_to_quat(q0,q1,q2,q3)

m11=2*q0^2+2*q1^2-1;
m12=2*q1*q2+2*q0*q3;
m13=2*q1*q3-2*q0*q2;
m23=2*q2*q3+2*q0*q1;
m33=2*q0^2+2*q3^2-1;

psi=atan2(m12,m11);
theta=asin(-m13);
phi=atan2(m23,m33);

if abs(phi)<10^-10
    phi=0;
end


if abs(q)<10^-10
    q0=0;
end

if abs(q2)<10^-10
    q0=0;
end

if abs(q3)<10^-10
    q0=0;
end
end