欧拉角，四元数，方向余弦矩阵三者相互转换的matlab函数代码

以下是欧拉角、四元数、方向余弦矩阵三者相互转换的 MATLAB 函数代码：

欧拉角转方向余弦矩阵：

function dcm = euler2dcm(euler)
% euler: 3x1欧拉角（单位: rad）
% dcm: 3x3方向余弦矩阵

phi = euler(1);
theta = euler(2);
psi = euler(3);

cphi = cos(phi);
sphi = sin(phi);
cth = cos(theta);
sth = sin(theta);
cpsi = cos(psi);
spsi = sin(psi);

dcm = [cth*cpsi, cth*spsi, -sth;
       sphi*sth*cpsi-cphi*spsi, sphi*sth*spsi+cphi*cpsi, sphi*cth;
       cphi*sth*cpsi+sphi*spsi, cphi*sth*spsi-sphi*cpsi, cphi*cth];
end

方向余弦矩阵转欧拉角：

function euler = dcm2euler(dcm)
% dcm: 3x3方向余弦矩阵
% euler: 3x1欧拉角（单位: rad）

phi = atan2(dcm(3,2), dcm(3,3));
theta = asin(-dcm(3,1));
psi = atan2(dcm(2,1), dcm(1,1));

euler = [phi; theta; psi];
end

四元数转方向余弦矩阵：

function dcm = quat2dcm(quat)
% quat: 4x1四元数
% dcm: 3x3方向余弦矩阵

q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);

dcm = [1-2*q2^2-2*q3^2, 2*q1*q2-2*q0*q3, 2*q0*q2+2*q1*q3;
       2*q1*q2+2*q0*q3, 1-2*q1^2-2*q3^2, 2*q2*q3-2*q0*q1;
       2*q1*q3-2*q0*q2, 2*q0*q1+2*q2*q3, 1-2*q1^2-2*q2^2];
end

方向余弦矩阵转四元数：

function quat = dcm2quat(dcm)
% dcm: 3x3方向余弦矩阵
% quat: 4x1四元数

q0 = 0.5*sqrt(1+dcm(1,1)+dcm(2,2)+dcm(3,3));
q1 = (dcm(3,2)-dcm(2,3))/(4*q0);
q2 = (dcm(1,3)-dcm(3,1))/(4*q0);
q3 = (dcm(2,1)-dcm(1,2))/(4*q0);

quat = [q0; q1; q2; q3];
end