捷联式姿态解算过程

（1）加速度记测得载体相对惯性空间比力b ib a ，经过误差补偿之后的b
ib a 经过姿态矩阵n b C 的变换得到n ib a ：n n b ib b ib =C a a
将n ib a 进行误差补偿之后通过积分运算得到速度分量n en V 。

n en V 一方面作
为系统的输出，一方面作为输入用来求解位置角速率n en W 。

（2）陀螺仪测得载体相对惯性空间角速率b i b W ， 首先通过速度分量n en V 求得位移角速率n en W ，因为n eny W =0，
n
enx n eny W W ⎡⎤⎢⎥⎢⎥⎣⎦=22yt xt 22y yt xt yt t x x t t sin cos ()R R sin cos R 11sin cos R R 11sin cos R R R αααααααα⎡⎤
-⎢⎥⎢⎥⎢⎥⎢⎥
-⎢⎥⎣-++⎦
-（）（）n x n y V V ⎡⎤
⎢⎥⎢⎥⎣⎦
这样就根据陀螺仪测得的角速率b i b W ，上面所求得的位移角速率n
en W ，
加上已知的地球角速率e i e W 来求的姿态角速率b n b W 。

b b n b n e i e n i e n e i e W =C W =C C W 且 b b n e n n e n W =C W b b b b n b i b i e e n W =W W W --=b i b W b n e n n e i e e n C C W +W -（）。

然后根据姿态角速率利用四元数微分方程求出四元数中的元素a,b,c,d.
四元数描述了一个坐标系或一个矢量相对某一个坐标系的旋转。

a 是标量部分表示了转角的一半余弦值，b ，c ，d 是矢量部分表示瞬时转轴的方向，瞬时转动轴与参考坐标系轴间方向的余弦值。

表达式A=a+bi+cj+dk 。

四元数微分方程：.
b
n b A =1/2AW
..x
..001/200b
b b
nbx nby
nbz
b b b nb nbz
nby
b b b nby
nbz
nbx
b b b nbz nby
nbz
a w w w a w w w
b b w w w
c c w w w
d d ⎡⎤⎢⎥⎡⎤---⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦
⎣⎦⎢⎥⎣⎦
这样求出四元数中a ，b ，c ，d 四个元数。

这里的四元数是用来描述载体坐标系相对游动方位坐标系的转动，即：
2222
b n 2222
n b 2222n b x x a b c d 2bc ad 2bd+ac y =2bc+ad a b c d 2cd-ab y 2bd ab 2cd+ab a -b -c d z z ⎡⎤+---⎡⎤
⎡⎤⎢⎥⎢⎥⎢⎥-+-⎢⎥⎢⎥⎢⎥⎢⎥⎢
⎥⎢⎥-+⎣⎦⎣⎦⎣⎦（）
（）（
）（）（
）（）
这样就可方向余弦矩阵与四元数姿态矩阵是完全等效的，即
2222
n 2222
b 2222a b
c
d 2bc ad 2bd+ac C =2bc+ad a b c d 2cd-ab 2bd ab 2cd+ab a -b -c d ⎡⎤
+---⎢⎥
-+-⎢
⎥⎢⎥-+⎣⎦
（）（）（）（）（
）
（）
而n b cos sin sin cos sin cos cos sin sin cos sin s sin sin sin C =sin sin in cos cos sin sin cos sin cos co sin s sin cos cos γψγθψ
θψγψγθψγψγθψθψγψγθψγθθ
γθ-⎡⎤
⎢⎥⎢⎥⎢⎥-⎣
+⎦
-+-
这样利用姿
态矩阵和四元数中各对应项相等的原则得出姿态矩阵中的各个元素，然后就能求出姿态参数，即三个姿态角航向角ψ、俯仰角θ、滚转角γ。