北航卡尔曼滤波课程-捷联惯导静基座初始对准实验

卡尔曼滤波实验报告

捷联惯导静基座初始对准实验

一、实验目的

① 掌握捷联惯导的构成和基本工作原理； ② 掌握捷联惯导静基座对准的基本工作原理；

③ 了解捷联惯导静基座对准时的每个系统状态的可观测性； ④ 了解双位置对准时系统状态的可观测性的变化。

二、实验原理

选取状态变量为：[]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