最优估计第七章

最优估计理论主讲人：卢晓东
第7 章卡尔曼滤波的应用实例
由于卡尔曼滤波在计算过程中，不需要存储以前数据，适合计算机递推运算，因此从1960年问世以来逐渐被应用到各种领域。

特别是在航空、航天和航海领域，卡尔曼滤波和其各种改进滤波方法被应用到导航、制导和控制等场合。

尽管在本书的前面内容中已经详细介绍了卡尔曼滤波的原理、方法和步骤，但是在实际问题中，如何进行建模、分析、参数设置以及卡尔曼滤波的具体计算，需要通过工程实例才能较好地说明。

本章将从航空和航天领域的实际应用中选取出几个典型问题，针对卡尔曼滤波方法的建模、参数选取和仿真等几个环节给出具体的步骤，以供读者参考和学习。

§7.1 卡尔曼滤波在目标测量中的应用
0204060
80100051015x 104时间(秒)位置(米)020*********
-20-10010
时间(秒)位置误差(米)
真实值滤波值0204060
80100500
1000
1500
2000
25003000时间(秒)速度(米/秒)
020*********-60-40-2002040时间(秒)速度误差(米/秒)真实值滤波值
0204060
80100010
20
3040时间(秒)加速度(米/秒2)020*********
-20-1001020时间(秒)加速度误差(米/秒2)真实值滤波值
0204060
80100051015
x 104时间(秒)位置(米)020*********
-1001020
时间(秒)位置误差(米)
真实值滤波值204060
8010005001000150020002500时间(秒)速度(米/秒)020*********
-60-40-20020
时间(秒)速度误差(米/秒)
真实值滤波值
0204060
80100010
20
3040
时间(秒)加速度(米/秒2)020*********
-20-1001020时间(秒)加速度误差(米/秒2)真实值滤波值
§7.2 EKF在航天器自主导航中的应用
333()()
()()()()()()()()()
()()
()()()()()x y z x y z x v y v z v dx t v t dt dy t v t dt dz t v t dt dv t x t w t dt
r t dv t y t w t dt r t dv t z t w t dt
r t µµµ⎧=⎪⎪⎪=⎪⎪=⎪⎪⎨=−+⎪⎪⎪=−+⎪⎪⎪=−+⎪⎩222
()()()()r t x t y t z t =++
333(1)()[()]()()()()()()()()[(),]()()()()()()()x x x x x y y x y z k k f k T
x k v k T y k v k T z k v k T x k v k T k k r k y k v k T r k z k v k T r k µµµ+≈+∆+∆⎡⎤⎢⎥
+∆⎢⎥⎢⎥+∆⎢⎥⎢⎥−∆=⎢⎥⎢⎥⎢⎥−∆⎢⎥⎢⎥⎢⎥−∆⎢⎥⎣⎦
ϕ
对离散系统方程应用§6.3节的方法进行线性化可得：
ˆ()(/)33332255522335555(1,)()ˆˆˆˆˆˆ[3()()]3()()3()()ˆˆˆ()()()ˆˆˆˆˆˆ3()()[3()()]3()()ˆˆˆ()()()ˆˆ3()()3ˆ()k k k k k k T x k r k T x k y k T x k z k T r k r k r k x k y k T y k r k T y k z k T r k r k r k x k z k T r k µµµµµµµµ=×××∂+=∂∆⋅−∆∆∆ =∆−∆∆∆x x
Φx I I I ϕ2255ˆˆˆˆ()()[3()()]ˆˆ()()y k z k T z k r k T r k r k µ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥∆−∆⎢⎥⎣⎦
020*******
80010001.4961.496
1.4961.4961.4961.4961.496
x 108X 轴位置（k m ）时间（秒）02004006008001000
-400-2000200400时间（秒）X 轴位置误差（k m ）真实值几何解算值020*******
800100001
234
x 105时间（秒）Y 轴位置（k m ）02004006008001000
-1000-500
05001000时间（秒）Y 轴位置误差（k m ）真实值几何解算值
020*******
8001000-4000-2000
020004000
时间（秒）Z 轴位置（k m ）02004006008001000
-4000-2000
020004000时间（秒）Z 轴位置误差（k m ）真实值几何计算值图7-4 航天器利用纯天文导航得到的位置结果与真值的比较图
§7.3 UKF在飞行器视觉导航中的应用
ˆ()(/)
2(1,)()ˆˆˆˆ[cos ()sin ()][sin ()cos ()]1 ˆˆ()()ˆˆ[sin ()cos ()]1x x Φx k k k e n e n H H e n k k k T V k V k T V k V k r k r k T V k V k ρρρρρρ=∂+=∂∆+−∆−⎡⎤+⎢⎥=⎢⎥−∆−+⎢⎥⎣⎦ϕ
ˆ()(/)ˆˆ()[(),]|(/)()
ˆˆ[sin ()cos ()]ˆ()ˆ()ˆˆˆ()[cos ()sin ()]ˆˆˆˆ{[()]cos ()[()]sin ()}ˆ()ˆ()x x u x x x k k k e n H H e n e n n e H k k k k k k T V k V k k r k r k T V k V k T V k V k V k V k k r k ρρρρρρρρρρ=∂=−∂∆−⎡⎤+⎢⎥=⎢⎥−∆+⎢⎥⎣⎦
∆+⋅+−⋅+−−ϕϕ
ˆˆˆ[sin ()cos ()]()()ˆˆˆˆ{[2()]sin ()[2()]cos ()}ˆ()ˆˆˆˆ{[()]sin ()[()]cos ()}e n H e n n e H n e e n T V k V k k r k T V V k k V V k k r k T V V k k V V k k ρρρρρρρρρρρ⎡⎤⎢⎥⎢⎥∆−++⎢⎥⎣⎦∆−⋅−+⋅⎡⎤⎢⎥=⎢⎥∆−⋅−−⋅⎢⎥⎣⎦
§7.4 粒子滤波在导弹捷联惯导中的应用
()sin ()()sin ()()0()0[()cos ()()sin ()]/cos ()()0()tan ()[()cos ()()sin ()]()()()0()()0()()0y z y z x y z x x y y z z t t t t t t t t t t t t t t t t t t t t n t t n t t n t ωγωγϑψωγωγϑγωϑωγωγωωω+⎡
⎤⎡⎤⎡⎢⎥⎢⎥⎢−⎢⎥⎢⎥⎢−−⎢⎥⎢⎥⎢==+⎢⎥⎢⎥⎢⎢⎥⎢⎥⎢⎢⎥⎢⎥⎢⎢⎥⎢⎥⎢⎣⎦⎣⎣⎦x [()]()
f t t ⎤⎥⎥⎥⎥⎥⎥⎥⎦=+x w 假设捷联惯组中的速率陀螺输出主要受到高斯噪声影响，那么观测方程可写为
()000100()()()000010()()()000001()()()[()]()
x x y y z z t v t t t t v t t v t t t t t ωωω⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥==+⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦
=+=+z x Hx v h x v
010********-60
-40-200俯仰角(度)01020304050050偏航角(度)0102030
4050
050100滚转角(度)时间(秒)010********-10010W x (度/秒)010********-5
5
W y (度/秒)0102030
4050-100
10
W z (度/秒)时间(秒)010203040
50-20
20W x (度/秒)010203040
50-20020W y (度/秒)0102030
4050-20020时间(秒)W z (度/秒)观测值真值观测值
真值
观测值
真值
图7-13 姿态角和角速率的真值010********-10010W x (度/秒)滤波值真值010********-10010W y (度/秒)
滤波值真值010********-10010时间(秒)W z (度/秒)滤波值真值图7-14 测量的角速率值图7-15 滤波后的角速率值
010********-10
10W x (度/秒)010********-10
10
W y (度/秒)0102030
4050-10
10
时间(秒)W z (度/秒)滤波前滤波后
滤波前滤波后滤波前滤波后图7-16 滤波前后角速率的误差图
本章要求
1) 掌握对工程问题的建模、线性化方法。

2）熟悉对工程问题的EKF,UKF,PF滤波方法编程。