matlab/ins/RPY2Quaternion.m

   1 function q = RPY2Quaternion(rpy)
   2
   3 DEG2RAD = pi / 180;
   4 phi = DEG2RAD * rpy(1) / 2;
   5 theta = DEG2RAD * rpy(2) / 2;
   6 psi = DEG2RAD * rpy(3) / 2;
   7 cphi = cos(phi);
   8 sphi = sin(phi);
   9 ctheta = cos(theta);
  10 stheta = sin(theta);
  11 cpsi = cos(psi);
  12 spsi = sin(psi);
  13
  14 q(1) = cphi * ctheta * cpsi + sphi * stheta * spsi;
  15 q(2) = sphi * ctheta * cpsi - cphi * stheta * spsi;
  16 q(3) = cphi * stheta * cpsi + sphi * ctheta * spsi;
  17 q(4) = cphi * ctheta * spsi - sphi * stheta * cpsi;
  18
  19 if q(1) < 0
  20     q = -q;
  21 end