第
3
期
2003
年
10
月
雷达科学与技术
Radar Science and Technology
Vol. 1 No. 3
October 2003
粒子滤波器及其在目标跟踪中的应用
江宝安, 卢焕章
(
½防科技大学
ATR
½家重点实验室, 湖南长沙
410073)
摘
要
:
为了在物 理条件下对目标进 行精确建模, 有时需要运用非 线性、
非高 斯系统。而常规的卡尔
曼滤波算法要求系统是线性高斯型的, 因 而不½ 直接用 来解决非 线性、
非高 斯问题。为 了解决 这一问 题,
人们开发出各种 非线性滤波算法。一种是扩展卡尔曼算法
( EKF ) ,
它对 非线性 系统进行 局部线 性化, 从而
间接利用卡尔曼 算法进行滤波与估算; 另一种 是序列 蒙特卡½ 算法, 亦即 粒子滤 波器(
PF) ,
它是最 近出现
的解决非线性问 题的有效算法。本文简要介绍非 线性跟 踪的最 优与次优 贝叶斯 算法, 重点 关注粒 子滤波
器, 通过再入大气层弹道目标的例子, 说明
PF
在目标跟踪中的应用。
关键词
:
目标跟踪; 非线性 系统; 贝叶斯算法; 粒子滤波器
中图分类号
:TN953
文献标识码:
A
文章编号:
1672 2337( 2003) 03 0170 05
Particle Filter for Target Tracking
JIANG Bao an, LU Huan zhang
( National K ey Lab of ATR , N UDT , Changsha
410073,
China )
Abstract:
For many applications, it is becoming important to involve some elements with nonlinearity and non
Gaussionity in order to model accurately the underlying dynamics of a physical system. Moreover, in this case, it is usu
ally difficult to directly utilize Kalman filter( KF) , because KF is based on condition of linear Gaussian system. For solv
ing these problems, several variants of nonlinear/ non Gaussian filters are developed. One is extended Kalman filter
( EKF) , it makes a local linearization to a nonlinear system, so KF can be utilized indirectly to filter and estimate. The
other is sequential Monte Carlo method based on point mass, which is called particle filter( PF) . It is an efficient method
dealing with nonlinear/ non Gaussian problems. In this paper, we review both optimum and suboptimum Bayesian
algorithms for nonlinear tracking problems, with focus on PF. And the performances of the PF are discussed and com
pared with the EKF through tracking a re entry ballistic object.
Key words:
target tracking; nonlinear system; Bayesian algorithm; particle filter
向量的维数一般来说½于状态向量的维数。
1
引言
在科学与工程实践中, 许多问题要求利 用带
为了分析一个动态系统, 至少需要两个模型:
描述随时间演化的状态模型( 系统模型) 和与状态
有关的带有噪声的测量模型。在利用贝叶斯方法
对状态进行 估计时, 需要建立基于 已获信息的后
验概率密度½数(
pdf )
。由于这个
pdf
包含所有的
目标统计信 息, 故可以认为它是目 标估计的完全
解。原则上讲, 从
pdf
可以获取一个最优估计。½
在许多情况下获取精确的
pdf
是不可½的, 这就需
要进行各种次优估计。其中,
EKF
和
PF
是两种主
要方法。由于测量数据 是以序列方式 获取的, 运
有噪声的测量值对随时间变化的系统状态进行滤
波与估计。在本文中, 我们利用状态空间法, 用差
分方程对随时间演化的动态系统 建模, 并假 定测
量值是以离散方式获取的。状态空间法的焦点是
状态向量的确定, 例如在跟踪问题中, 状态向量的
确定是与目标的运动特性密切相关的。测量向量
代表与状态向量有关的含有噪声 的观测值, 测量
收稿日期:
2003- 09- 02;
修回日期:
2003- 10- 12
2003
年第
3
期
江宝安: 粒子滤波器及其在目标跟踪中的应用
可夫过程
p ( x
k
/ x
k-
1
, z
1
k-
1
)
171
= p ( x
k
/ x
k-
1
)
。
由系统
用贝叶斯理论可以以递½的方式对测量数据进行
序列处理而不必是批处理, 因而没有必要对 以前
的测量数据进行存储和再处理, 节省了大量 的存
储空 间。这 种贝叶 斯递 ½滤 波器由 两个 步骤 组
成: 预测和 更新。预测是利用 系统模型预测 从一
个测量时刻到下一个时刻的前向状态
pdf,
而更新
操½是利用最新的测量值对这个先验
pdf
进行修正。
本文重点介绍 粒子滤波器的 原理、
方法 以及
在目标跟踪中的应用, 具½安排如下: 第
2
节描述
非线性跟踪问题, 第
3
节介绍在满足一定条件下的
最优贝叶斯解, 即卡尔曼滤波解。第
4
节介绍两种
次优贝叶斯算法, 即
EKF
和
PF。第 5
节以实例说
明
PF
的应用。
模型式(
1)
和统计值
v
k-
1
,
可以确定状态演化的概
率密度
p ( x
k
/ x
k-
1
)
。
在时刻
k
获得测量值
z
k
,
利用
贝叶斯规则更新先验概率:
p ( x
k
/ z
1
k
) =
常数
p ( z
k
/ z
1
k-
1
)
p ( z
k
/ x
k
) p ( x
k
/ z
1
p ( z
k
/ z
1
k-
1
)
=
p ( z
k
/ x
k
)
k-
1
)
(
4)
k-
1
)
p ( x
k
/ z
1
dx
k
取决于由测量模型式(
2)
和统计值
n
k
所定义的
似然½数
p ( z
k
/ x
k
)
。
在更新公式(
4)
中, 测量值
z
k
被用来修正先验概率密度, 以获取½前状态的后
验概率密度½数。
3)
和式(
4)
是最优贝叶斯估
式(
计的一般概念表达式, 通常不可½对它进行精确
的分析。
在满足一定的条件下, 可以得到最优贝叶
斯解。
½如果条件不满足, 可以利用
EKF
或
PF
获得
次优贝叶斯解。
2
非线性贝叶斯跟踪
考虑具有加性高斯过程噪声和测量噪声的离
散非线性滤波问题。目标的状态方程为
x
k
= f
k
( x
k-
1
, v
k-
1
)
这里,
f
k
R
n
x
3
(
1)
最优算法(
KF)
卡尔曼滤波假定在每一时刻后验概率密度是
R
n
v
R
是状态向量x
k-
1
的非线
n
x
性方程,
{ v
k-
1
, k
自然数集。
N}
是离散过程噪声序列,
n
x
和
高斯型的, 因此全部 参数为均值和 协方差。如果
p ( x
k-
1
/ z
1
k-
1
)
是高斯分布, 可以证明在下述条件
下
p ( x
k
/ z
1
k
)
也是高斯分布:
( 1)
v
k-
1
和
n
k
是高斯分布;
( 2)
f
k
( x
k-
1
, v
k-
1
)
是
x
k-
1
和
v
k-
1
的线性½数;
( 3)
h
k
( x
k
, n
k
)
是
x
k
和
n
k
的线性½数。
这样, 式(
1)
和式(
2)
可以改写为
x
k
= F
k
z
k
= H
k
x
k-
1
+ v
k-
1
x
k
+ n
k
(
5)
(
6)
n
v
分别是状态向量和过程噪声向量的维数,
N
是
目标的测量方程为
z
k
= h
k
( x
k
, n
k
)
这里
, h
k
{ n
k
, k
R
n
k
(
2)
R
n
n
R
z
是
x
k
的非线性方 程,
n
N }
是离散测量噪声序列,
n
z
和
n
n
分别是
测量向量和测量噪声向量的维数。 贝叶斯学派
以
的观点, 跟踪问题就是在给定测量数据
z
1
k
的条件
下, 估算状态向量
x
k
的值, 即估计后验概率密度½
数
p ( x
k
/ z
1
k
)
。
若假定初始先验概率密度½数
pdf
p ( x
o
/ z
o
)
p ( x
o
)
是已知的(
x
o
表示初始状态向
量,
z
o
表示没有测量值)
,
则从原则上讲, 通过预测
和更新就可以递½的方式估计后验概率密度½数
pdf
p ( x
k
/ z
1
k
)
。
假定 在 时 刻
k -
1
概 率 密 度 ½ 数
pdf
p ( x
k-
1
/ z
1
k-
1
)
F
k
和H
k
是已知的系统矩阵和测量矩阵。
k-
1
和
n
k
v
分别是均值为零, 方差为
Q
k-
1
和
R
k
的统计独立的
高斯½噪声。
这里,
F
k
, H
k
以及噪声参数
Q
k-
1
和
R
k
可以是时变的。
卡尔曼算法如下所示:
p ( x
k-
1
/ z
1
p ( x
k
/ z
1
k-
1
)
= N ( x
k-
1
; x
k-
1/
k-
1
, p
k-
1/
k-
1
)(
7)
^
(
8)
(
9)
(
10)
F
T
^
k
x
k/ k-
1
)
^
(
11)
(
12)
(
13)
(
14)
k-
1
)
= N ( x
k
; x
k / k-
1
, p
k / k-
1
)
^
x
k-
1/
k-
1
^
p
k/ k-
1
( z
k
- h
k
h
k
p
k/ k-
1
h
T
+ R
k
k
p ( x
k
/ z
1
k
) = N ( x
k
; x
k/ k
, p
k / k
)
^
其中,
x
k / k-
1
= F
k
^
p
k/ k-
1
= Q
k-
1
+ F
k
^
x
k/ k
= x
k / k-
1
+ K
k
^
^
p
k/ k
= p
k / k-
1
- K
k
S
k
= h
k
p
k/ k-
1
是已知的, 那么利用系统模型式(
1)
就可以预测时刻
k
的先验概率密度:
p ( x
k
/ z
1
k-
1
)
= p ( x
k
/ x
k-
1
)
p ( x
k-
1
/ z
1
k-
1
)
dx
k-
1
(
3)
注意: 在式(
3)
中, 利用了式(
1)
所描述的一阶马尔
172
K
k
= p
k / k-
1
其中,
S
k
是更新项z
k
- h
k
尔曼
益。
h
T
k
S
-
1
k
雷 达科学与技术
第
1
卷第
3
期
(
15)
具有更½的性½。
粒子滤波器(
PF)
序列重要度 采样(
SIS)
算 法是一种 蒙特卡½
( MC)
方法, 它是序列蒙特卡½(
SMC)
滤波的基础。
这种
SMC
方法以各种名目出现, 如自助
( bootstrap)
滤波、
粒子滤波等。以
MC
模拟实现递½贝叶斯滤
波, 关键的思想是利用一组带有相 关权值的随机
样本, 以及基于这些样本的估算来 表示后验概率
密度
p ( x
k
/ z
1
k
)
。
½样本数非常大时, 这种概率估
算将等同于 后验
pdf。为
了详细描述
PF
算法, 令
Ns
{ x
ik
, w
ik
}
i =
1
表示代表后验
pdf
p ( x
k
/ z
1
k
)
的随机粒
4. 2
子, 权值
w
ik
经过½一化处理, 在时刻
k
的后验概率
密度可以近似为
N
s
x
k / k-
1
的方差,
K
k
是卡
如果假设条件 成立, 卡尔 曼解就是跟踪 问题
的最优解。这意 味着在线性高斯 环境下, 没 有任
½其他算法的性½比卡尔曼滤波器的性½更½。
4
次优算法
在许多情况下, 第
3
节中所做的假设不成立, 因
此不½利用卡尔曼滤波器得到目标的最优贝叶斯估
计。本节我们来考虑两种非线性贝叶斯滤波器, 即
扩展卡尔曼滤波器(
EKF)
和粒子滤波器(
PF)
。
4. 1
扩展卡尔曼滤波器(
EKF)
如果因为½数的非线性, 式(
1)
和式(
2)
不½改
写成式(
5)
和式(
6)
的½式, 那么方程的局部线性化
可½是对非线性问题的充分描述。EKF 正是基于
这样的思想, 其算法如下:
p ( x
k-
1
/ z
1
p ( x
k
/ z
1
p ( x
k
/ z
1
k
)
k-
1
)
p ( x
k
/ z
1
k
)
i=
1
w
ik
( x
k
- x
ik
)
i
(
27)
利用重要 度采样原 理对权值
w
k
进行选择, 假定
p (x)
( x ) ,
从 中 很 难 得 出
x
的 采 样 值。令
i
x q( x ) , i =
1,
, N
s
, q( )
是重要度密度½数, 对
密度
p ( x )
估计可表示为
N
s
N ( x
k-
1
; x
k-
1/
k-
1
, p
k/ k-
1
)(
16)
^
N ( x
k
; x
k/ k-
1
, p
k/ k-
1
)
^
N( x
k
; x
k / k
, p
k / k
)
^
p
k-
1/
k-
1
H
k
^
F
T
^
k
(
17)
(
18)
(
19)
(
20)
(
21)
(
22)
其中,
p(x )
i=
1
w
ik
( x - x
i
)
(
28)
k-
1
)
其中,
x
k/ k-
1
= f
k
( x
k-
1/
k-
1
)
^
p
k / k-
1
= Q
k-
1
+ F
k
^
x
k / k
= x
k / k-
1
+ K
k
^
^
p
k / k
= p
k / k-
1
- K
k
( z
k
- h
k
( x
k / k-
1
) )
^
p
k/ k-
1
这里,
f
k
( )
和
h
k
( )
是非线性方程, 而
F
k
和H
k
是
^
^
这些非线性方程的局部线性化参数:
F
k
=
^
H
k
=
^
df
k
( x )
| x = x
k-
1/
k-
1
^
dx
dh
k
( x )
| x = x
k / k-
1
^
dx
p
k/ k-
1
H
T
^
k
+ R
k
S
-
1
k
(
23)
(
24)
(
25)
(
26)
p ( x
i
)
(
29)
q ( x
i
)
i
如果样本
x
k
来自重要度密度½数q
( x
k
/ z
1
k
) ,
那么
i
式(
29)
中的权
w
k
可定义为
i
p ( x
k
/ z
1
k
)
i
w
k
(
30)
q( x
ik
/ z
1
k
)
重要度密度½数
q ( x
k
/ z
1
k
)
可½如下分解:
q( x
k
/ z
1
k
) = q( x
k
/ x
k-
1
, z
1
k
) q ( x
k-
1
/ z
1
k-
1
)
(
31)
后验概率密度
p ( x
k
/ z
1
k
)
可分解为
w
ik
p ( zk / x k , z1 k-
1)
p ( xk / z
1
k ) =
p ( xk / z1 k-
1)
p ( zk / z1 k-
1)
p ( zk / x k , z
1
k-
1
)
p ( xk / xk-
1
, z
1
k-
1
)
p ( zk / xk )
p ( zk / xk )
p ( x k / xk-
1
)
p ( xk-
1
/ z
1
k-
1
)
p ( xk-
1
/ z
1
k-
1
)
=
S
k
= H
k
^
(
32)
K
k
= p
k / k-
1
H
T
^
k
将式(
31)
和(
32)
代入式(
30)
可得权更新公式:
w
ik
=
=
i
EKF
只利用非线性½数泰勒展开式中的一次
项, 保留了泰勒展开式中的更多项, ½更精确地逼
近实际的非线性½数, ½增加了复杂度, 限制了它
的应用。
EKF
总是估算
p ( x
k
/ z
1
k
)
为高斯分布。如果
真正的概率密度是非高斯型的, 那么高斯分 布就
不½很½地标识它, 在这种情况下,
PF
相对于
EKF
p ( z
k
/ x
k
) p ( x
k
/ x
k-
1
) p ( x
k-
1
/ z
1
k-
1
)
q( x
ik
/ x
ik-
1
, z
1
k
) q ( x
ik-
1
/ z
1
k-
1
)
w
ik-
1
i
w
k-
1
i
i
i
i
p ( z
k
/ x
ik
) p ( x
ik
/ x
ik-
1
)
q ( x
ik
/ x
ik-
1
, z
1
k
)
p ( z
k
/ x
k
) p ( x
k
/ x
k-
1
)
i
i
q ( x
k
/ x
k-
1
, z
1
k
)
w
k
i
i
i
i
(
33)
( 34)
权
w
k
½一化
w
k
/
i
w
k
i
2003
年第
3
期
江宝安: 粒子滤波器及其在目标跟踪中的应用
密度为先验概率密度:
q ( x
k
/ x
ik-
1
, z
k
) = p ( x
k
/ x
ik-
1
)
(
35)
将式(
40)
代入式(
33)
可得:
w
ik
w
ik-
1
p ( z
k
/ x
ik
)
173
这样, 可估算后验概率密度
p ( x
k
/ z
1
k
) :
N
s
p ( x
k
/ z
1
k
)
i=
1
w
ik
( x
k
- x
ik
)
(
40)
(
41)
½
N
s
时, 估计值式(
34)
接近于真实的后验概
率密度
p ( x
k
/ z
1
k
)
。
对粒子滤波器有两个问题需要说明:
( 1)
退化问题
PF
的一般问题是退化(
degeneracy)
现象。经过
几次迭代, 除一个粒子以外, 所有的粒子只具有微
小的权值。退化现象意味着大量的计算工½½被
用来更新那些对
p ( x
k
/ z
1
k
)
的估计几乎没有½响
的粒子上。
对退化现象的一个恰½的 测度是有效
采样尺度
N
eff
,
有效采样尺度定义为
N
eff
=
N
s
1
+
Var(
w
ik
)
(
36)
由于实现的 简易性, 这样的重要度 密度选择是最
常用的。½然, 还 有许多其他的重 要度密度选择
方式, 这里不详细叙述。在设计
PF
时, 重要度密
度选择是重要的设计步骤。
5
应用
本节通过 对再入大 气层弹 道目标的 运动估
计, 说明
PF
在非线性模型跟踪中的应用。为了简
便, 假定目标垂直下降, ½用于目标的力只有空气
阻力和重力。其系统状态方程如下:
( h) v
2
h=- v
v=-
+ g (
42)
2
其中,
h
为目标高度,
v
为目标速度,
( h)
为 空气
g
密度,
g
为重力加速度,
为弹道系数。
这里, 空气
e
-
h
其中,
w
ik
由式(
33)
和(
34)
确定,
Var(
w
ik
)
为
w
ik
的
方差。
½然不½确切地计算
N
eff
,
½却可以得出
N
eff
的近似估计值:
N
eff
=
^
N
1
s
(
37)
密度模型( 以
kg/ m
3
计) 为
( h) =
-
4
,
其中
( w
ik
)
2
i=
1
由式(
36)
得
N
eff
N
s
,
小的
N
eff
意味着有严重的退
=
1 754,
=
1 49 10
。
弹道系数取决于目标
的质量、
½状等因素, 为简化分析, 假定 是已知
的。
= [ hv ]
T
,
连续状态方程可表示为
而x
x
t
=
( x
t
)
(
43)
利用
Euler
估计公式, 取一个微小的积分步长
,
可得
x
k+
1
= x
k
+
( x
k
) = f ( x
k
) ;
再引入过程
噪声, 以弥补模型的不完善性, 最后得离散时间状
态方程为
x
k+
1
= f ( x
k
) + v
k
其中,
f ( x
k
) =
=
1
-
0
1
]
T
g
( x
k
[
1]
)
2
( x
k
[
2]
)
2
化现象。
显然, 退化现象对
PF
产生了不利的½响。
减小这 一不 利½响 的首要 方法是 增加粒 子数 目
N
s
。
½这通常是不实际的。
因此, 我们主要依靠选
取½的重要度密度
q ( )
和再采样来减小这种不利
的½响。
( 2)
选取½的重要度密度
这种方法是选取½的重要度密度
q ( )
以便把
N
eff
最大化。
最优重要度密度如文献[
10]
所述, 为
q( x
k
/ x
k-
1
, z
k
)
opt
= p( x
k
/ x
k-
1
, z
k
)
=
p ( z
k
/ x
k
, x
k-
1
)
i
i
i
i
i
(
44)
x
k
- G
[ D( x
k
) - g]
p( x
k
/ x
k-
1
)
p ( z
k
/ x
k-
1
)
i
(
38)
G= [
0
D( x
k
) =
将式(
38)
代入式(
33)
得:
w
k
i
w
k-
1
i
p ( z
k
/ x
k-
1
)
p ( x
k
/ x
ik-
1
)
dx
k
(
39)
= w
ik-
1
p ( z
k
/ x
k
)
式(
44)
中过程噪声
v
k
假定是零均值高斯½噪声,
其方差为
3
2
这种最优重要度密度有两个主要 缺点, 即它 需要
从
p ( x
k
/ x
ik-
1
, z
k
)
½ 取 样 本 并 估 算 积 分 值
p ( z
k
/ x
k
)
p
i
( x
k
/ x
k-
1
)
Q= q
3
2
2
dx
k
。
在一般情况下, 并不
2
有关参数 取值如下: 初始 高度
45 7km,
初始速度
½直接解决这两个问题, 通常的做法是取重 要度
174
3350m/ s,
= 23950kg/ ms
2
,
雷 达科学与技术
第
1
卷第
3
期
= 0 1s,
q
= 0 1。
再入大气层弹道目标的高度和速度随时间而
变化, 如图
1
所示。
图
2
用蒙特卡½仿真得到的高度和速度跟踪均方误差
看出, 用
EKF, PF
½ ½较½ 地解决 非线性 跟踪问
题, ½
PF
对噪声的抑制½力更强, 滤波精度更高,
图
1
再入大气层弹道目标的高度和速度随时 间变化的
情况
而且通过增加 粒子数目, 还可以改 进
PF
的性½,
同时
PF
可以很容易地推广到多目标多传感器数
据融合与跟踪问题中。PF 的主要缺点是计算量比
较大, 尚没有一个统一的标准来估算
PF
的性½。
利用雷达以等间距
T
s
对目标高度进行测量,
测量方程为
z
k
= H
x
k
+ n
k
(
45)
其中,
H
1
= [
1 0]
, n
1
是测量, 假定为零均值高斯
k
½噪声, 其方差为
1
,
与过程噪声
v
k
相互独立。
有
关 参 数 取 值 如 下:
T =
1s,
=
200m,
p
0
=
diag[
(
200)
; (
914m/ s)
]
。
在检测概率
p
d
=
1
的 条件下, 用
EKF, PF300
(
粒子数
N
s
= 300)
和
PF600(
粒子数
N
s
= 600)
分别
对高 度和速 度进行
100
次独 立蒙特 卡½ 仿真 所
得的均方误差如图
2
所示。由这些误差曲线可以
2
2
6
小结
PF
滤波方法是一种递½贝叶斯估计方法。该
方法不要求 模型是线性的, 对任意 分布的噪声½
适用, 而且算法简单, 易于编程实现。与扩展卡尔
曼滤波方法比较, 这种算法的滤波效果更½, 适用
范围更广。
(
下½第
178
页)
京公网安备 11010802033920号
Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved
评论