卡尔曼滤波实验报告
捷联惯导静基座初始对准实验
一、实验目的
① 掌握捷联惯导的构成和基本工作原理； ② 掌握捷联惯导静基座对准的基本工作原理；
③ 了解捷联惯导静基座对准时的每个系统状态的可观测性； ④ 了解双位置对准时系统状态的可观测性的变化。
二、实验原理
选取状态变量为：[]T E
N E N U x y x y z X V V δδεεε=ψψψ∇∇，其
中导航坐标系选为东北天坐标系，E V δ为东向速度误差，N V δ为北向速度误差， E ψ为东
向姿态误差角 ，N ψ为北向姿态误差角， U ψ为天向姿态误差角， x ∇为东向加速度偏置，y ∇为北向加速度偏置，x ε为东向陀螺漂移，y ε为北向陀螺漂移，z ε为天向陀螺漂移。则
系统的状态模型为：
X AX W =+
(1)
其中，
1112212211
12
1321222331323302sin 000
002sin 000000000sin cos 0000sin 000000cos 0000000000000000000000000000000000000000000000000000
0L g C C L g C C L L C C C L C C C L C C C A Ω-⎡
⎤⎢⎥-Ω⎢⎥⎢⎥Ω-Ω⎢
⎥-Ω⎢⎥⎢⎥Ω=⎢
⎥⎢⎥⎢⎥⎢
⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣
⎦
[00000]E N E N U
T V V W W W W W W δδψψψ=，E
D V
W W δψ为零均值高斯
白噪声，分别为加速度计误差和陀螺漂移的噪声成分，Ω为地球自转角速度，ij C 为姿态矩阵n
b C 中的元素，L 为当地纬度。
量测量选取两个水平速度误差：[ ]T
E N Z V V δδ=，则量测方程为：
10000000000100000000E E N N V X V δηδη⎡⎤⎡⎤⎡⎤=+⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦
(2)
即Z HX η=+
其中，H 为量测矩阵，[]T
E
N ηηη=为量测方程的随机噪声状态矢量，为零均值高
斯白噪声。
要利用基本卡尔曼滤波方程进行状态估计，需要将状态方程和量测方程进行离散化。 系统转移矩阵为：
2323
/1111102!3!!
n n
k k k k k k n T T T I TA A A A n ∞
-----=Φ=++++
=∑ (3)
其中，T 为采样间隔。 量测矩阵为：
10000000000100000000k H ⎡⎤
=⎢⎥
⎣⎦
(4)
所以经过离散化，系统的数学模型为：
,1111
k k k k k k k k k k X X W Z H X V ----=Φ+Γ⎧⎨
=+⎩
(5)
其中，系统噪声驱动阵1k I -Γ=。
有了状态方程和量测方程，可以采用以下滤波方程进行卡尔曼滤波：
/11,1/1,11,11111
/1/1/1/1/1()
()()()k k k k k T T k k k k k k k k k k T T k k k k k k k k k k k k k k k k k T T k k k k k k k k k k
X X P P Q K P H H P H R X X K Z H X P I K H P I K H K R K φφφΛ
Λ
-------------ΛΛΛ
---⎧=⎪⎪=+ΓΓ⎪⎪⎪=+⎨⎪⎪=+-⎪
⎪=--+⎪⎩
(6)
滤波方程中所用的量测值k Z ——速度误差，可由捷联惯导采用角增量法解算。
三、静基座初始对准流程
陀螺加计测量数据
解析粗对准
开始
捷联惯导解算
卡尔曼滤波
对准完毕
结束
N
Y
修正
图1 静基座初始对准流程图
四、实验结果及分析
4.1 仿真数据单位置初始对准 仿真条件：
陀螺常值漂移:0.02°/h ； 陀螺随机漂移:0.01°//h ； 加速度计常值偏置:100ug ； 加速度计随机误差:50ug ； 初始失准角ΨN ，ΨE ，ΨD: 1° 惯导所处位置的地理纬度： L = 45° 仿真结果：
时间/s
速度误差/m /s
时间/s
速度误差/m /s
-3
时间/s
速度误差/m /s
时间/s
速度误差/m /
s
东向水平失准角
时间/s
误差角/d e g
东向水平失准角均方差
时间/s
误差角/d e
g
北向水平失准角
时间/s
误差角/d e g
北向水平失准角均方差
时间/s
误差角/d e
g
时间/s
误差角/d e g
时间/s
误差角/d e g
时间/s
偏置/μg
-4
时间/s
偏置/μg
时间/s
偏置/μg
-4
时间/s
偏置/μg
