MATLAB惯性系统定位误差事后校正技术研究仿真(4)
(2.1)
式中，ψ为航向角，θ为俯仰角，γ为横滚角。
常用的求解姿态阵的方法有欧拉角法、方向余弦法、四元数法。其中，四元数法只需要求解一个微分方程，相比其他方法计算量也较小，且能全姿态工作，目前被广泛用在SINS中。本论文选用四元数法进行姿态更新。
载体坐标系相对于导航坐标系的转动用四元数Q来表示，为：
(2.2)
式中， ， ， ， 为四个元素，i，j，k为导航坐标系的基。四元数Q与坐标变换阵的关系如下：
= (2.3)
由于n系至b系的旋转过程中坐标系始终是直角坐标系，所以 是正交矩阵，记 为
(2.4)
则有
(2.5)
由式(2.1)，式(2.5)可以得出姿态角：
(2.6)
SINS的姿态更新实质上就是如何计算四元数Q，目前四元数更新有毕卡逼近算法，等效旋转矢量法等算法。而等效旋转矢量法又分单子样，双子样，三子样，四子样等算法，毕卡求解法实质上是旋转矢量的单子样算法。由于旋转矢量算法的子样数越多，姿态计算的精度就越高，计算量也越大，越适合于高动态工作环境。考虑到地籍测量的低动态工作环境，姿态解算中采用毕卡逼近算法求解四元数微分方程。
记 (2.7)
其中， ， ， 为x，y，z陀螺在 采样时间间隔内的角增量（已经过位置速率及地球自转速率的补偿）。
记
则采用毕卡逼近算法求得的四元数更新方程如下：
(2.9)
2.3 SINS误差模型
导航系统的误差源有许多，其中主要有惯性仪表本身的误差，惯性仪表的安装误差和标度因数误差，系统的初始条件误差，系统的计算误差以及各种干扰引起的误差，这些误差都会影响系统的导航精度，要实现这些误差的补偿，就必须建立相应的误差模型。对于中低纬度地区工作的捷联惯导系统，为简化起见，一般选取地理坐标系作为导航坐标系。这里以“东北天（ENU）”坐标系为系统的地理坐标系，平台误差角为 、 、 ，速度误差为 、 、 ，位置误差为 、 、 （分别为纬度误差、经度误差、高度误差），地球自转角速度为 ，椭球赤道平均半径为Re=6378137m，卯酉圈曲率半径为 ,子午圈曲率半径为 ，椭圆度f=1/298.257；东北天轴上的速度分别为 、 、 ，纬度、经度、高度分别为 、 、 ，东北天方向上的等效陀螺漂移分别为 、 、 ，等效加速度计误差分别为 、 、 。忽略陀螺和加速度计的安装误差和标度因数误差。
SINS的姿态角误差方程： (2.10c)
SINS的速度误差方程： (2.11c)