惯导解算步骤

1.初始对准得到载体的姿态角后，由欧拉角得到四元数初始值：
其中q=a+bi+cj+dk，[a,b,c,d]也即[q0,q1,q2,q3]。

2.由四元数初始值构造姿态转移矩阵n
c——载体坐标系b系到导航
b
坐标系（也即地理坐标系）n系的转移矩阵
其中，俯仰角θ，滚转角φ，航向角ψ
3.由n b C求出e b C
n b e n e b C C C =
其中：
其中，ϕ和λ分别为纬度和经度。

4. 由e
b C 反求e 系下四元数的初始值
[a,b,c,d]也即[q0,q1,q2,q3]。

5. 由四元数按时间传递的特性：
也即：
⎥⎥
⎥⎥⎦
⎤
⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢
⎢
⎣⎡------=⎥
⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡32103210000021q q q q q q q q x
y
z
x z
y
y z
x z y
x ωωωωωωωωωωωω （2-24） 利用毕卡逼近法求解可得：
()()()*1
2
0M dt q t e q t ω⎰
= （2-25）
令：
[]()*0000x y z x
z y y z x z
y
x
M dt θθθθ
θθθωθθθθθθ-∆-∆-∆⎡⎤
⎢⎥∆∆-∆⎢
⎥∆==⎢⎥
∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦
⎰ （2-26） （2-25）可简写为：
()[]()1
2
0q t e q θ∆= （2-27）
将[]1
2
e
θ∆展开可得：
()[][][][]()23
1111022!23!2!2n
q t I q n θθθθ⎡⎤
⎛⎫⎛⎫⎛⎫
∆∆∆⎢⎥=+∆+++
++ ⎪ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎝⎭
⎣⎦
（2-28） 由于：
[][][]
[]2
24
2
222
03
04
0x y z I I
I
θθθθθθθθθθ⎡⎤∆==-∆+∆+∆=-∆⎣⎦∆=-∆∆∆=-∆ （2-29） 将式（2-29）代入式（2-28）整理可得：
[]000sin 2()cos (0)2q t I q θθθθ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭
（2-30） 在实际解算中，把0cos 2θ∆和0sin 2
θ
∆展为级数形式并取有限项，得四元
数的各阶近似算法。

一阶算法：
[]1
(1)()2
q n I q n θ⎧⎫
+=+∆⎨⎬⎩
⎭
（2-31）
二阶算法：
()[]2
01(1)1()82q n I q n θθ⎧⎫⎛⎫∆⎪⎪
+=-+∆ ⎪⎨⎬ ⎪⎪⎪⎝⎭⎩⎭
（2-32）
若取一阶算法则有：
其中，角增量△θ：
其中的b e R 即b e C ，b e C =T e b
C )(
2．卡尔曼滤波
连续系统：
Z(t)=HX(t)+V(t) 离散系统：
w~N(0,Q) , v~N(0,R)。