1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/matlab/ins/RPY2Quaternion.m
2012-04-03 03:42:20 -05:00

22 lines
488 B
Matlab

function q = RPY2Quaternion(rpy)
DEG2RAD = pi / 180;
phi = DEG2RAD * rpy(1) / 2;
theta = DEG2RAD * rpy(2) / 2;
psi = DEG2RAD * rpy(3) / 2;
cphi = cos(phi);
sphi = sin(phi);
ctheta = cos(theta);
stheta = sin(theta);
cpsi = cos(psi);
spsi = sin(psi);
q(1) = cphi * ctheta * cpsi + sphi * stheta * spsi;
q(2) = sphi * ctheta * cpsi - cphi * stheta * spsi;
q(3) = cphi * stheta * cpsi + sphi * ctheta * spsi;
q(4) = cphi * ctheta * spsi - sphi * stheta * cpsi;
if q(1) < 0
q = -q;
end