MATLAB惯性系统定位误差事后校正技术研究仿真(9)
对于导航坐标系取地理坐标系的情况下，地球速度为 ，位置速度为 。
9）姿态速度的计算
姿态速度是指载体坐标系相对导航坐标系转动的角速度 ，可以通过下式进行计算：
(3.8)
式中 ：惯导系统中陀螺仪直接输出的值；
：由经过姿态更新后的最新值确定。
10）姿态角的计算
通过如下顺序的三次旋转如下图，可得到导航坐标系至载体坐标的转换矩阵 。
图3.2 导航坐标系与载体坐标系转换关系示意图
由式(2.1)、式(2.4)、式(2.5)、式(2.6)就可以实时计算出载体的三文姿态信息。其中航向角 和横滚角 的真值按表3.1和表3.2确定。
T22 T12 Ψ
→0 + 90°
→0 - -90°
+ + Ψ主
+ - Ψ主
- + Ψ主+180°
- - Ψ主-180°
表3.1 航向角Ψ真值表
γ主 T33 γ
+ + γ主
-
+ - γ主
- - γ主
表3.2 横滚角γ真值表
当求航向/横滚角时，当反正切函数的自变量的分子或分母趋近于零时将会产生较大误差，甚至导致计算溢出。特别是当俯仰角 时，载体的3个转动自由度将退化为2个自由度，此时姿态角的定义和计算方法应进行相应的变化，以实现惯导系统全姿态工作。
11）高度计算
惯导系统高度通道一般要引入外部信息进行阻尼，再得到高度值。
12）重力加速度的计算
重力加速度与高度、纬度的变化关系由下式表示：
(3.9)
3.4 GPS/SINS组合导航算法的设计
卡尔曼滤波算法是进行GPS/SINS组合导航解算的关键，因此本节主要对卡尔曼滤波算法进行介绍。实现卡尔曼滤波算法的核心环节是确定描述系统动力学特性的“状态方程”和“量测方程”，状态方程包括连续状态系统方程和离散状态系统方程。
1）连续状态系统方程