旋转矩阵和四元数互转

% rotationMatrix_to_quaternion.m
clear all;close all;clear;clc;

%欧拉角
roll = pi/2;
pitch = pi/3;
yaw = pi/4;
angle_xyz = [roll pitch yaw];
angle_zyx = [yaw pitch roll];

%欧拉角转旋转矩阵
rMX = [1      0      0;
    0 cos(roll) -sin(roll);
    0 sin(roll) cos(roll)];

rMY = [cos(pitch)  0 sin(pitch);
    0       1      0;
    -sin(pitch) 0 cos(pitch)];

rMZ = [cos(yaw) -sin(yaw) 0;
    sin(yaw) cos(yaw)  0;
    0      0       1];


rotationMatrix_zyx = rMZ*rMY*rMX;
rotationMatrix_xyz = rMX*rMY*rMZ;


%旋转矩阵转四元数
t=sqrt(1+rotationMatrix_zyx(1,1)+rotationMatrix_zyx(2,2)+rotationMatrix_zyx(3,3))/2;
quat_zyx_1=[t (rotationMatrix_zyx(3,2)-rotationMatrix_zyx(2,3))/(4*t) (rotationMatrix_zyx(1,3)-rotationMatrix_zyx(3,1))/(4*t) (rotationMatrix_zyx(2,1)-rotationMatrix_zyx(1,2))/(4*t)];

quat_zyx_2 = rotm2quat(rotationMatrix_zyx);
assert(max(max(quat_zyx_1-quat_zyx_2))<1.0e-15)


rotationMatrix_zyx_1 = quat2rotm(quat_zyx_2);
assert(max(max(rotationMatrix_zyx-rotationMatrix_zyx_1))<1.0e-15)