├── README.md ├── TestScript.m ├── axisAngle2quatern.m ├── axisAngle2rotMat.m ├── euler2quatern.m ├── euler2rotMat.m ├── quatern2euler.m ├── quatern2rotMat.m ├── quaternConj.m ├── quaternProd.m ├── quaternRotate.m ├── rotMat2euler.m └── rotMat2quatern.m /README.md: -------------------------------------------------------------------------------- 1 | Quaternion-MATLAB-Library 2 | ========================= 3 | -------------------------------------------------------------------------------- /TestScript.m: -------------------------------------------------------------------------------- 1 | % TestScript.m 2 | % 3 | % This script tests the quaternion library functions to ensure that each 4 | % function output is consistent. 5 | % 6 | % Date Author Notes 7 | % 27/09/2011 SOH Madgwick Initial release 8 | 9 | %% Start of script 10 | 11 | close all; % close all figures 12 | clear; % clear all variables 13 | clc; % clear the command terminal 14 | 15 | %% Axis-angle to rotation matrix 16 | 17 | axis = [1 2 3]; 18 | axis = axis / norm(axis); 19 | angle = pi/2; 20 | 21 | R = axisAngle2rotMat(axis, angle); 22 | num = ' % 1.5f'; 23 | a = sprintf('\rAxis-angle to rotation matrix:'); 24 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 25 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 26 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 27 | disp(strcat(a,b,c,d)); 28 | 29 | %% Axis-angle to quaternion 30 | 31 | q = axisAngle2quatern(axis, angle); 32 | num = ' % 1.5f'; 33 | a = sprintf('\rAxis-angle to quaternion:'); 34 | b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q); 35 | disp(strcat(a,b)); 36 | 37 | %% Quaternion to rotation matrix 38 | 39 | R = quatern2rotMat(q); 40 | num = ' % 1.5f'; 41 | a = sprintf('\rQuaternion to rotation matrix:'); 42 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 43 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 44 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 45 | disp(strcat(a,b,c,d)); 46 | 47 | %% Rotation matrix to quaternion 48 | 49 | q = rotMat2quatern(R); 50 | num = ' % 1.5f'; 51 | a = sprintf('\rRotation matrix to quaternion:'); 52 | b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q); 53 | disp(strcat(a,b)); 54 | 55 | %% Rotation matrix to ZYX Euler angles 56 | 57 | euler = rotMat2euler(R); 58 | num = ' % 1.5f'; 59 | a = sprintf('\rRotation matrix to ZYX Euler angles:'); 60 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler); 61 | disp(strcat(a,b)); 62 | 63 | %% Quaternion to ZYX Euler angles 64 | 65 | euler = quatern2euler(q); 66 | num = ' % 1.5f'; 67 | a = sprintf('\rQuaternion to ZYX Euler angles:'); 68 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler); 69 | disp(strcat(a,b)); 70 | 71 | %% ZYX Euler angles to rotation matrix 72 | 73 | R = euler2rotMat(euler(1), euler(2), euler(3)); 74 | num = ' % 1.5f'; 75 | a = sprintf('\rZYX Euler angles to rotation matrix:'); 76 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 77 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 78 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 79 | disp(strcat(a,b,c,d)); 80 | 81 | %% ZYX Euler angles to quaternion 82 | 83 | q = euler2quatern(euler(1), euler(2), euler(3)); 84 | a = sprintf('\rZYX Euler angles to quaternion:'); 85 | b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q); 86 | disp(strcat(a,b)); 87 | 88 | %% End of script -------------------------------------------------------------------------------- /axisAngle2quatern.m: -------------------------------------------------------------------------------- 1 | function q = axisAngle2quatern(axis, angle) 2 | %AXISANGLE2QUATERN Converts an axis-angle orientation to a quaternion 3 | % 4 | % q = axisAngle2quatern(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a quaternion where a 3D rotation 7 | % is described by an angular rotation around axis defined by a vector. 8 | % 9 | % For more information see: 10 | % http://x-io.co.uk/quaternions/ 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | q0 = cos(angle./2); 16 | q1 = -axis(:,1)*sin(angle./2); 17 | q2 = -axis(:,2)*sin(angle./2); 18 | q3 = -axis(:,3)*sin(angle./2); 19 | q = [q0 q1 q2 q3]; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /axisAngle2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = axisAngle2rotMat(axis, angle) 2 | %AXISANGLE2ROTMAT Converts an axis-angle orientation to a rotation matrix 3 | % 4 | % q = axisAngle2rotMat(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a rotation matrix where a 3D 7 | % rotation is described by an angular rotation around axis defined by a 8 | % vector. 9 | % 10 | % For more information see: 11 | % http://x-io.co.uk/quaternions/ 12 | % 13 | % Date Author Notes 14 | % 27/09/2011 SOH Madgwick Initial release 15 | 16 | kx = axis(:,1); 17 | ky = axis(:,2); 18 | kz = axis(:,3); 19 | cT = cos(angle); 20 | sT = sin(angle); 21 | vT = 1 - cos(angle); 22 | 23 | R(1,1,:) = kx.*kx.*vT + cT; 24 | R(1,2,:) = kx.*ky.*vT - kz.*sT; 25 | R(1,3,:) = kx.*kz.*vT + ky.*sT; 26 | 27 | R(2,1,:) = kx.*ky.*vT + kz.*sT; 28 | R(2,2,:) = ky.*ky.*vT + cT; 29 | R(2,3,:) = ky.*kz.*vT - kx.*sT; 30 | 31 | R(3,1,:) = kx.*kz.*vT - ky.*sT; 32 | R(3,2,:) = ky.*kz.*vT + kx.*sT; 33 | R(3,3,:) = kz.*kz.*vT + cT; 34 | end 35 | 36 | -------------------------------------------------------------------------------- /euler2quatern.m: -------------------------------------------------------------------------------- 1 | function q = euler2quatern(phi, theta, psi) 2 | %EULER2QUATERN Converts a ZYX Euler angle orientation to a quaternion 3 | % 4 | % q = euler2quatern(axis, angle) 5 | % 6 | % Converts ZYX Euler angle orientation to a quaternion where phi is a 7 | % rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://x-io.co.uk/quaternions/ 11 | % 12 | % Date Author Notes 13 | % 01/01/2015 SOH Madgwick Initial release 14 | 15 | cosPsi = cos(psi * 0.5); 16 | sinPsi = sin(psi * 0.5); 17 | 18 | cosTheta = cos(theta * 0.5); 19 | sinTheta = sin(theta * 0.5); 20 | 21 | cosPhi = cos(phi * 0.5); 22 | sinPhi = sin(phi * 0.5); 23 | 24 | q = [cosPsi * cosTheta * cosPhi + sinPsi * sinTheta * sinPhi 25 | -(cosPsi * cosTheta * sinPhi - sinPsi * sinTheta * cosPhi) 26 | -(cosPsi * sinTheta * cosPhi + sinPsi * cosTheta * sinPhi) 27 | -(sinPsi * cosTheta * cosPhi - cosPsi * sinTheta * sinPhi)]; % must be conjugate 28 | end 29 | 30 | -------------------------------------------------------------------------------- /euler2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = euler2rotMat(phi, theta, psi) 2 | %EULER2ROTMAT Converts a ZYX Euler angle orientation to a rotation matrix 3 | % 4 | % q = euler2rotMat(axis, angle) 5 | % 6 | % Converts ZYX Euler angle orientation to a rotation matrix where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://x-io.co.uk/quaternions/ 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = cos(psi).*cos(theta); 16 | R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi); 17 | R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi); 18 | 19 | R(2,1,:) = sin(psi).*cos(theta); 20 | R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi); 21 | R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi); 22 | 23 | R(3,1,:) = -sin(theta); 24 | R(3,2,:) = cos(theta).*sin(phi); 25 | R(3,3,:) = cos(theta).*cos(phi); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /quatern2euler.m: -------------------------------------------------------------------------------- 1 | function euler = quatern2euler(q) 2 | %QUATERN2EULER Converts a quaternion orientation to ZYX Euler angles 3 | % 4 | % q = quatern2euler(q) 5 | % 6 | % Converts a quaternion orientation to ZYX Euler angles where phi is a 7 | % rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://x-io.co.uk/quaternions/ 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 16 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 17 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 18 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 19 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 20 | 21 | phi = atan2(R(3,2,:), R(3,3,:) ); 22 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 23 | psi = atan2(R(2,1,:), R(1,1,:) ); 24 | 25 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 26 | end 27 | 28 | -------------------------------------------------------------------------------- /quatern2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = quatern2rotMat(q) 2 | %QUATERN2ROTMAT Converts a quaternion orientation to a rotation matrix 3 | % 4 | % R = quatern2rotMat(q) 5 | % 6 | % Converts a quaternion orientation to a rotation matrix. 7 | % 8 | % For more information see: 9 | % http://x-io.co.uk/quaternions/ 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 15 | R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4)); 16 | R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3)); 17 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 18 | R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2; 19 | R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2)); 20 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 21 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 22 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 23 | end 24 | 25 | -------------------------------------------------------------------------------- /quaternConj.m: -------------------------------------------------------------------------------- 1 | function qConj = quaternConj(q) 2 | %QUATERN2ROTMAT Converts a quaternion to its conjugate 3 | % 4 | % qConj = quaternConj(q) 5 | % 6 | % Converts a quaternion to its conjugate. 7 | % 8 | % For more information see: 9 | % http://x-io.co.uk/quaternions/ 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | qConj = [q(:,1) -q(:,2) -q(:,3) -q(:,4)]; 15 | end 16 | -------------------------------------------------------------------------------- /quaternProd.m: -------------------------------------------------------------------------------- 1 | function ab = quaternProd(a, b) 2 | %QUATERNPROD Calculates the quaternion product 3 | % 4 | % ab = quaternProd(a, b) 5 | % 6 | % Calculates the quaternion product of quaternion a and b. 7 | % 8 | % For more information see: 9 | % http://x-io.co.uk/quaternions/ 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | ab(:,1) = a(:,1).*b(:,1)-a(:,2).*b(:,2)-a(:,3).*b(:,3)-a(:,4).*b(:,4); 15 | ab(:,2) = a(:,1).*b(:,2)+a(:,2).*b(:,1)+a(:,3).*b(:,4)-a(:,4).*b(:,3); 16 | ab(:,3) = a(:,1).*b(:,3)-a(:,2).*b(:,4)+a(:,3).*b(:,1)+a(:,4).*b(:,2); 17 | ab(:,4) = a(:,1).*b(:,4)+a(:,2).*b(:,3)-a(:,3).*b(:,2)+a(:,4).*b(:,1); 18 | end 19 | 20 | -------------------------------------------------------------------------------- /quaternRotate.m: -------------------------------------------------------------------------------- 1 | function v = quaternRotate(v, q) 2 | %QUATERNPROD Rotates a vector by a quaternion 3 | % 4 | % v = quaternRotate(v, q) 5 | % 6 | % Rotates the 3D vector v by a quaternion q. 7 | % 8 | % For more information see: 9 | % http://x-io.co.uk/quaternions/ 10 | % 11 | % Date Author Notes 12 | % 02/10/2011 SOH Madgwick Initial release 13 | 14 | [row col] = size(v); 15 | v0XYZ = quaternProd(quaternProd(q, [zeros(row, 1) v]), quaternConj(q)); 16 | v = v0XYZ(:, 2:4); 17 | end 18 | 19 | -------------------------------------------------------------------------------- /rotMat2euler.m: -------------------------------------------------------------------------------- 1 | function euler = rotMat2euler(R) 2 | %ROTMAT2EULER Converts a rotation matrix orientation to ZYX Euler angles 3 | % 4 | % euler = rotMat2euler(R) 5 | % 6 | % Converts a rotation matrix orientation to ZYX Euler angles where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://x-io.co.uk/quaternions/ 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | phi = atan2(R(3,2,:), R(3,3,:) ); 16 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 17 | psi = atan2(R(2,1,:), R(1,1,:) ); 18 | 19 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /rotMat2quatern.m: -------------------------------------------------------------------------------- 1 | function q = rotMat2quatern(R) 2 | %ROTMAT2QUATERN Converts a rotation matrix orientation to a quaternion 3 | % 4 | % q = axisAngle2quatern(axis, angle) 5 | % 6 | % Converts a rotation matrix orientation to a quaternion. 7 | % 8 | % For more information see: 9 | % http://x-io.co.uk/quaternions/ 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | [row col numR] = size(R); 15 | q = zeros(numR, 4); 16 | K = zeros(4,4); 17 | for i = 1:numR 18 | K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i)); 19 | K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i)); 20 | K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i)); 21 | K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i)); 22 | K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i)); 23 | K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i)); 24 | K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i)); 25 | K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i)); 26 | K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i)); 27 | K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i)); 28 | K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i)); 29 | K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i)); 30 | K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i)); 31 | K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i)); 32 | K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i)); 33 | K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i)); 34 | [V,D] = eig(K); 35 | %p = find(max(D)); 36 | %q = V(:,p)'; 37 | q(i,:) = V(:,4)'; 38 | q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)]; 39 | end 40 | end --------------------------------------------------------------------------------