matlab練習程序（旋轉矩陣、歐拉角、四元數互轉）

歐拉角轉旋轉矩陣公式：

旋轉矩陣轉歐拉角公式：

旋轉矩陣轉四元數公式，其中1+r11+r22+r33>0：

四元數轉旋轉矩陣公式，q0^2+q1^2+q2^2+q3^2=1：

歐拉角轉四元數公式：

四元數轉歐拉角公式：

matlab代碼以下：

clear all;
close all;
clc;

%歐拉角
x = 0.5;
y = 0.6;
z = 0.7;
Ang1 = [x y z];

%歐拉角轉旋轉矩陣
Rx = [1      0      0;
    0 cos(x) -sin(x);
    0 sin(x) cos(x)];
Ry = [cos(y)  0 sin(y);
    0       1      0;
    -sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
    sin(z) cos(z)  0;
    0      0       1];
R = Rz*Ry*Rx;
R1 = R;

%旋轉矩陣轉歐拉角
x = atan2(R(3,2),R(3,3));
y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
z = atan2(R(2,1),R(1,1));
Ang2 = [x y z];

%旋轉矩陣轉四元數
t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
Q1 = q;

%四元數轉旋轉矩陣
R=[ 2*q(1).^2-1+2*q(2)^2    2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
    2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2     2*(q(3)*q(4)-q(1)*q(2));
    2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
R2 = R;

%歐拉角轉四元數
q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
    sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
    cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
    cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
Q2 = q;

%四元數轉歐拉角
x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
y = asin(2*(q(1)*q(3) - q(2)*q(4)));
z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
Ang3 = [x y z];

Ang1
Ang2
Ang3

R1
R2

Q1
Q2

參考：

http://www.javashuo.com/article/p-peclkifa-gw.html

http://www.javashuo.com/article/p-tyctgbim-bm.html