基于“新息误差”的粒子流滤波算法

周德运，刘 斌，2*，苏 茜

（1.冶金自动化与测量技术教育部工程研究中心（武汉科技大学），武汉 430081；2.冶金工业过程系统科学湖北省重点实验室（武汉科技大学），武汉 430081）

（∗通信作者电子邮箱liubin@wust.edu.cn）

0 引言

粒子滤波（Particle Filter，PF）是基于递推贝叶斯后验概率理论的一种序贯蒙特卡罗算法［1］，是一种基于蒙特卡罗模拟方法来实现贝叶斯滤波的算法。与卡尔曼滤波（Kalman Filter，KF）相比，粒子滤波不受系统模型与噪声的限制（不需要系统模型为线性，也不需要系统噪声为高斯分布），因此被广泛应用于视觉跟踪、目标定位导航、通信与信号处理等众多领域。

粒子滤波作为一种重要的非线性递归贝叶斯滤波方法，具有良好的算法可扩展性和普适性，然而粒子滤波仍然存在一些理论、方法上的缺陷——权值退化［2］与粒子多样性丧失问题。权值退化是序贯重要性采样（Sequential Importance Sampling，SIS）难以避免的问题，文献［3］中指出这是由于贝叶斯公式的函数相乘形式导致的。1993 年，Gordon 等引入重采样方法，基于同分布原则［4］，对权值更新后的粒子集合重新采样，获得一个粒子权值相等的新的粒子集，有效缓解了权值退化问题［5］；然而，对严重退化的粒子集进行无偏重采样所得到的粒子可能几乎全部源自极少数粒子的自我复制，造成粒子多样性丧失的问题。针对上述问题，国内外学者做了大量的研究，主要集中在提议分布（重要性采样函数）的选取与重采样的优化中：文献［6］通过参考最新观测信息提出辅助粒子滤波器；文献［7］通过非线性高斯滤波器来获得近似后验，并以此作为提议分布，提出无迹粒子滤波器（Unscented Particle Filter，UPF）；文献［8］提出混合多核偏最小二乘粒子滤波方法；文献［9］引入智能算法，将蝙蝠算法与粒子滤波相结合；文献［10］提出了基于似然分布调整的粒子群优化粒子滤波方法；文献［11］采用粒子滤波应用于多扩展目标跟踪，并提出顺序采样方法来解决维度灾难的问题；文献［12-13］提出粒子流滤波（Particle Flow Filter，PFF），以粒子流动的方式来计算贝叶斯公式。文献［13］中指出粒子流滤波器比经典粒子滤波器快了好几个数量级，而且对于复杂非线性问题比扩展卡尔曼滤波精确几个数量级。

1 贝叶斯估计与粒子滤波

1.1 叶斯滤波

对于状态估计问题（定位、跟踪、动态参数）的状态空间模型可以描述为：

其中：t 表示连续时间，k 表示离散时间；状态xt、xk∈Rd为连续、离散时间状态；wt、wk-1为过程噪声；ft、fk为连续、离散状态转移方程；zk∈Rm为k时刻的观测值，h为观测方程，vk为观测噪声。

贝叶斯滤波为非线性系统的状态估计问题提供了一种基于概率分布形式的解决方案。贝叶斯滤波包括预测和更新两个过程，假设k -1时刻概率密度已知为p(xk-1|z1：k-1)，预测过程即获取先验概率密度，由Chapman-Kolmogorow方程有：

然后，利用贝叶斯公式求解后验概率密度p(xk|z1：k)实现更新过程：

其中：p(xk|z1：k)为后验概率密度、p(xk|z1：k-1)为先验概率密度、p(zk|xk)为似然，分母p(zk|z1：k-1)为归一化常数：

1.2 基本粒子滤波器

粒子滤波基于蒙特卡罗模拟方法，利用所求状态空间中大量的样本点来近似状态后验分布，从而将积分问题转换为有限样本的求和问题。然而在实际计算中通常无法直接从后验分布中进行采样，因此需要引入一个易于采样的重要性函数生成采样样本，从而利用一组加权的样本xk=来近似后验概率密度：

式（6）中，权值更新公式为：

对粒子权重进行归一化处理后即可用样本均值代替复杂的积分运算。式（3）～（7）即SIS，文献［2］指出SIS 存在严重的粒子权值退化现象。权值退化指滤波器经过多次迭代后，很多粒子的权重都变得很小（趋近于0），只有少数粒子（甚至一个）的权重比较大，并且粒子权值的方差随着时间增大，状态空间中的有效粒子数较少，大幅浪费计算资源。因此在SIS之后再进行重采样在一定程度上解决了这个问题，形成了常见的SIR（Sampling Importance Resampling）粒子滤波器。

1.3 粒子流滤波器

粒子流滤波是将状态空间中服从先验分布的粒子移动到其对应的后验分布上，以粒子流动的方式代替贝叶斯公式实现贝叶斯滤波。粒子流滤波需要建立一个微分方程来平滑移动粒子，得到后验随时间变化的形式。

文献［12-13］中，忽略式（4）的分母获得未归一化的贝叶斯公式。为简化表达，此处以p(x) 表示后验概率密度p(xk|z1：k)，g(x)表示先验概率密度p(xk|z1：k-1)，l(x)表示似然p(zk|xk)，则未归一化贝叶斯公式可以简化为：

对式（8）两端同时取对数并定义如下同伦函数：

式中：f(x，λ)为满足Fokker-Planck方程的函数，w为过程噪声，可以认为f(x，λ)为粒子从先验分布移动到后验分布的“速度场”。

不考虑噪声w，p(x，λ)满足零扩散项的Fokker-Planck 方程［15］，有式（11）成立：

其中∇⋅(⋅)表示(⋅)的散度。对式（9）求λ 偏导，并结合式（11），可得：

由式（12）求出f(x，λ)后对式（10）进行0到1积分，将先验粒子平滑移动到后验分布上。其中，f(x，λ)的求解是粒子流滤波中一个难点，文献［16］给出了f(x，λ)的17种求解方法。

2 改进粒子流滤波器

2.1 粒子流模型改进优化

与粒子流滤波不同的是，本文考虑归一化形式下的贝叶斯公式。在粒子流动过程中引入了“新息误差”结构，使用Galerkin 有限元法求得f(x，λ)的弱形式解（数值解）。将式（1）和（2）的滤波问题，重写为下列随机微分方程（Stochastic Differential Equation，SDE）形式：

其中：Xt∈Rd为t时刻的状态；Ztn为t=tn时刻的观测值；a(⋅)、h(⋅)为状态转移函数与测量函数；{Bt}为独立于X 的d 维标准维纳过程；Wtn是一个独立于X 的m 维变量，每个分量都服从高斯分布，协方差为R。通常将函数h(⋅)表示为列向量h=(h1(x)，h2(x)，…，hm(x))，第j个元素表示为hj，假设h 对x可微。

2.2 求解映射P*与P

映射P*与P可以分为两部分：第一部分为由n -1时刻的后验通过状态方程得到n时刻的先验，即由由两者是相同的；第二部分即由n 时刻的先验得到后验，对于P*第二部分由贝叶斯公式即式（4）得到，构造同伦函数：

其中：l(x)为似然，λ ∈[0，1]。由式（16）可知同伦函数定义了粒子从先验分布（λ=0）到后验分布（λ=1）的变化过程中的概率分布，l(x)表达式为：

对式（16）两端同时取对数：

对式（18）两端对λ求偏导有：

式（19）即映射P*的第二部分，接下来求映射P 的第二部分，由式（11）有：

注意到式（19）的表达形式，将f(x，λ)分为包含最新观测Ztn与不包含最新观测两部分。

引入新息误差，令：

其中K(x，λ)、v(x，λ)满足连续二次可微，且满足边界条件：

式中：K(x，λ)为d × m 矩阵，可以表示为K=[∇φ1，∇φ2，…，∇φm]，∇表示微分算子，∇φj表示φj的梯度；v(x，λ)为d × 1 矩阵。为简化表达，用pn、K、v 表示pn(x，λ)、v(x，λ)、K(x，λ)，故式（20）可以写为：

式中，K(x，λ)为式（22）、（24）BVP 方程的解。同理，v(x，λ)满足下列BVP方程：

式（26）与式（19）具有相同的表达形式，由此可知在先验相等的情况下映射P、P*是相同的，即后验结合式（21）与式（13）有：

由式（27）可知，本文提出的改进粒子流滤波引入了“新息误差”结构Ztn-h(xi)，粒子流在求解f(x，λ)时得到的弱形式解与观测方程有关涉及(Ztn-h(xi))2项。而且，本文提出的改进算法在求解BVP 方程时未引入最新的观测，与粒子流滤波相比，一定程度上削弱了对观测的依赖性。

2.3 求解BVP方程

对于固定时刻t有K=[∇φ1，∇φ2，…，∇φm]，求解BVP方程（24），其弱形式解可定义如下，对任意一阶可微函数（测试函数）满足式（28）：

基于Galerkin 有限元法可以用有限维空间中一组基函数的线性组合来近似φj(x，λ)：

故式（28）可以表示为：

将式（31）中的ψ(x)由ψl(x)替换，令L=d，则式（31）可以表示为由d 个方程组成的线性矩阵方程Amj=b，Asl表示A 中第s行l列元素，b中第s个元素表示为bs，故A、b可以表示为：

结合式（32）与（34）可知，A=I 为一个单位阵，即可以得到mj=b。

由式（30）可知：

由式（35）可以得到K(x，λ)为：

同理可得BVP方程式（25）的解：

2.4 改进粒子流滤波算法步骤

综上所述，改进的粒子流滤波算法步骤如下：

步骤1 初始化，从初始分布p*(0)中采样N个样本；

步骤2 从t=1到t=T，执行步骤3～步骤5；

步骤4 λ=0，执行映射P；

1）如果λ ≤1；

3）计算新息误差

5）令λ=λ+Δλ，返回1）；

步骤6 t=t+1，返回步骤3。

3 实验仿真结果与分析

3.1 一维非线性滤波模型

选择非线性一维系统的状态方程为：

其中：w(k)、v(k)符合高斯分布，均值为0、方差为Q、R。

用粒子滤波、粒子流与本文提出的改进的粒子流滤波算法对该非线性系统进行状态估计和跟踪。由于粒子滤波结果存在随机性，进行30 次蒙特卡洛仿真。选择性能指标为均方根误差（Root Mean Square Error，RMSE）：

取粒子数N=50，仿真时长T=50，粒子流与改进粒子流取相同时间步长Δλ。表1 为在R、Q、Δλ 在不同设定下，状态估计偏差和运行时间的对比。当Q=0.1、R=0.1 时，仿真结果如图1（a）所示；当Q=1、R=0.1 时，仿真结果如图1（b）所示。

图1 Q为0.1或1时的状态估计均方根误差Fig.1 RMSE of state estimation with Q of 0.1 or 1

由图1 可知，当过程噪声较小时，三种算法均可取得较好的结果；在过程噪声较大时，粒子流滤波效果较差。这是由于噪声较大时求得的f(x，λ)误差较大，粒子滤波与改进粒子流滤波结果相对较好，其中改进的粒子流滤波具有更高的精度。从表1 可以看出，当过程噪声与量测噪声都较大时，改进粒子流滤波仍可取得较好的结果。

此外由表1可以看出，PFF与改进的PFF算法运行时间高于粒子滤波。PFF 与改进的PFF 算法运行时间取决于时间步长Δλ，当Δλ 相同时，本文算法与粒子流算法运行时间相近。这是因为本文改进了“速度场”f(x，λ)的结构，提高了滤波精度的同时并没有增加计算量。

表1 三种算法仿真结果对比Tab.1 Comparison of simulation results of three algorithms

当N=50，过程噪声为伽马噪声w～Γ(3，2)时，对比PF、UPF、PFF、改进PFF算法，仿真结果如图2所示。

由图2 可知，在过程噪声为伽马噪声情况下，PFF 算法无法进行较好的估计；在测量噪声较大情况下，改进PFF算法有相对较好的效果；在测量噪声较小时，UPF 与改进的PFF 算法有较好的效果。文献［17］证明当观测噪声较为显著时，基于最新观测优化的提议分布并不能带来更高的滤波精度。

由上述仿真实验可以看出，粒子滤波、粒子流滤波、UPF在过程噪声较小、观测信息比较精确时才更有效。原因是PF利用最新的观测信息计算权重，PFF、UPF 参考最新的观测信息来优化提议分布。另外利用智能算法、启发式算法与粒子滤波结合也是基于最新的观测，然而多数方案缺乏坚实的理论证明，且以增加计算复杂度为代价。其中与观测相关的项设为b=(Zk-h(xk))，粒子滤波在计算权重时是根据观测方程来计算的涉及项；粒子流在求解f(x，λ)时，得到的弱形式解涉及b2项，且弱形式解为数值解，当噪声变大时得到的解的误差较大；改进的粒子流滤波虽然也利用了最新的观测新息，但是作为新息误差项而引入只涉及b，从而削弱了观测的影响。而且，BVP方程求解也未引入最新观测，一定程度上抑制了系统对观测的依赖性。

图2 不同噪声R下的状态估计均方根误差Fig.2 RMSE of state estimation under different noise R

3.2 机动目标跟踪模型

以单站单目标纯方位角度跟踪系统模型为例，本文目标的机动模型为恒速度模型（Constant Velocity，CV）与恒转向模型 （Constant Turn，CT），目标的状态为X(k)=[xp(k)，xv(k)，yp(k)，yv(k)]T，状态的4 个分量分别代表x方向坐标、x方向速度、y方向坐标、y方向速度。目标的状态方程如下：

其中：w(k)、v(k)为过程噪声与测量噪声，且满足均值为零，协方差为Q与R的高斯分布，(x0，y0)为基站坐标。

在CV 模型下状态转移矩阵为ΦCV，在CT 模型下为ΦCT，目标初始状态为 X(0)=[1000，10，1500，15]T，w(k)=[wxp(k)，wxv(k)，wyp(k)，wyv(k)]T，各参数值如下：

当粒子数都为N=50，目标初始位置(x0，y0)=（1 355.33，1 736.23）（随机取得）。由于粒子滤波存在随机性，在上述条件下进行200次蒙特卡洛仿真。仿真时长为60 s，采样周期为1 s，系统距离的噪声方差为r1=0.05，速度方差为r2=0.1。目标开始按照CV 模型运动，40 s后按照CT 模型以角速度ω=5°/s 做匀转弯运动。选取性能指标为目标位置均方根误差，仿真结果如表2、图3 所示，其中N1、N2分别表示PF 与改进PFF粒子数。

表2 两种算法跟踪性能比较Tab.2 Tracking performance comparison of two algorithms

由表2、图3 可知在相同条件下，改进的粒子流滤波具有更高的跟踪精度与计算效率。

从表2可以看出，改进的粒子流滤波在粒子数为50时，跟踪精度已高于粒子滤波在粒子数为200 时的精度，并且算法运行时间更短。这是因为在满足一定数值近似误差要求下，蒙特卡罗近似所需的样本数随着状态维度的增加会急剧增加，俗称维数灾难［18］，而且高维状态空间的粒子权值更容易退化。在实验3.1 节中，改进粒子流滤波运行时间更长是因为在处理一维情况下粒子滤波运算效率高，但在处理多维情况下粒子滤波运算时间将会大大增加。综合考虑滤波精度与运行时间，改进的粒子流算法具有较高的综合性能。

另外，从表2 可以看出，在过程噪声变大的情况下，粒子滤波已经不能较好地反映真实的轨迹，误差已经比较大，而改进的粒子流滤波依然能相对较好地反映真实的轨迹，各状态估计的误差保持在相对较小的范围内，证明本文算法具有较好的鲁棒性。另外粒子滤波在粒子数增大的情况下滤波精度有明显提高，而改进的粒子流滤波在粒子数增加的情况下滤波精度并没有提高，这是由于改进的算法避免了重采样对粒子的舍弃，而粒子数较多时，会导致低似然区域的粒子增加。这也说明了改进的粒子流算法适用于粒子数较少时的高精度快速预测，例如激光雷达、红外目标跟踪等。

图3 PF和改进PFF的仿真结果比较Fig.3 Comparison of simulation results of PF and improved PFF

4 结语

本文在粒子流动过程中引入了新息误差，构造了一种与卡尔曼滤波相似的结构。与基于最新观测信息优化粒子滤波的方法相比在一定程度上削弱了对观测的依赖性，在观测误差较大情况下也有相对较好的结果。由于算法过程中利用粒子流动形式实现贝叶斯公式以及未进行重采样，故不存在粒子权值退化问题与粒子多样性丧失问题。实验结果表明，本文提出的改进方法，在提高状态估计精度的同时，在处理多维情况下运算效率更高，具有较高的综合性能。本文在求解BVP 方程时并未对其解析解进行分析，下一步研究方向是如何求得解析解及其满足的条件以及如何获得更优的数值解，提高滤波精度。