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

28 lines
635 B
Matlab

function [rpy p y] = Quaternion2RPY(q)
% [rpy p y] = Quaternion2RPY(q)
%
% JC 2012-02-26
assert(size(q,2) == 4, 'Quaternion wrong shape');
RAD2DEG = 180 / pi;
q0s = q(:,1) .^ 2;
q1s = q(:,2) .^ 2;
q2s = q(:,3) .^ 2;
q3s = q(:,4) .^ 2;
R13 = 2.0 * (q(:,2) .* q(:,4) - q(:,1) .* q(:,3));
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0 * (q(:,2) .* q(:,3) + q(:,1) .* q(:,4));
R23 = 2.0 * (q(:,3) .* q(:,4) + q(:,1) .* q(:,2));
R33 = q0s - q1s - q2s + q3s;
rpy(:,2) = RAD2DEG * asin(-R13);
rpy(:,3) = RAD2DEG * atan2(R12, R11);
rpy(:,1) = RAD2DEG * atan2(R23, R33);
if nargout > 1
p = rpy(:,2);
y = rpy(:,3);
rpy(:,2:3) = [];
end