姿态解算公式

最近在学习姿态分解，总结出下面的公式：
这里对于什么是姿态分解不做分析，只总结公式：
1.右手坐标系，左乘矩阵，ZXY顺规
绕Z轴旋转 φ

绕X轴旋转 θ

绕Y轴旋转γ

由于矩阵为左乘矩阵，则整体旋转矩阵为
R = RyRxRz =

四元数法姿态解算 ：
1.初始化四元数;
ZXY顺规
由四元数表示空间中一个向量绕某轴旋转一定的角度
绕Z轴旋转φ角
绕X轴旋转θ角

绕Y轴旋转γ角

则该向量在空间中的旋转可以表示为：

化简得到：

利用四元数表示旋转矩阵：