惯导解算步骤.docx

初始对准得到载体的姿态角后，由欧拉角得到四元数初始值：其中q=a+bi+cj+dk，[a,b,c,d]也即[q0,q1,q2,q3]。由四元数初始值构造姿态转移矩阵——载体坐标系b系到导航坐标系（也即地理坐标系）n系的转移矩阵其中，俯仰角，滚转角，航向角由求出其中：其中，和分别为纬度和经度。由反求e系下四元数的初始值[a,b,c,d]也即[q0,q1,q2,q3]。由四元数按时间传递的特性：也即：（2-24）利用毕卡逼近法求解可得：（2-25）令：（2-26）（2-25）可简写为：（2-27）将展开可得：（2-28）由于：（2-29）将式（2-29）代入式（2-28）整理可得：（2-30）在实际解算中，把和展为级数形式并取有限项，得四元数的各阶近似算法。一阶算法：（2-31）二阶算法：（2-32）若取一阶算法则有：其中，角增量△θ：其中的即，=2．卡尔曼滤波连续系统：Z(t)=HX(t)+V(t)离散系统：w~N(0,Q) , v~N(0,R)