Code covered by the BSD License  

Highlights from
conversionMatrix2Quaternion

image thumbnail
from conversionMatrix2Quaternion by Marco Borges
Function to convert Rotation Matrix to Quaternion

conversionMatrix2Quaternion(M, varargin)
function [q, M] = conversionMatrix2Quaternion(M, varargin)
% conversionMatrix2Quaternion - function to convert Rotation Matrix to Quaternion coeff
%
% Syntax:  q = conversionMatrix2Quaternion(M(3,3))
%          q = conversionMatrix2Quaternion(M(1,1),M(1,2),M(1,3),M(2,1),M(2,2),M(2,3),M(3,1),M(3,2),M(3,3))
%          q = conversionMatrix2Quaternion([theta, psi, phi])
%          q = conversionMatrix2Quaternion(theta, psi, phi)
%
% Inputs:
%          M - Rotation Matrix (3,3)
%     *theta - X rotation angle (deg)
%       *psi - Y rotation angle (deg)
%       *phi - Z rotation angle (deg)
%
% Outputs:
%     q - quaternion values [q0 qx qy qz]
%     M - Rotation Matrix used to compute q
%
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: none;

% Author: Marco Borges, Ph.D. Student, Computer/Biomedical Engineer
% UFMG, PPGEE, Neurodinamica Lab, Brazil
% email address: marcoafborges@gmail.com
% Website: http://www.cpdee.ufmg.br/
% Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
% April 2013; v2; Last revision: 2013-04-18
% Changelog: v2 - add linear velocity extraction

%------------- BEGIN CODE --------------
% M = [m00 m01 m02;
%     m10 m11 m12;
%     m20 m21 m22];

if length(M) == 1 && nargin == 8
    M = [M, varargin{1}, varargin{2}; varargin{3}, varargin{4}, varargin{5}; varargin{6}, varargin{7}, varargin{8}];
    
elseif length(M) == 9
    M = [M(1), M(2), M(3); M(4), M(5), M(6); M(7), M(8), M(9)];
    
elseif length(M) == 3 % [theta (x rot), psi (y rot), phi (z rot)]
    theta = M(1); psi = M(2); phi = M(3);
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];

elseif length(M) == 1 && nargin == 3 % (theta (x rot), psi (y rot), phi (z rot))
    theta = M; psi = varargin{1}; phi = varargin{2};
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];
end
    

tr = M(1,1) + M(2,2) + M(3,3);

if (tr > 0)
    S = sqrt(tr+1.0) * 2; % S=4*qw
    qw = 0.25 * S;
    qx = (M(3,2) - M(2,3)) / S;
    qy = (M(1,3) - M(3,1)) / S;
    qz = (M(2,1) - M(1,2)) / S;
elseif ((M(1,1) > M(2,2)) && (M(1,1) > M(3,3)))
    S = sqrt(1.0 + M(1,1) - M(2,2) - M(3,3)) * 2; % S=4*qx
    qw = (M(3,2) - M(2,3)) / S;
    qx = 0.25 * S;
    qy = (M(1,2) + M(2,1)) / S;
    qz = (M(1,3) + M(3,1)) / S;
elseif (M(2,2) > M(3,3))
    S = sqrt(1.0 + M(2,2) - M(1,1) - M(3,3)) * 2; % S=4*qy
    qw = (M(1,3) - M(3,1)) / S;
    qx = (M(1,2) + M(2,1)) / S;
    qy = 0.25 * S;
    qz = (M(2,3) + M(3,2)) / S;
else
    S = sqrt(1.0 + M(3,3) - M(1,1) - M(2,2)) * 2; % S=4*qz
    qw = (M(2,1) - M(1,2)) / S;
    qx = (M(1,3) + M(3,1)) / S;
    qy = (M(2,3) + M(3,2)) / S;
    qz = 0.25 * S;
end

q = [qw qx qy qz];
%-------------- END CODE ---------------

Contact us