一种高实时性粒子滤波重采样算法

统 仿 真 学 报 V ol. 21 No. 18

2009年9

月 Journal of System Simulation Sep., 2009

第21卷第18期 系

一种高实时性粒子滤波重采样算法

赵 丰1,2，汤 磊1，张 武1，赵宗贵3

（1. 解放军理工大学 指挥自动化学院, 南京 210007; 2. 海南省军区, 海口 570236; 3. 中国电子科技集团公司第28研究所, 南京 210014）

摘 要：重采样是解决粒子滤波退化问题的主要方法。传统的重采样算法，如系统重采样、残差重

采样，以及Bolic 等提出的“残差系统重采样”算法，均存在运算时间较长、占用存储空间较大等问题。而时效性是制约粒子滤波方法实用性的瓶颈。对粒子滤波的基本原理进行了论述；提出了一种高实时性粒子滤波重采样算法——“简单重采样算法”，通过仿真实验与分析，该算法在状态估计精度上与其它重采样算法相当，但却具有计算量小、速度快、实时性强等优点，适于硬件实现。 关键词：粒子滤波；退化；序贯重要性采样；重采样算法

中图分类号：TP391 文献标识码：A 文章编号：1004-731X (2009) 18-5789-05

High Real-time Resampling Algorithm for Particle Filters

ZHAO Feng1,2, TANG Lei1, ZHANG Wu1, ZHAO Zong-gui3

(1.Institute of Command and Automation, PLA Univ. of Sci & Tech, Nanjing 210007, China; 2.Hainan Province Military District, Haikou 570236, China;

3.The 28th Research Institute, CETC, Nanjing 210014, China)

Abstract: Resampling is a critically operation to solve degeneracy problem with particle filters. The common characteristics with traditional resampling algorithms, such as system resampling, residual resampling and Bolic’s “residual-systematic resampling”, are all high complexity and large storage. Time efficiency bottlenecks the practicability of particle filters. Particle filters were reviewed and a new high real-time resampling algorithm called simple resampling was proposed. The simulation results prove the proposed algorithm has the same precision, but it is low complexity, high speed, and suitable for hardware implementation.

Key words: particle filters; degeneracy; sequential importance sampling; resampling algorithm

引 言

粒子滤波是一种基于Monte Carlo仿真的递推贝叶斯估计方法，它可以有效地处理非线性、非高斯噪声环境下的状态估计问题。其基本原理是根据系统状态的一组带有权值的随机样本来表示实际问题所需要的后验概率密度函数，这些样本被形象地称为“粒子”。粒子滤波的三个基本步骤是：产生新的粒子（采样阶段）、计算粒子权重（权重阶段）和重采样。而重采样在粒子滤波应用中是至关重要的。Kong ，Liu [1]等已证明重要性权的方差随着时间而增大，即粒子的权值会很快两极分化，少量的粒子权重很大，而大部分的粒子权重几乎可以忽略不计，这称为权的退化现象。重采样的目的就是将权重小的粒子用权重大的粒子分解代替。标准的重采样算法都是基于分层采样方法的，如系统重采样（System Resampling: SR），残差重采样（Residual Resampling: RR）[3]，多项式重采样（Multinomial Resampling: MR）等。Bolic

[4-6]

[2]

算法没有比较运算，适于硬件实现。

本文中提出了一种新的重采样算法，这种算法不仅具有Bolic 算法的优点，且计算量大大减少，速度快，实时性强。

1 关于粒子滤波中的重采样

1.1 状态分布密度的贝叶斯估计[7]

离散动态系统用空间状态方程和测量方程可以分别表示为：

x k =f (x k −1, u k −1)

z k =h (x k , v k )

k =1,2, (1)

k =1,2, (2)

假设初始状态的分布密度p (x 0|z 0) ≡p (x 0) 已知（z 0可以认为是无测量）；离散状态序列x 1:k 具有马尔可夫特性，离散测量序列z 1:k 为独立测量序列，且与各状态独立。这里：

x 1:k ={x 1, x 2, , x k },z 1:k ={z 1, z 2, z k },k ∈N 则状态x k

的密度的贝叶斯预测方程：

p (x k |z 1:k −1) =∫

S x k −1

p (x k |x k −1, z 1:k −1) p (x k −1|z 1:k −1) dx k −1 (3)

等提出了一种

新的重采样算法（Residual-Systematic Resampling: RSR），该

(S x k −1为x k −1的状态空间)

收稿日期：2007-09-11 修回日期：2008-04-01

基金项目：“十一五”国防科技预先研究课题 (513060302)，“信息综合处理、融合技术”。

作者简介：赵丰(1972-), 男, 江苏南京人, 博士生, 工程师, 研究方向为信息融合、指挥自动化；汤磊(1978-), 男 江苏南京人, 博士生, 讲师, 研究方向为图像处理，信息融合等；张武(1974-), 男, 陕西大荔人, 博士生, 研究方向为目标跟踪与识别；赵宗贵(1943-)，男，黑龙江省哈尔滨人，研究员，博导，研究方向为信息融合、专家系统等.

与状态x k 的密度的贝叶斯更新方程为：

p (z k |x k , z 1:k −1) p (x k |z 1:k −1)

p (x k |z 1:k ) = (4)

p (z k |z 1:k −1) 其中，p (z k |z 1:k −1) =∫

S x k

p (z k |x k ) p (x k |z 1:k −1) dx k

式(3)(4)构成了状态x k 的最优贝叶斯估计，但其解析解只在特定高斯条件或状态空间有限时成立。

• 5789 •

2009年9月 系 统 仿

真 学 报 Sep., 2009

=1。为避免情况时，某个权值为1，其余均为0，此时N eff

thresh

1.2 粒子滤波中的权值估计[7, 8]

为了计算(3)(4)式，引入序贯重要性采样算法(SIS)。SIS

(Sequential Importance Sampling)算法是一种序贯Monte Carlo 方法，其核心思想是用一组带权值的随机粒子

i i N s

{x 0:k , w k }i =1来表示并计算所需的后验概率密度，

N s 1

时，则进行重采样。

2 重采样算法

p (x 0:k |z 1:k ) ≈∑w δ(x 0:k

i

k

重采样的思想是通过对样本重新采样，繁殖重要性权高

−x ) (5) 的粒子，淘汰权值低的粒子，从而抑制退化，如图1所示。

i

0:k

(i ) (i ) N s

k 重采样前{x k , w }i =1 (i )

重采样后{x k ,1/N s }i N =s 1

i i

其中，δ为狄拉克函数，诸w k 满足∑i w k =1。权重的选取

采用“重要性采样”（Importance Sampling）原则：状态x 的分布为p (x ) 时，状态x 的采样（粒子）服从分布密度q (x ) （q (x ) 称为重要性密度函数），它易于采样，且满足当

i p (x ) >0时，均有q (x ) >0。当x 0:k 从q (x 0:k |z 1:k ) 中抽取时，

显然有：

i

w k ∝

i

p (x 0:k |z 1:k ) (6) i q (x 0:k |z 1:k )

将q (x 0:k |z 1:k ) 进行分解，注意x 0:k −1与z k 的独立性：

q (x 0:k |z 1:k ) =q (x k |x 0:k −1, z 1:k ) q (x 0:k −1|z 1:k −1) (7) 利用贝叶斯估计，从(4)式可知成立（注意x 0:k 的马尔可夫特性）：

图1 重采样原理示意图

图中，圆表示粒子，面积表示其权重。重采样前各粒子

(i ) (i )

k x k 对应的权值为w ，经过重采样后，粒子总数保持不变，

p (x 0:k |z 1:k ) =

=

p (z k |x 0:k , z 1:k −1) p (x 0:k |z 1:k −1)

p (z k |z 1:k −1) p (z k |x k ) p (x k |x k −1)

p (x 0:k −1|z 1:k −1)

p (z k |z 1:k −1)

(8)

权值大的粒子分成了多个粒子，权值太小的粒子则被抛弃，重采样后每个粒子权值相同，均为1/N s 。

∝p (z k |x k ) p (x k |x k −1) p (x 0:k −1|z 1:k −1)

2.1 已有的重采样算法描述

下面将几种重要的重采样算法以伪代码的形式描述如下：

将(7)(8)代入(6)，有：

(1) 系统重采样算法（伪代码1） i i i

p (z |x ) p (x |x ) −1k k k k i i

w k ∝w k (9) 产生一个随机数U ~u [0,1/N ] −1

i i q (x k |x 0:k −1, z 1:k )

（赋予U 初值为0～1/N之间均匀分布的随机数）

由状态序列x 0:k 的马尔可夫特性，有q (x k |x 0:k −1, z 1:k ) = W cum =0

(i ) k （W cum 表示权值w 的累加和，初始赋值为0） q (x k |x k −1, z k ) ，(9)式可改写为：

i i i for 1:n =N p (z k |x k ) p (x k |x k −1) i i

w k ∝w k (10) −1

i i （N 次循环，N 为粒子总数） q (x k |x k −1, z k )

i i

显然，实现w k −1到w k 的递推运算，只依赖上周期状态

k =0

（k 表示每个粒子被分解的次数，初始值为0）

W cum =W cum +w (n )

x k −1和本周期测量z k 。状态的后验密度p (x k |z 1:k ) 可以简单表示为：

N s

(1)

k ，二次循环为(W cum 开始累加，首次循环为为w

i i

p (x k |z 1:k ) ≈∑w k δ(x k −x k ) (11) (1)(2)

k k w +w ，…) i =1

1.3 退化问题

SIS 粒子滤波存在权的退化问题，它可以用有效样本数N eff

[1]

while (W cum >U )

（比较W cum 与U 的大小，W cum 较小时直接退出循环）

度量：

k =k +1

N eff =

（当W cum 较大时，则该粒子分解的次数加1） N s

(12) *i

U =U +1/N 1+Var (w k )

（U 每次递增1/N） 因为上式很难计算得到，可以计算它的估计值[7]：

end 1 =N (13) eff N s

i (n ) =k i 2

k (w ) ∑i =1(i (n ) 表示各粒子最终被分解的个数，i (n ) =0表示原粒

i

k 其中，{w }是归一化权值。从(14)式不难看出：理想情况时，

=N ；最坏 i =1/N ，此时N 归一化权值是相同的，即w eff s

k

s

子被抛弃)

end

• 5790 •

2009年9月 赵丰，等：一种高实时性粒子滤波重采样算法 Sep., 2009

从上述代码不难看出，系统重采样算法通过循环比较的调整，具体方法描述如下：如某个粒子的权值为

W cum 与U 的大小，来确定原粒子分解的个数。

(2) 残差重采样算法。其算法原理与系统重采样算法相似，它先将初始权值乘以粒子数N 再取整，获得粒子分解个数的初值；再对取整后的小数部分进行系统重采样，获得粒子分解个数的更新值，两部分相加为粒子分解的最终值。，显然，当粒子权值大部分为0时（某几个粒子权值较大）残差重采样算法的运行时间较小。其原因是，系统重采样对每个粒子不加区分地进行比较和循环；而残差重采样在取整（因为它们的小数部分运算后就抛弃了那些权值为0的粒子

也是0），循环运行次数较少。这一点，在后面的仿真实验中得到了验证。

i ×(1/N ) −1/2N ≤w (n )

个粒子的权值上，以避免该次分解引起的权值损失，如此反复，直到最后一个粒子。这种方法在分解过程中对已有粒子这样可序列中相邻粒子的权值进行了少量(小于1/2N ) 调整，以并保证使重采样后粒子数不变，所有粒子权值之和为1。

从上述方法描述不难看出，简单重采样算法的基本思想有两点：首先，该算法舍弃了产生随机数U 这个过程。其它算法中均有，“赋予U 初值为0～1/N之间均匀分布的随机数”。很显然，如果“给定U 初值为0”，对整个算法精度并无影响，这一点在第三部分的算法效果分析中也得到了验证。其次，该算法隐化了比较运算。算法中求取各粒子分解个数的伪代码是i (m ) =round (w (m ) i N ) （round 运算可以改为取整运算），该代码实际是比较粒子权值与1/N 的倍数之间的大小，并直接得出每个粒子的分解个数。此运算效率大大高于比较运算，并可以一步求解得出结果，也不需要系统重采样算法中的U 值累加过程等，省略了中间值。

从以上分析不难知道，简单重采样算法简单直观，仅一重循环，且无比较运算，无需产生和存储任何中间值。

(3) 残差系统重采样算法（伪代码2） 产生一个随机数U ~u [0,1/N ]

for 1:n =N

i (n ) =[(w (n ) −U ) i n ]+1

①

（[ ]为取整运算。各粒子的权值与U 值比较后确定分解

的个数，权值越大，分解的个数越大；权值为零时，i (n ) =0）

i (n )

U =U +−w (n )

N

(与系统重采样不同，U 值调整依据的是粒子的权值) end

残差系统重采样算法与前述几种算法不同，U 值随机产生后并不进行累加运算，而是通过每次粒子的分解结果进行调整，整个算法只有一重循环，且无比较运算。

3 仿真实验与结果分析

为了对前述几种重采样算法进行比较，本文引用了2个实例分别对前述几种算法的采样结果和采样时间进行比较与分析。

2.2 本文提出的重采样算法

本文提出一种新的重采样算法，暂命名为“简单重采样”（Simple Resampling），它的算法描述如下：

简单重采样算法（伪代码3）

3.1 几种重采样算法粒子分解分析

下面引用的实验数据是一个N =5粒子的情况，它们的权重分别是7/20，6/20，2/20，2/20，3/20[5]。 3.1.1 实验结果及分析

在相同的上述实验数据之下，三种重采样算法得到的结果（各粒子的分解数）均为2、1、1、0、1。下面图示给出了系统重采样、残差系统重采样和简单重采样结果。图2、

i (1)=round (w (1)i N ) ②

(round 为四舍五入运算。依据第一个粒子权值求取粒子的分解数)

for 2m =:N

（N －1次循环，N 为粒子总数）

3中的符号含义与伪代码中的符号含义一致。图4显示了本文提出的“简单重采样算法”中的权值更新过程。

14/20

w (m ) =w (m ) +w (m −1) −i (m −1) /N

（依据上一个粒子的分解情况，调整当前粒子权值）

i (m ) =round (w (m ) i N )

(根据调整后的权值，直接求取当前粒子的分解个数) end

简单重采样算法最直观的考虑是，将每个粒子的权重与

U (5)

U (4)U (3)U (2)

W(1)

U (1)7/20

W c u m (1)

W(4) W(3) 2/20 2/20

W c u m (3)

W(5) 3/20

1/N 的整数倍比较，从而直接得出每个粒子的分解数。因为从其它算法不难知道，每个1/N 即代表一次分解。当然，每个粒子的权重都不可能是1/N 的整数倍，因此需要额外

①

4/20

此处原作者的算法表述有误。“取整”应改为“取小”，如－1.6应取－2，而非－1。 ②

round 可以改写为取整运算，如i (m ) =round (w (m ) i N ) 改写为i (m ) =[w (m ) i N +0.5]。

• 5791 •

W c u m (2)

4/20

W c u m (4)

W(2)6/20

W c u m (5)

4/20

1 2 3 4 5

图2 系统重采样算法

粒子序号

2009年9月 系 统 仿

1

真 学 报 Sep., 2009

i

i =1,...,5000; k =1,...,100。选取重要性密度函数q (x k |x k −1, z k ) =

权

值 累 加 和

U (3)

U (2)

w(2)

U (4)w(3)

U (5) w(4)

w(5)

i i i i

p (x k |x k −1) ，将其代入(10)式，有w k ∝w k −1p (z k |x k ) ∝ i 2exp(−(z k −(x k ) /20) 2/σn ) 。设定N thresh （如取N thresh ＝N/3），

eff

thresh

本文分别采用系统重采样、残差重采样、残差系统重采样和简单重采样四种算法进行计算，主程序按照第一部分给出的公式编写，各算法调用子程序严格按照伪代码编写。计内存768MB ，仿真软件为Matlab 算平台为CPU PIV 2.93G，

U

1 2 3 4 5 粒子序号 图3 残差系统重采样算法[5]

1

(1)

w(1)

7.0。实验结果分析主要从几种重采样方法产生的粒子对系

统状态x 的估计及估计误差进行比较。

(1) 状态x 的估计比较。图5为100个时标，每个时标上粒子数为5000情况，几种算法得出的估计值与x 真值的比较图。从图中可以看出，四种算法的仿真结果几乎一致，从局部放大情况来看，差异很小。这说明四种重采样算法得出的状态估计结果基本一致。而因为U 初值的变化而引起的变化（系统重采样与残差重采样），当粒子数目较大时，也几乎可以忽略不计。

图6为粒子数5000，在100(2) 状态x 的估计方差比较。

个时间点上进行蒙特卡罗仿真状态估计值与真值的均方根

w(4)

权

值 累 加 和

w(1)

w(3) w(2)

w(5) w(5)’ w(4)’

w(3)’ w(2)’

w(1)’

1 2 调整前后的权值

图4 简单重采样算法

误差曲线图。从图中可以看出，四种算法的仿真估计均方根误差基本一致。

为简单起见，图2、3中的U 初值（应赋予U 初值为0～

1/N之间均匀分布的随机数）均选取了U 初值等于1/2N= 1/10。事实上，这两种算法的结果都会因为U 初值的变化，而稍有变化。但这种变化对整个粒子滤波的性能影响不大，尤其是当粒子数目较大时。这一点可在第二个实例中得到证实。

3.2 几种重采样算法效果分析

3.2.1 实验模型

为了验证简单重采样算法的有效性，本文引用如下的模型[8]，

图5 1次仿真真值及四种算法的状态估计值比较图 其状态方程为：

x 25x k −1

x k =k −1++k +v 8cos(1.2) k 2

21+x k −1

其测量方程为： 2x

z k =k +v k ' 20式中k 是时标，v k ，v ' k 分别为过程噪声和测量噪声，它们状态变量x k 均服从零均值高斯分布，方差分别为σ和σ，

2

m

2n

为一维变量，初始状态x 0服从Gauss 分布。 3.2.2 实验结果性能分析

依据上述模型中的系统方程与测量方程，设定初始时刻

i

，然后按状态值x 0和粒子总数，按Gauss 分布产生粒子x 0

图6 100次蒙特卡罗仿真四种算法的RMSE 值比较图

3.2.3 重采样时间分析

仿真环境不变，分别选取5000、10000和50000个粒子时，100个时间点上的平均计算时间，如表1所示。从表1

状态方程(14)依次递推求取x k 的相应粒子，序列{x i k }，

• 5792 •

2009年9月 赵丰，等：一种高实时性粒子滤波重采样算法 Sep., 2009

不难看出，四种重采样算法的计算时间都随着粒子数的增加而增加，而且计算时间与重采样粒子数目基本上成比例；但是简单重采样算法的计算时间大约仅为其它算法的十分之一左右，可见，本文提出的简单重采样算法在计算时间（计算量）上较其它重采样算法具有一个数量级的优势。

表1 选取不同粒子数时几种算法的计算时间

系统重 采样

残差重 采样

残差系统重 采样

简单重 采样 2.22ms 13.6ms

4 结论

粒子滤波是解决非线性非高斯系统估计问题的重要方法之一，而重采样是解决粒子权值退化的主要方法。与扩展、Unscented 卡尔曼滤波（UKF ）等非线卡尔曼滤波（EKF ）

性滤波方法相比，粒子滤波具有更好的性能，但粒子滤波的计算复杂度较高，计算时间也较长。而大多数的非线性应用，如机动目标跟踪、计算机视觉、状态监视与故障诊断等，对实时性要求高。因此，如何减少粒子滤波的计算复杂度，并通过硬件加以实现，是当前研究的热点和难点。

本文提出的简单重采样算法具有循环层数少、计算复杂度低、不需比较运算和易于硬件实现等多个优点。从仿真结果看，该算法的输出结果与其它算法在性能上相同，但运算时间大大缩小。

在下一步工作中，我们将进一步研究简单重采样算法在

5000 11.68ms 8.83ms 15.36ms 0.96ms 10000 21.76ms 13.77ms 27.04ms 50000 104.8ms 70.2ms 126.1ms

从前述的算法描述中不难知道，各算法的计算复杂度均是O (N ) 的。为了定性分析各算法的计算时间，我们对系统重采样算法、残差系统重采样算法与本文提出的简单重采样算法三种算法的各种不同运算及所需存储空间进行了比较，具体见表2。

表2 三种算法不同运算复杂度与所需存储空间

系统重 采样

乘（除）运算 0 加（减）运算 比较运算 循环层数 是否需要产生随机数 所需存储空间

w 1:N +i 1:N

3N

残差系统重采样 2N 3N

简单重 采样 2N 2N N 1层 否

非线性非高斯滤波中的应用，以及该算法实现的硬件结构。

参考文献：

[1]

A Kong, J S Liu, W H Wong. Sequential imputations and Bayesian missing data problems [J]. Journal of the American Statistical Association (S0162-1459), 1994, 89(425): 278-288.

[2] N J Gordon, D J Salmond, A F M Smith. Novel approach to

nonlinear/non-Gaussian Bayesian state estimation [J]. Radar and Signal Processing, IEE Proceedings F, 1993, 140(2): 107-113. J S Liu, R Chen. Sequential Monte Carlo Methods for Dynamic Systems [J]. Journal of the American Statistical Association (S0162-1459), 1998, 93(443): 1032-1044. [4]

M Bolic, P M Djuric, H Sangjin. New resampling algorithms for particle filters [C]// IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP '03). USA: IEEE, 2003, 2II: 589-92. [5]

M Bolic, P Djuric, S Hong. Resampling Algorithms for Particle Filters: A Computational Complexity Perspective [J]. EURASIP Journal on Applied Signal Processing (S1110-8657), 2004(15) 2267-2277. [6] M Bolic, P M Djuric, H Sangjin. Resampling algorithms and

architectures for distributed particle filters [C]// IEEE Transactions on Signal Processing, [also see IEEE Transactions on Acoustics, Speech, and Signal Processing]. USA: IEEE, July 2005: 2442-2450. [7]

S M HERMAN. A Particle Filtering Approach to Joint Passive Radar Tacking and Target Classification. Electrical Engineering [D]:Urbara, University of Illinois, 2002. [8]

M S Arulampalam, S Maskell, N Gordon, T Clapp. A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking [J]. IEEE Trans. Signal Processing (S1053-587X), 2002, 50(2): 174-188.

N 0 0 2层 是

U ＋W cum ＋m ＋k ＋

1层 是

[3]

取整运算 0 N

U +m ＋w 1:N +i 1:N m ＋w 1:N +i 1:N

表2中N 为粒子数，w 1:N 和i 1:N 分别表示{w i }i N =1和{i i }i N =1。 依据表2的分析再重新分析表1中的数据，不难得出如下结论：系统重采样算法计算复杂度最高，所需存储空间最大，因此计算时间较长。表1中的残差重采样算法表2没有具体分析，其算法原理与系统重采样基本相同，但算法的计算复杂度会随着粒子的权值变化而变化，当粒子权值相对集中时，计算时间反而较少，表1的实验数据也证实了这一点。残差系统重采样算法计算复杂度较低，所需存储空间也没有系统重采样多，但因为对U 值的计算较为复杂，计算时间反而增加了。本文提出的简单重采样算法计算复杂度最低，所需存储空间最少，计算时间因此大大减少。

• 5793 •