粒子滤波详解

2.4粒子滤波例子滤波是以贝叶斯滤波和重要性采样为基本框架的。因此，想要掌握例子滤波，对于上述两个基本内容必须有一个初步的了解。重要性采样呢，其实就是根据对粒子的信任程度添加不同的权重，添加权重的规则就是：对于我们信任度高的粒子，给它们添加的权重就相对大一些；否则，就加的权重小一些。根据权重的分布形式，实际上就是它与目标的相似程度。粒子滤波的结构实际上就是加一层重要性采样思想在里面的蒙特卡罗方法（MonteCarlomethod,即以某时间出现的频率来指代该事件的概率）。该方法的基本思想是用一组样本（或称粒子）来近似表示系统的后验概率分布，然后使用这一近似的表示来估计非线性系统的状态。采用此思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman滤波只能处理线性高斯分布的概率问题。粒子滤波的一大优势也在于此，因此近年来该算法在许多领域得到成功应用。2.4.1贝叶斯滤波理论贝叶斯滤波泛指一类以贝叶斯定理为基础的滤波技术，其根据所获得的观测，对状态后验概率分布、状态先验概率分布、状态估计值以及状态预测值等感兴趣量进行递归计算。假设有一个系统，我们知道它的状态方程，和测量方程如下：𝑥𝐾=𝑓𝑘(𝑥𝑘−1,𝑣𝑘−1)（状态方程）（2.4.1）𝑦𝐾=ℎ𝑘(𝑥𝑘,𝑛𝑘)（测量方程）（2.4.2）其中x为系统状态，y为测量到的数据，f,h是状态转移函数和测量函数，v,n为过程噪声和测量噪声，噪声都是独立同分布的。由贝叶斯理论可知，状态估计问题（目标跟踪、信号滤波）就是根据之前一系列的已有数据𝑦1:𝑘（测量数据）递推的计算出当前状态𝑥𝑘的可信度，这个可信度就是概率公式p(𝑥𝑘|𝑦1:𝑘)，它需要通过预测和更新两个步奏来递推的计算。预测过程是利用系统模型(状态方程2.4.2)预测状态的先验概率密度，也就是通过已有的先验知识对未来的状态进行猜测，即p(𝑥𝑘|𝑥𝑘−1)。更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度，也就是对之前的猜测进行修正。处理这些问题之前，假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态x(k)只与上一个时刻的状态x(k-1)有关,k时刻测量到的数据y(k)只与当前的状态x(k)有关。预测阶段：为了进行递推，不放假设已知k-1时刻的概率密度函数p(𝑥𝑘−1|𝑦1:𝑘−1)，先不知k时刻的测量数据𝑦𝑘，预测状态x(k)出现的概率为：p(𝑥𝑘|𝑦1:𝑘−1)=∫p(𝑥𝑘|𝑥𝑘−1)p(𝑥𝑘−1|𝑦1:𝑘−1)𝑑𝑥𝑘−1。（2.4.3）更新阶段：当获取k时刻的观测值𝑦𝑘后，利用观测值修正先验概率密度p(𝑥𝑘|𝑦1:𝑘−1)，得到后验概率密度p(𝑥𝑘|𝑦1:𝑘)这就是滤波了。p(𝑥𝑘|𝑦1:𝑘)=p(𝑦𝑘|𝑥𝑘,𝑦1:𝑘−1)p(𝑥𝑘|𝑦1:𝑘−1)p(𝑦𝑘|𝑦1:𝑘−1)=p(𝑦𝑘|𝑥𝑘)p(𝑥𝑘|𝑦1:𝑘−1)p(𝑦𝑘|𝑦1:𝑘−1)（2.4.4）其中归一化常数：p(𝑦𝑘|𝑦1:𝑘−1)=∫p(𝑦𝑘|𝑥𝑘)p(𝑥𝑘|𝑦1:𝑘−1)𝑑𝑥𝑘（2.4.5）可得：p(𝑥𝑘|𝑦1:𝑘)=p(𝑦𝑘|𝑥𝑘)p(𝑥𝑘|𝑦1:𝑘−1)∫p(𝑦𝑘|𝑥𝑘)p(𝑥𝑘|𝑦1:𝑘−1)𝑑𝑥𝑘（2.4.6）综上所述，（2.4.3）和（2.4.6）实现了由k-1时刻后验滤波概率p(𝑥𝑘−1|𝑦1:𝑘−1)到k时刻后验滤波概率p(𝑥𝑘|𝑦1:𝑘)的递推更新过程。由测量方程（2-16）可知y(k)只与x(k)有关，p(𝑦𝑘|𝑥𝑘)也称之为似然函数，只和测量噪声n(k)的概率分布有关：p(𝑦𝑘|𝑥𝑘)=∫δ(𝑦𝑘−h(𝑥𝑘,𝑛𝑘))p(𝑛𝑘)𝑑𝑛𝑘（2.4.7）2.4.2蒙特卡洛采样蒙特卡洛方法（MonteCarloMethod）也称统计模拟方法，是20世纪40年代中期由于科学技术的发展和电子计算机的发明而被提出的一种以概率统计理论为指导的一类非常重要的数值计算方法。蒙特卡洛方法的基本原理是，事件的概率可以用大量试验中发生的频率来估计，当样本容量足够大时，可以认为该事件发生的频率即为其概率。因此，可以先对影响其可靠度的随机变量进行大量的随机抽样，然后把这些抽样值一组一组地代入功能函数式，确定结构是否失效，最后从中求得结构的失效概率。蒙特卡洛方法在金融工程学、宏观经济学、计算物理学（如粒子运输计算、量子热力学计算、空气动力学计算）等领域应用广泛。假设我们能得到一些服从一个目标概率分布p(x)的样本（𝑥1,𝑥2……𝑥𝑁）（每个样本看成一个粒子）。那么这些粒子作为一部分自变量的某些函数的期望值：E[f(x)]=∫𝑥𝑓(𝑥)𝑑𝑥=∫𝑓(𝑥)𝑝(𝑥)𝑑𝑥𝑁1（2.4.8）蒙特卡洛采样的思想就是用部分样本的平均值代替积分，求期望：E[f(x)]≈𝑓(𝑥1)+𝑓(𝑥2)+⋯+𝑓(𝑥𝑁)𝑁（2.4.9）在上一节中，贝叶斯后验概率的计算里要用到积分（2.4.5），但这个积分很难直接算出。为了解决这个积分难的问题，可以用蒙特卡洛采样来代替计算后验概率。假设能从后验概率中采样到N个样本，那么后验概率的计算可近似表示为：𝑝̂(𝑥𝑛|𝑦1:𝑘)=1𝑁∑𝛿(𝑥𝑛−𝑥𝑛(𝑖))𝑁𝑖=1≈p(𝑥𝑛|𝑦1:𝑘)（2.4.10）当前状态的期望值：E[f(𝑥𝑛)]≈∫𝑓(𝑥𝑛)𝑝̂(𝑥𝑛|𝑦1:𝑘)d𝑥𝑛=1𝑁∑𝑓(𝑥𝑛(𝑖))𝑁𝑖=1（2.4.11）通过这些采样的粒子（样本）的状态值直接平均就得到了期望值，也就是滤波后的值，这里的f(x)就是每个粒子的状态函数。这就是粒子滤波了，只要从后验概率中采样很多粒子，用它们的状态求平均就得到了滤波结果。2.4.3贝叶斯重要性采样在蒙特卡洛采样中，我们从后验概率中采集到了N个样本（粒子），但事实上后验概率不知道啊，怎么从后验概率分布中采样！所以直接应用是行不通的。无法直接从后验概率分布p(𝑥𝑘|𝑦1:𝑘)中采样，就从一个已知的可以采样的分布q(𝑥𝑘|𝑦1:𝑘)里去采样：那么上节里当前状态的期望值：E[f(𝑥𝑘)]≈∫𝑓(𝑥𝑘)𝑝̂(𝑥𝑘|𝑦1:𝑘)d𝑥𝑛=∫𝑓(𝑥𝑘)p(𝑥𝑘|𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)d𝑥𝑘=∫𝑓(𝑥𝑘)p(𝑦1:𝑘|𝑥𝑘)p(𝑥𝑘)p(𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)d𝑥𝑘=∫𝑓(𝑥𝑘)𝑊𝑘(𝑥𝑘)p(𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)d𝑥𝑘（2.4.12）其中权值𝑊𝑘(𝑥𝑘)=p(𝑦1:𝑘|𝑥𝑘)p(𝑥𝑘)q(𝑥𝑘|𝑦1:𝑘)∝p(𝑥𝑘|𝑦1:𝑘)q(𝑥𝑘|𝑦1:𝑘)（2.4.13）由因为测量数据的概率p(𝑦1:𝑘)=∫p(𝑦1:𝑘|𝑥𝑘)p(𝑥𝑘)𝑑𝑥𝑘（2.4.14）（2.4.12）可以进一步写成E[f(𝑥𝑘)]=𝐸q(𝑥𝑘|𝑦1:𝑘)[𝑊𝑘(𝑥𝑘)𝑓(𝑥𝑘)]𝐸q(𝑥𝑘|𝑦1:𝑘)[𝑊𝑘(𝑥𝑘)]（2.4.15）所以，通过采样N个样本{𝑥𝑘(𝑖)}～q(𝑥𝑘|𝑦1:𝑘),用样本的平均来求它们的期望，（2.4.15）可近似为E[f(𝑥𝑘)]=∑𝑊̃𝑘(𝑥𝑘(𝑖))𝑁𝑖=1𝑓(𝑥𝑘(𝑖))（2.4.16）其中𝑊̃𝑘(𝑥𝑘(𝑖))=𝑊𝑘(𝑥𝑘(𝑖))∑𝑊𝑘(𝑥𝑘(𝑖))𝑁𝑖=1（2.4.17）贝叶斯重要性采样提出了重要性概率密度的概念，从一个已知分布中进行采样得到独立同分布的采样点，取代从后验概率密度中进行采样，从理论上提供了解决采样难的方法。2.4.4序列重要性采样(SIS)贝叶斯重要性采样解决了在不知后验概率的前提下，无法从后验概率采样的问题。但估计后验滤波概率需要利用所有的观测数据，每次新的观测数据到来p(𝑥𝑘|𝑦1:𝑘)都得重新计算，每个粒子的权重𝑤𝑘(𝑥𝑘(𝑖))也需重新计算，这样会造成计算量过大。为了解决这一问题，人们提出了序列重要性采样（SequentialImportanceSampling,SIS）。假设重要性概率密度q(𝑥0:𝑘|𝑦1:𝑘)可以分解为q(𝑥0:𝑘|𝑦1:𝑘)=q(𝑥0:𝑘−1|𝑦1:𝑘−1)q(𝑥𝑘|𝑥0:𝑘−1,𝑦1:𝑘)（2.4.18）设系统的状态是一个马尔科夫过程，且给定系统状态下各次观测独立，则有p(𝑥0:𝑘)=p(𝑥0)∏𝑝(𝑥𝑖|𝑥𝑖−1)𝑘𝑖=1（2.4.19）p(𝑦1:𝑘|𝑥1:𝑘)=∏𝑝(𝑦𝑖|𝑥𝑖)𝑘𝑖=1（2.4.20）此时，通过参考分布q(𝑥0:𝑘−1|𝑦1:𝑘−1)得到样本集{𝑥0:𝑘−1(𝑖),𝑖=1,2,…,𝑁}以及从q(𝑥𝑘|𝑥0:𝑘−1,𝑦1:𝑘)得到样本点𝑥𝑘(𝑖)，这样就可得到新的样本集{𝑥0:𝑘(𝑖),𝑖=1,2,…,𝑁}粒子权值的递归形式如下𝑊𝑘(𝑖)∝𝑝(𝑥0:𝑘(𝑖)|𝑌𝑘)𝑞(𝑥0:𝑘(𝑖)|𝑌𝑘)=𝑊𝑘−1(𝑖)𝑝(𝑦𝑘|𝑥𝑘(𝑖))𝑝(𝑥𝑘(𝑖)|𝑥𝑘−1(𝑖))𝑞(𝑥𝑘(𝑖)|𝑥0:𝑘−1(𝑖),𝑌𝑘)（2.4.21）通常，还需要对粒子权值进行归一化处理，即𝑊𝑘(𝑖)=𝑊̃𝑘(𝑖)∑𝑊̃𝑘(𝑖)𝑁𝑖=1（2.4.22）K时刻，系统状态𝑥𝑘的估计值可表示为𝑥𝑘=∑𝑊𝑘(𝑖)𝑥𝑘(𝑖)𝑁𝑖=1（2.4.23）序列重要性采样算法（SIS）以递推的方式实现了贝叶斯重要性采样，以递归更新的方式实现了对粒子权值的重新计算。2.4.5SIR算法及粒子退化在应用SIS滤波的过程中，存在一个粒子退化的问题。就是经过几次迭代以后，很多粒子的权重都变得很小，可以忽略了，只有少数粒子的权重比较大。并且粒子权值的方差随着时间增大，状态空间中的有效粒子数较少。随着无效采样粒子数目的增加，使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子上，使得估计性能下降。克服序列重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。因此，一般采用以下两种途径：(1)选择合适的重要性概率密度函数；(2)在序列重要性采样之后，采用重采样方法。对于第一种方法：选取重要性概率密度函数的一个标准就是使得粒子权值的方差{𝑊𝑘(𝑖)}𝑖=1𝑁最小。Doucet等给出的最优重要性概率密度函数为:q(𝑥𝑘(𝑖)|𝑥𝑘−1(𝑖),𝑦𝑘)=p(𝑥𝑘(𝑖)|𝑥𝑘−1(𝑖),𝑦𝑘)=𝑝(𝑦𝑘|𝑥𝑘(𝑖),𝑥𝑘−1(𝑖))𝑝(𝑥𝑘(𝑖)|𝑥𝑘−1(𝑖))𝑝(𝑦𝑘|𝑥𝑘−1(𝑖))（2.4.24）此时，粒子权值为𝑊𝑘(𝑖)=𝑊𝑘−1(𝑖)𝑝(𝑦𝑘|𝑥𝑘−1(𝑖))（2.4.25）以𝑝(𝑥𝑘(𝑖)|𝑥𝑘−1(𝑖),𝑦𝑘)作为重要性概率密度函数需要对其直接采样在实际情况中，构造最优重要性概率密度函数的困难程度与直接从后验概率分布中抽取样本的困难程度等同。因此我们更习惯采用重采样方法。重采样的思路是：既然那些权重小的不起作用了，那就不要了。要保持粒子数目不变，得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，并且权重越