MATLAB旋转矩阵、欧拉⾓、四元数互转欧拉⾓转旋转矩阵公式： 旋转矩阵转欧拉⾓公式：
旋转矩阵转四元数公式，其中1+r11+r22+r33>0：
四元数转旋转矩阵公式，q0^2+q1^2+q2^2+q3^2=1：
欧拉⾓转四元数公式：
四元数转欧拉⾓公式：
假处分
matlab代码如下：
1 clear all;
2 close all;
3 clc;
4
5 %欧拉⾓
6 x = 0.5;
7 y = 0.6;
8 z = 0.7;
9 Ang1 = [x y z];
10
11 %欧拉⾓转旋转矩阵
12 Rx = [100;
130 cos(x) -sin(x);
140 sin(x) cos(x)];中文考试
15 Ry = [cos(y) 0 sin(y);
16010;
17 -sin(y) 0 cos(y)];
18 Rz = [cos(z) -sin(z) 0;
19 sin(z) cos(z) 0;
20001];
21 R = Rz*Ry*Rx;
22 R1 = R;
23
24 %旋转矩阵转欧拉⾓
25 x = atan2(R(3,2),R(3,3));军事学院
26 y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
27 z = atan2(R(2,1),R(1,1));
28 Ang2 = [x y z];
29
30 %旋转矩阵转四元数
31 t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
32 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)];
33 Q1 = q;
34
35 %四元数转旋转矩阵
36 R=[ 2*q(1).^2-1+2*q(2)^22*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3)); 372*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^22*(q(3)*q(4)-q(1)*q(2)); 382*(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]; 39 R2 = R;
40
41 %欧拉⾓转四元数
42 q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...断章 赏析
43 sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
44 cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
45 cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
46 Q2 = q;
47
48 %四元数转欧拉⾓
49 x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
50 y = asin(2*(q(1)*q(3) - q(2)*q(4)));
51 z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
52 Ang3 = [x y z];
54 Ang1
55 Ang2
56 Ang3 57
58 R1
氰酸酯树脂
59 R2 60
61 Q1
62 Q2
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议