eulr2qrot.m

惯性导航系统传递对准系统模型程序

function qua_vec = eulr2qrot(eul_vect)

phi = eul_vect(1);
theta = eul_vect(2);
psi = eul_vect(3);

Ro = sqrt(power(phi,2) + power(theta,2) + power(psi,2));

if Ro == 0,
    qua_vec(1) = 1;
    qua_vec(2) = 0;
    qua_vec(3) = 0;
    qua_vec(4) = 0;
else
    qua_vec(1) = cos(Ro/2);
    qua_vec(2) = phi*sin(Ro/2)/Ro;
    qua_vec(3) = theta*sin(Ro/2)/Ro;
    qua_vec(4) = psi*sin(Ro/2)/Ro;
end