MEMS惯性GPS信息融合技术的个人导航定位方法(5)
使用粗对准后的姿态角计算初始速度。由已知的航向角 ，俯仰角 ，横滚角 和相对载体系的初始速度 ，根据公式：
= （2.9）
计算得到由导航系到载体系的姿态转换矩阵，再根据
（2.10）
即可计算得到相对导航系的初始速度 。
实际设计中，用于陀螺仪的精度较低，使用磁传感器的信号通过投影求反三角函数对航向角进行解算。
2. 精对准
首先将粗对准后所得姿态角、速度及初始位置信息代入精对准中进行姿态解算、比力方程求解地理速度及位置解算，并读入加速度计及陀螺仪信号。然后卡尔曼滤波进行修正，以速度误差建立观测方程。
若GPS信号可用，以惯性测量单元的速度输出与GPS速度输出之差作为速度误差；若GPS信号不可用，由于精对准时，理论速度应为0，即以惯性测量单元的速度与理论速度之差作为观测量。建立速度误差的观测方程后，估计累积速度、位置误差，并在精对准之后强制校正。
由速度误差建立观测方程估计出数学平台误差角，由数学平台误差角进行计算实现对姿态角的校正。
捷联惯性导航系统中常用的坐标系有导航系n、机体系b和计算系c，三个坐标系之间的相互关系为：
（2.11）
在数学平台误差角为小量的条件下，近似表达式为：
（2.12）
其中， 是捷联惯性导航系统的数学平台误差角，且
（2.13）
式中， ， 和 分别为东北天三个方向的平台误差角。又有：
= （2.14）
则可计算得到修正后的姿态矩阵 ，进而得到修正后的姿态角信息[10]。
2.1.2. 捷联解算中的姿态解算
姿态矩阵的解算，即方向余弦矩阵的解算，主要完成三大功能：
1) 利用捷联陀螺仪测量载体相对于惯性空间的角速度 ，常采用四元数法解算姿态矩阵 ；
2) 从姿态阵的元素中提取载体的姿态信息 ， ， ；
3) 利用 把加速度计的输出 从载体坐标系变换到导航坐标系 ，即比力变换
（2.15）
在求得载体相对导航系的比力后，再扣除有害加速度，通过一次和两次积分可分别求得当前速度和位置[10]。其算法流程图如下图所示：
图2.2 姿态解算的算法流程
1．四元数算法解算姿态
四元数法是一种间接处理的捷联惯性导航系统姿态矩阵解算方法，用四元数的微分方程解算求解得到中间变量四元数，然后再转换为载体坐标系与地理坐标系间的方向余弦矩阵 。优点为可大大减少计算量，并且具有更好的计算性能[ ]。
四元数是一个由1个实数单位和3个虚数单位i，j，k构成的含有四个元的数，其复数定义形式为：
（2.16）
且定义规范化的四元数 为：
（2.17）