寿光网站开发,厦门的服装商城网站建设,app开发网站建设公司,金沙江支付 WordPress文章目录 matlab里的旋转矩阵、四元数、欧拉角四元数根据两向量计算向量之间的旋转矩阵和四元数欧拉角转旋转矩阵旋转矩阵转欧拉角旋转矩阵转四元数参考链接 matlab里的旋转矩阵、四元数、欧拉角
旋转矩阵dcmR四元数quatq[q0,q1,q2,q3]欧拉角angle[row,pitch,yaw]
% 旋转矩阵… 文章目录 matlab里的旋转矩阵、四元数、欧拉角四元数根据两向量计算向量之间的旋转矩阵和四元数欧拉角转旋转矩阵旋转矩阵转欧拉角旋转矩阵转四元数参考链接 matlab里的旋转矩阵、四元数、欧拉角
旋转矩阵dcmR四元数quatq[q0,q1,q2,q3]欧拉角angle[row,pitch,yaw]
% 旋转矩阵转四元数
q dcm2quat(R)
% 欧拉角转四元数
q angle2quat(r1,r2,r3,S)
% 旋转矩阵转欧拉角
[r1,r2,r3] dcm2angle(R,S)
% 四元数转欧拉角
[r1,r2,r3] quat2angle([q0 q1 q2 q3],S)
% S是[ZYX,ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’]
% 四元数转旋转矩阵
R quat2dcm([q0 q1 q2 q3])
% 欧拉角转旋转矩阵
R angle2dcm(r1,r2,r3,S)四元数
% 四元数单元化
q1quatnormalize(q1);
% 四元数转为旋转矩阵q1是单元化四元数
R1quat2dcm(q1);%q1的第一位是实部
% 模Modulus
quatmod(p)
% 范数Norm
quatnorm(p)
% 单位化Normalize
quatnormalize(p)
% 求逆Inverse
quatinv(p)
% 四元数除法
quatdivide(q,p)
% 四元数乘法
quatmultiply(p,q)
% 共轭四元数
quatconj(p)
% 旋转函数
quatrotate(p)
% 四元数和欧拉角互换的函数
quat2angle(p)
angle2quat(p)根据两向量计算向量之间的旋转矩阵和四元数
function [R, q, theta] vec2quat_R(v1, v2)% 将向量转换为单位向量u1 v1/norm(v1);u2 v2/norm(v2);if norm(u1u2) 0q [0 0 0 0];elseu cross(u1,u2);u u/norm(u);theta acos(dot(u1,u2));q [cos(theta/2) sin(theta/2)*u];end% 四元数转为方向余弦矩阵dcm[2*q(1).^2-12*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-12*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-12*q(4)^2];% 四元数转为旋转矩阵rot permute(dcm, [2 1 3]);
end欧拉角转旋转矩阵
function R Eular2R(x,y,z,mode)Rotx [1 0 0;0 cos(x) -sin(x);0 sin(x) cos(x)];Roty [cos(y) 0 sin(y);0 1 0;-sin(y) 0 cos(y)];Rotz [cos(z) -sin(z) 0;sin(z) cos(z) 0;0 0 1];switch modecase 1 %ZYXR Rotz*Roty*Rotx;case 1 %XYZR Rotx*Roty*Rotz;case 1 %ZXYR Rotz*Rotx*Roty;case 1 %YZXR Roty*Rotz*Rotx;otherwiseR Rotz*Roty*Rotx;end 旋转矩阵转欧拉角
function eular R2eular(R)x atan2(R(3,2),R(3,3));y atan2(-R(3,1),sqrt(R(3,2)^2R(3,3)^2));z atan2(R(2,1),R(1,1));eular [x y z];旋转矩阵转四元数 function q R2quat(R)tsqrt(1R(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)];
end参考链接
Matlab ——旋转矩阵四元数欧拉角之间的转换
