psi_tr(jk)=atan2( 2*( q(2)*q(3) + q(1)*q(4) ) , ( 1-2*q(3)*q(3)-2*q(4)*q(4) ) );
theta_tr(jk)=asin( 2*( q(1)*q(3) - q(2)*q(4) ) );
gamma_tr(jk)=atan2( 2*( q(3)*q(4) + q(1)*q(2) ) , 1-2*q(2)*q(2)-2*q(3)*q(3) );
以下内容来自 http://blog.csdn.net/u012763833/article/details/50650995
四元数、欧拉角及方向余弦矩阵的相互转换公式
1. 欧拉角转四元数(常用来初始化四元数)
按Z-Y-X的旋转变换顺序有:
2. 四元数与旋转矩阵(常用来作坐标变换)
1.b系到R系的坐标变换矩阵
2.R系至b系的坐标变换矩阵公式
详情:http://blog.csdn.net/u012763833/article/details/52583350
3. 欧拉角转方向余弦矩阵
由以上两式可得,经三次基本旋转对应的坐标变换为:
从上述方向余弦矩阵中可方便提取欧拉角
4. 四元数转欧拉角