初始对准得到载体的姿态角后,由欧拉角得到四元数初始值:Р其中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)
全文预览
