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 = [            ;

     cos(x) -sin(x);

     sin(x) cos(x)];

Ry = [cos(y)   sin(y);

                 ;

    -sin(y)  cos(y)];

Rz = [cos(z) -sin(z) ;

    sin(z) cos(z)  ;

                 ];

R = Rz*Ry*Rx;

R1 = R;

%旋转矩阵转欧拉角

x = atan2(R(,),R(,));

y = atan2(-R(,),sqrt(R(,)^+R(,)^));

z = atan2(R(,),R(,));

Ang2 = [x y z];

%旋转矩阵转四元数

t=sqrt(+R(,)+R(,)+R(,))/;

q=[t (R(,)-R(,))/(*t) (R(,)-R(,))/(*t) (R(,)-R(,))/(*t)];

Q1 = q;

%四元数转旋转矩阵

R=[ *q().^-+*q()^    *(q()*q()-q()*q()) *(q()*q()+q()*q());

    *(q()*q()+q()*q()) *q()^-+*q()^     *(q()*q()-q()*q());

    *(q()*q()-q()*q()) *(q()*q()+q()*q()) *q()^-+*q()^];

R2 = R;

%欧拉角转四元数

q = [cos(x/)*cos(y/)*cos(z/) + sin(x/)*sin(y/)*sin(z/) ...

    sin(x/)*cos(y/)*cos(z/) - cos(x/)*sin(y/)*sin(z/) ...

    cos(x/)*sin(y/)*cos(z/) + sin(x/)*cos(y/)*sin(z/) ...

    cos(x/)*cos(y/)*sin(z/) - sin(x/)*sin(y/)*cos(z/)];

Q2 = q;

%四元数转欧拉角

x = atan2(*(q()*q()+q()*q()), - *(q()^+q()^));

y = asin(*(q()*q() - q()*q()));

z = atan2(*(q()*q()+q()*q()), - *(q()^+q()^));

Ang3 = [x y z];

Ang1

Ang2

Ang3

R1

R2

Q1

Q2

参考：

https://blog.csdn.net/lql0716/article/details/72597719

https://www.cnblogs.com/21207-iHome/p/6894128.html

巴特西

matlab练习程序（旋转矩阵、欧拉角、四元数互转）

最新文章

热门文章