卡尔曼滤波与组合导航课程实验报告

⑨ 准备过程中由实验教师介绍捷联惯导 /GPS 组合实验系统的组成、工作原理；
2） 捷联惯导 /GPS 组合导航系统静态导航实验 ① 实验系统准备 5 分钟之后， 通过监控软件， 将实验系统设置为 “组合导航” 状态； ② 记录 IMU 的原始输出，即角增量和比力信息； ③ 记录数据过程中， 由实验教师介绍采用卡尔曼滤波方法进行捷联惯导 /GPS 组合导 航的基本原理； ④ 记录 IMU 数据 5 分钟后，停止记录数据，利用监控计算机中的捷联惯导 /GPS 组 合导航软件进行静态导航解算，并显示静态导航结果；
《卡尔曼滤波与组合导航》课程实验报告
实验 姓名
日期
捷联惯导 /GPS 组合导航系统静态导航实验
陈星宇
系院专业
17
班级
ZY11172
2012-5-15
指导教师
宫晓琳
实验序号 学号 成绩
3 ZY1117212
一、实验目的
① 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； ② 掌握采用卡尔曼滤波方法进行捷联惯导 /GPS 组合的基本原理； ③ 掌握捷联惯导 /GPS 组合导航系统静态性能； ④了解捷联惯导 /GPS 组合导航静态时的系统状态可观测性；
03 6 ，
T
H V 03 3 diag 1, 1, 1 03 9 ， v v VE v VN v VU v L v v H 为量测噪声。 量测噪声方
差阵 R 根据 GPS 的位置、速度噪声水平选取。
（ 3） 卡尔曼滤波方程
状态一步预测： X?k /k 1
k,k 1 X?k 1
状态估计： X?k X?k /k 1 K k (Zk H k X?k/ k 1 )
机常值漂移和加速度计随机常值零偏。（下标
E、 N、 U 分别代表东、北、天）
系统的噪声转移矩阵 G 为：
C
n b
03 3
G
03 3
C
n b
09 3 09 3 15 6
系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：
T
ww w w w w w
x
y
z
x
y
z
系统的状态转移矩阵 F 组成内容为：
F
FN FS ，其中 FN 中非零元素为可由惯导误差模型获得。
2、实验步骤
1） 实验准备（由实验教师操作）
① 将 IMU 安装在大理石实验台上，确认 IMU 的安装基准面靠在大理石实验平台上
的方位基准尺上；
② 将 IMU 与导航计算机、 导航计算机与稳压电源、导航计算机与监控计算机、 GPS
接收机与导航计算机、 GPS 天线与 GPS 接收机、GPS 接收机与 GPS 电池之间的连接线正
C
n b
03 3
FS
03 3
C
n b
。
06 9 FM
03 3 03 3 9 6
（ 2）量测方程
量测变量 z VE VN VU L
T
H ， ， VE 、 VN 、 VU 、 L 、
和 H 分别为捷联解算与 GPS的东向速度、北向速度、天向速度、纬度、经度和高度之
T
差；量测矩阵 H H V H P ， H P 03 6 diag RM H , (RN H )cos L,
四、实验结果与分析
1、 SINS/GPS 组合导航后得纬度曲线和 GPS 导航纬度曲线对比如下图
34.95 34.9
34.85
GPS 纬 度 组合 导航后纬 度
34.8
） 34.75
度 （ 34.7
度 纬 34.65
34.6
34.55
34.5
34.45
0
1
2
3
4
5
6
7
8
4
x 10
2、SINS/GPS 组合导航后得经度曲线和 GPS 导航经度曲线对比如下图
110.1
110 109.9
GPS 经 度 组 合 导 航 后经 度
109.8
109.7 ） 度 （ 109.6 度 经 109.5
109.4
109.3
109.2
109.1
0
1
2
3
4
5
6
7
8
4
x 10
3、 SINS/GPS 组合导航后得高度曲线和 GPS 导航高度曲线对比如下图
3500
3000
2500
确连接；
③ 打开 GPS 信号转发器；
④ 打开监控计算机中的监控软件；
⑤ 打开稳压电源开关，确认稳压电源输出为 28V ；
⑥ 打开捷联惯导 /GPS 组合实验系统电源，实验系统开始启动，注意 30s 内严禁动
IMU ； ⑦ 打开 GPS 接收机电源，确认通过信号转发器可以接收到
4 颗以上卫星；
⑧ 将监控软件设置为“准备”状态，准备时间 5 分钟；
） 2000 m
（
度 高
1500
1000
GPS 高 度 组合 导航后 高 度
500
0
0
1
2
3
4
5
6
7
8
4
x 10
4、 SINS/GPS 组合导航后得东向速度曲线和 GPS 导航东向速度曲线对比如下图
80
GPS 东 向 速 度
60
组 合 导 航 后东 向 速 度
40
） /s m 20 （ 度 速0 向 东
二、实验原理
（ 1）系统方程 X FX GW
X
E NU
vE vN
vU
L
T
hx yz
x
y
z
其中， E 、 N 、 U 为数学平台失准角； vE 、 vN 、 vU 分别为载体的东向、北向和天向速度误差；
L 、 、 h 分别为纬度误差、经度误差和高度误差；
x 、 y 、 z 、 x 、 y 、 z 分别为陀螺随
8
6
4
/s）
m
（ 度
2
速
向
天0
GPS 天 向 速 度 组 合 导 航 后 天 向速 度
-2
-4
0
1
2
3
4
5
6
7
8
x 10 4
7、 SINS/GPS 组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图
200
150
100
50
度
0
-50
-100
-150
-200 0
组 合 导 航 后航 向 角 组 合 导 航 后俯 仰 角 组 合 导 航 后横 滚 角
-20
-40
-60
0
1
2
3
4
5
6
7
8
4
x 10
5、 SINS/GPS 组合导航后得北向速度曲线和 GPS 导航北向速度曲线对比如下图
60
4020/s） m（度0速向
北 -20
-40
-60 0
GPS 北 向 速 度 组 合 导 航 后 北 向 速度
1
2
3
4
5
6
7
8
x 10 4
6、 SINS/GPS 组合导航后得天向速度曲线和 GPS 导航天向速度曲线对比如下图
滤波增益： K k
Pk / k
1
H
T k
(H k Pk /k
1H
T k
Rk ) 1
一步预测均方误差： Pk /k 1
P Q T
k ,k 1 k 1 k,k 1
k1
三、实验内容及步骤
估计均方误差： Pk ( I K k H k ) Pk / k 1
1、实验内容
① 捷联惯导 /GPS 组合导航系统静态导航实验；