matlab/ins/Quaternion2RPY.m

   1 function [rpy p y] = Quaternion2RPY(q)
   2 % [rpy p y] = Quaternion2RPY(q)
   3 %
   4 % JC 2012-02-26
   5
   6 assert(size(q,2) == 4, 'Quaternion wrong shape');
   7 RAD2DEG = 180 / pi;
   8
   9 q0s = q(:,1) .^ 2;
  10 q1s = q(:,2) .^ 2;
  11 q2s = q(:,3) .^ 2;
  12 q3s = q(:,4) .^ 2;
  13
  14 R13 = 2.0 * (q(:,2) .* q(:,4) - q(:,1) .* q(:,3));
  15 R11 = q0s + q1s - q2s - q3s;
  16 R12 = 2.0 * (q(:,2) .* q(:,3) + q(:,1) .* q(:,4));
  17 R23 = 2.0 * (q(:,3) .* q(:,4) + q(:,1) .* q(:,2));
  18 R33 = q0s - q1s - q2s + q3s;
  19
  20 rpy(:,2) = RAD2DEG * asin(-R13);
  21 rpy(:,3) = RAD2DEG * atan2(R12, R11);
  22 rpy(:,1) = RAD2DEG * atan2(R23, R33);
  23
  24 if nargout > 1
  25     p = rpy(:,2);
  26     y = rpy(:,3);
  27     rpy(:,2:3) = [];
  28 end