MEMS惯性GPS信息融合技术的个人导航定位方法(6)
四元数提供了从一个坐标系到另一个坐标系的数学关系。将普通的3文空间矢量扩展为4文，如地理坐标系3文矢量 和载体坐标系的3文矢量 可表示为：
（2.18）
经推导后即可得到四元数与方向余弦矩阵的关系为：
（2.19）
其中 ， 和 分别为航向角、俯仰角和横滚角，从导航系到载体系的姿态转换矩阵为：
（2.20）
四元数矩阵为：
= （2.21）
由地理系到载体系的变换四元数为：
（2.22）
由上一时刻的姿态角信息（横滚角、俯仰角、航向角）计算出 和 。然后由陀螺仪的输出 求出 ，公式为：
（2.23）
再由四元数运动学微分方程求解，即设 矢量为四元数形式，表示载体坐标系相对地理坐标系的角速度在载体坐标系的投影，其与 对应的四元数 具有微分方程关系：
（2.24）
可用毕卡逼近法计算得：
（2.25）
在对指数积分式取近似的情况下，定义：
（2.26）
代入可得到四元数的解析表达式：
（2.27）
计算得到四元数矩阵 ，即可由上述公式得到当前时刻的姿态矩阵 ，进而计算得到当前时刻的姿态角[ ]。
2．等效转动矢量算法
四元数法求解姿态矩阵中用到了角速度矢量的积分，即：
（2.28）
根据刚体转动的特性，当刚体运动不是定轴转动时，若采用角速度矢量积分，计算会产生误差，称为转动不可交换性误差（又称圆锥误差）。旋转矢量表示为：
（2.29）
它可以唯一确定刚体在t时刻的运动姿态，即以 为轴，转动角度大小等于旋转矢量 的幅值 。当以旋转矢量描述机体运动姿态时，旋转矢量微分方程可以表示为：
（2.30）
式中， 是载体角速度矢量。考虑到旋转矢量很小，对旋转矢量微分方程取二阶近似，则为：
（2.31）
忽略高次项，则可进一步化简为：
（2.32）
由上式计算所得的 代替 对四元数 进行修正后再计算得到姿态矩阵 和姿态角信息，即航向角 、俯仰角 和横滚角 ，并对航向角 和横滚角 进行象限判断[10]。
3．比力方程
加速度计测量的是载体相对惯性空间的绝对加速度和引力加速度之差，称作“比力”，即为作用在单位质量上的外力。载体相对地球运动，地球又相对惯性空间运动。因此，对地球表面的运动载体而言，由加速度计输出的比力表示了载体相对惯性系的非引力加速度；而对于在地球表面导航的载体，须知道载体相对于地球系的加速度。