前言

文中算法公式摘自《捷联惯导算法与组合导航原理》（严恭敏、翁浚 编著）、《惯性导航》（秦永元 编著），其他理解仅代表个人观点。本文是对姿态角和姿态矩阵之间转化的理解。

---

一、定义

机体坐标系b系定义：
o飞机质心
x轴指向机头方向
z轴处在飞机对称面垂直x轴向下
y轴垂直oxz平面指向飞机右侧

导航系n系的定义：
xyz轴指向北东地（或东北天）方向

姿态角的定义（这里地面系指北东地坐标）：
俯仰角：机体系x轴与地平面夹角，俯仰角抬头为正
横滚角：机体系z轴与包含机体系x轴铅锤面的夹角，右滚为正
航向角：机体系x轴在地平面投影与地面系x轴的夹角，投影在地面系x轴右侧为正。

二、姿态角转化为姿态矩阵

导航系转化为机体系：向右旋转航向角、向上旋转俯仰角、向右旋转横滚角，该顺序旋转符合姿态角的定义。
先用北东地坐标系

转化到三维坐标

采用北东地坐标系时：先绕z轴转航向角，再绕y轴转俯仰角，最后绕x轴转横滚角

采用东北天坐标系时（机体系右前上，航向角北偏东为正时）：先绕z轴转航向角，再绕x轴转俯仰角，最后绕y轴转横滚角。
如果航向角北偏西为正，取负即可。

三、根据姿态矩阵求姿态角

根据姿态矩阵中的角度求解姿态角。
之前程序中由于俯仰角范围在-pi/2~pi/2 范围内，余弦值为正，其实并不完全合理。
根据余弦项可以求出俯仰角，可以知道俯仰角的正负，因此可以判断出横滚和航向角正余弦
的符号，根据四象限反正切，可以求出横滚和航向角。