粒子滤波器 particle filter

粒子滤波是一种基于蒙特卡洛模拟的非线性滤波方法，其核心思想是用随机采样的粒子表达概率密度分布。

粒子滤波的三个重要步骤为：
1）粒子采样，从建议分布中抽取一组粒子；
2）粒子加权，根据观测概率分布，重要性分布以及贝叶斯公式计算每个粒子的权值；
3）估计输出，输出系统状态的均值协方差等。
此外 ，为了应对粒子退化现象，还采用了重采样等策略。

蒙特卡洛模拟

蒙特卡洛是一种利用随机数来计算物理和数学问题的计算方法。其基本原理：
对于高维空间 X 上的高维积分：
其中p(x) 是随机变量X 的概率分布,f(X) 是关于p(x)的任意可积函数。如果从概率分布p(x) 上抽取N 个独立样本
则样本集是独立同分布。而且有P(x)可以表示为：
则高维积分可以近似的表示为：
这种基于随机模拟的积分称为蒙特卡洛积分。

如果概率分布p(x)具有标准固定的形式，比如高斯分布，则直接从p(x)中抽取粒子，但是通常p(x) 的分布未知，或者十分复杂，所以需要其他的采样策略进行采样，在后续小节中介绍重要性采样。

标准的粒子滤波器

由上述分析我们可以得出，对于系统的后验概率 p(X0:k|Z1:k),对其进行采样会非常困难，那么我们选取一个非高斯非线性的容易抽样的分布 q(X0:k|Z1:k)，则称

q(X0:k|Z1:k)为建议性分布（重要性函数），则从q(X0:k|Z1:k)中抽取N 个粒子有：
则对于后验概率p(X0:k|Z1:k)，
为归一化权值，且有：
由此可见，后验概率p(X0:k|Z1:k) 可以由 一组加权的粒子近似。而这样的采样方法称为重要性采样。

为了实现递推的贝叶斯状态估计，把建议性分布写成递推形式：
并且 由递推的贝叶斯估计公式，我们有：
将式子一和二带入权值

进行化简有：
如果假设状态 X 是一马尔科夫过程，且观测条件是相互独立的，则上式改写为：
于是后验概率 p(X0:k|Z1:k)为：
3.重采样策略
在标准粒子滤波中，经常出现退化现象，其表现为：经过若干次的地推计算后，除了少数粒子外，其余粒子的权值可以忽略不计，从而使得大量递推计算浪费在对几乎不起任何作用的粒子更新上，甚至最后只剩下一个权值很大的有效粒子，而其他粒子的权值几乎为零，从而产生一个退化分布。

针对粒子退化的问题，Gordon 等提出了一种Bootstrap 的粒子滤波的算法，该算法在每部迭代的过程中，根据粒子权值对粒子进行重采样，在一定的程度上克服了这个问题。重采样的方法舍弃了权值较小的粒子，代之以权值较大的粒子。重采样的策略包括固定时间间隔重采样，以及根据粒子权值就行动态重采样。
标准粒子滤波的算法流程：

( 1 ) 初始化：对于 K =0，