旋转矩阵，四元素，欧拉角转换

Rot_to_EulerZYX：旋转矩阵转欧拉角

function [ euler ] = Rot_to_EulerZYX( R )
% rotation matrix --> [roll, pitch, yaw] 
pitch = asin(-R(3,1));
if (abs(pitch - pi/2) < 1.0e-3) 
    roll = 0.0;
    yaw = atan2(R(2,3) - R(1,2), R(1,3) + R(2,2)) + roll;
elseif (abs(pitch + pi/2) < 1.0e-3) 
    roll = 0.0;
    yaw = atan2(R(2,3) - R(1,2), R(1,3) + R(2,2)) - roll;
else 
    roll = atan2(R(3,2), R(3,3));
    yaw = atan2(R(2,1), R(1,1));
end

euler = [roll;pitch;yaw];
end

quat_to_EulerZYX: 四元素转欧拉角

function [euler] = quat_to_EulerZYX(quat)
% [qx qy qz qw] --> [roll, pitch, yaw] 

	qx = quat(1);
	qy = quat(2);
	qz = quat(3);
    qw = quat(4);
	
	roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy));
	pitch = asin(2*(qw*qy-qz*qx));
	yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz));

    euler = [roll, pitch, yaw];
end

EulerZYX_to_quat：欧拉角转四元素

function quat = EulerZYX_to_quat(euler_angle)
% [roll, pitch, yaw] -->  [qx qy qz qw]

	roll = euler_angle(1);
	pitch = euler_angle(2);
	yaw = euler_angle(3);
	sy = sin(yaw*0.5);
	cy = cos(yaw*0.5);
	sp = sin(pitch*0.5);
	cp = cos(pitch*0.5);
	sr = sin(roll*0.5);
	cr = cos(roll*0.5);
	w = cr*cp*cy + sr*sp*sy;
	x = sr*cp*cy - cr*sp*sy;
	y = cr*sp*cy + sr*cp*sy;
	z = cr*cp*sy - sr*sp*cy;
    quat=[x y z w];
end

EulerZYX_to_Rot： 欧拉角转旋转矩阵

function R = EulerZYX_to_Rot(euler_angle)
% [roll, pitch, yaw] -->  rotation matrix

roll = euler_angle(1);
pitch = euler_angle(2);
yaw = euler_angle(3);

cp = cos(pitch);
sp = sin(pitch);
sr = sin(roll);
cr = cos(roll);
sy = sin(yaw);
cy = cos(yaw);

R = [cp * cy    (sr * sp * cy) - (cr * sy)      (cr * sp * cy) + (sr * sy);
     cp * sy    (sr * sp * sy) + (cr * cy)      (cr * sp * sy) - (sr * cy);
     -sp        sr * cp                         cr * cp];

quat_to_Rat:四元素转旋转矩阵

function rot = quat_to_Rot(quat)
% [qx qy qz qw] --> rotation matrix
	
	qx = quat(1);
	qy = quat(2);
	qz = quat(3);
    qw = quat(4);
    
    rot=[1 - 2*qy^2 - 2*qz^2,	2*qx*qy - 2*qz*qw,	2*qx*qz + 2*qy*qw;
         2*qx*qy + 2*qz*qw,	1 - 2*qx^2 - 2*qz^2,	2*qy*qz - 2*qx*qw;
         2*qx*qz - 2*qy*qw,	2*qy*qz + 2*qx*qw,	1 - 2*qx^2 - 2*qy^2];

end

Rot_to_quat: 旋转矩阵转四元素

function [ quat ] = Rot_to_quat( R )
% rotation matrix --> [qx qy qz qw]

% for corner case, see  http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/

    quat = zeros(1,4);
    quat(4) = 0.5 * sqrt(1 + R(1,1) + R(2,2) + R(3,3));
    quat(1) = (R(3,2)-R(2,3))/(4*quat(4));
    quat(2) = (R(1,3)-R(3,1))/(4*quat(4));
    quat(3) = (R(2,1)-R(1,2))/(4*quat(4));
   
end

反对称矩阵

function [ cp ] = skew_matrix( vec )
% skew matrix for vector3

cp = [0         -vec(3) vec(2);
      vec(3)    0       -vec(1);
      -vec(2)   vec(1)  0];

end