本文参考博客地址

粒子滤波

粒子滤波的状态转移方程和观测方程如下：

其中的x(t)为t时刻状态，u(t)为控制量，w(t)和e(t)分别为状态噪音和观测噪音，粒子滤波从观测y(t)和上个时刻状态x(t - 1)，u(t)，w(t)中过滤出t时刻的状态x(t)的过程如下：
1、初始状态：用大量粒子模拟X(t)，粒子在空间均匀分布。
2、预测阶段：要依据上个时间点系统的状态的概率分布来采样，产生的样本称为粒子，它们的分布就是上个时间点系统状态的概率分布，根据状态转移方程，每个粒子得到一个预测粒子；
3、校正阶段：根据条件概率对预测粒子进行评价，该条件概率值即为粒子的权重，越接近于真实状态的粒子，其权重越大。
4、重采样：根据粒子权重对粒子进行筛选，即要大量保留权重大的粒子，又要有一小部分权重小的粒子。
5、将重采样后的粒子带入步骤2状态转移方程得到新的预测粒子

粒子滤波理论

推导过程参考
贝叶斯滤波
贝叶斯滤波实际上就是根据上一时刻系统状态的概率分布来计算当前时刻系统状态的概率分布
从贝叶斯理论的角度看，状态估计问题(目标跟踪、信号滤波)就是根据之前一系列的已有数据(后验知识)递推地计算出当前状态的可信度，即，它需要通过预测和更新连个步骤来递推计算，方程如下：

预测过程：预测过程通过状态转移方程（公式1）来完成，即通过一些先验知识来对系统的未来状态进行猜测，猜的是可行度用条件概率来表示，更新过程中，利用新到的观测值来修正先验概率()得到后验概率。
在处理类似问题时，一般先假设系统的状态转移服从隐马尔科夫模型，即当前时刻的状态仅仅和上一个时刻的状态有关系。同时，假设k时刻的测量数据仅仅和k时刻的状态有关
公式如下：
$p(x_k|y_{1 : k - 1}) = \int p(x_{k},x_{k - 1}|y_{1 : k - 1})dx_{k - 1}\\ = \int p(x_{k}|x_{k - 1}, y_{1 : k - 1})p(x_{k - 1}|y_{1 : k})dx_{k - 1}\\ = \int p(x_{k}|x_{k - 1})p(x_{k - 1}|y_{1 : k})dx_{k - 1}\\$
更新过程：由得到后验概率，这是通过新到的观测值修正的过程，公式如下：

关于贝叶斯滤波的简介就到这里，我们可以注意到，贝叶斯后验概率的计算过程中需要用到积分，但是对于一般的非线性，非高斯系统，很难得到后验概率的解析解，所以就得引进蒙特卡洛采样了。

蒙特卡洛采样

蒙特卡洛采样通过与总体分布一致的采样样本(粒子)来估计总体分布某些函数(往往可以表示某一事件的发生)期望值的一种方法，其思想是用平均值来代替积分。
因为贝叶斯后验概率的计算过程中用到积分，为了解决积分难的问题，可以用蒙特卡洛采样来代替计算后验概率，假设可以从后验概率中采样到N个样本，那么后验概率的计算可以表示为：

其中为狄拉克函数(dirac delta function)，即为指示函数，用来表示某事件是否发生。
既然用蒙特卡洛方法能够用来直接估计后验概率，现在估计出了后验概率，那到底怎么用来做图像跟踪或者滤波呢？：
$E\left [ f(x_n) \right ]\approx \int f(x_n) \hat {P}\left ( x_{n} | y_{1 : k}\right ) dx_n \\ = \frac{1}{N}\sum_{i = 1}^{N}\int f(x_n)\delta \left ( x_n - x_n^{(i)} \right )dx_n\\ =\frac{1}{N}\sum_{i = 1}^{N}f(x_n^{(i)})$
从上面可知，粒子滤波实际上就是从后验概率（通过贝叶斯滤波或蒙特卡洛采样得到）中采样很多粒子，用它们的状态(以粒子为参数的狄拉克函数，表示某事件是否发生)求平均就得到了滤波结果。
蒙特卡洛采样的假设是可以从后验概率分布中采样N个样本，但是此时后验概率未知，所以，直接用是不行的，我们需要祭出重要性采样这个大神了

重要性采样

重要性采样的核心思想就是从任意一个已知的分布(可以对应系统任意状态？)中采样，而在用均值估计期望时与蒙特卡洛采样不同的是它采用例子的加权平均值估计系统当前的期望，下面推导权重的计算。
设当前系统状态分布为即是可采样的，这样以上求期望的式子可写成（2）式

其中

由于

(2)式可进一步写成：

上面的期望计算都可以采用蒙特卡洛方法来解决，也就是说，通过采样N个样本，再用样本的均值来估计当前系统状态期望，所以(3)式可近似为：

其中

以上的权重是归一化后的权重，而(2)式中的权重是没有归一化的。
到这里已经解决了不能从后验概率直接采样的问题，但是上面这种每个粒子的权重都直接计算的方法，效率低，因为每增加一个采样都需要重新计算，所以我们需要寻找一种能避免每次增加一个采样都计算的方法，对的，我们要引入的递推公式，推导如下：
首先假设重要性概率密度函数，这里x的下标是0:k，也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为：

则后验概率密度函数的递推形式可以表示未：

其中，为了表示方便，将用来表示，注意 Y 与 y 的区别。同时，上面这个式子和上一节贝叶斯滤波中后验概率的推导是一样的，只是 之前的x(k)变成了这里的x(0:k)，就是这个不同， 导致贝叶斯估计里需要积分，而这里后验概率的分解形式却不用积分。
粒子权值的递归形式可以表示为:

注意，这种权重递推形式的推导是在前面（2）式的形式下进行推导的，也就是没有归一化。而在进行状态估计的公式为：

这个公式中的的权重是归一化以后的，所以在实际应用中，递推计算出w(k)后,要进行归一化，才能够代入(4)式中去计算期望。
上面(5)式中的分子是不是很熟悉，在上一节贝叶斯滤波中我们都已经做了介绍，,的形状实际上和状态方程中噪声的概率分布形状是一样的，只是均值不同了。因此这个递推的(5)式和前面的非递推形式相比，公式里的概率都是已知的，权重的计算可以说没有编程方面的难度了。权重也有了以后，只要进行稍微的总结就可以得到SIS Filter。
重要性采样通过引入重要性概率密度函数来解决蒙特卡洛采样中后验概率无法采样的问题，如今已经有很多经典的算法来解决关于如何从后验概率采样（也就是如何生成特定概率密度的样本）的问题（如拒绝采样，Markov Chain Monte Carlo，Metropolis-Hastings算法，Gibbs采样），可以参考博文

序贯重要性采样滤波(Sequential Importance Sampling(SIS) Filter)

在实际应用中我们假设重要性分布函数满足：

这个假设说明重要性分布只和前一时刻的状态以及测量有关了，那么(5)式就可以转化为：

下面是序贯重要性滤波算法：
SIS采样形式化如下

具体伪代码表示如下
For i = 1 : N

采样：
根据

递推计算各个例子的权重
End For

这个算法（SIS）即为粒子滤波的前身，因为它的实践体验中有很多问题（如：粒子权重退化）因此需要通过重采样（resample）去解决，采用重采样后，就产生了基本的粒子滤波算法。
这个算法还有一个问题是重要性概率密度函数的选择问题。

重采样

在SIS采样滤波中，有一个粒子退化的问题，主要表现在1）经过迭代后有些粒子的权重变得很小，可以忽略了2）粒子权值的方差越来越大，这样就会造成无效采样，使得大量的计算浪费在对估计后验概率分布几乎不起作用的粒子上，使算法性能下降。
通常采用有效厘子湖来衡量粒子退化程度即：

这个式子的含义是，有效粒子数越小，即权重的方差越大，也就是说权重大的和权重小的之间差距大，表明权值退化越严重。在实际计算中，有效粒子数可以近似为：

在进行序贯重要性采时，若上式小于一定的阈值，则应该采取一些措施加以制止。
克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。因此，一般采用以下两种途径：(1)选择合适的重要性概率密度函数；(2)在序贯重要性采样之后，采用重采样方法。
这里重要讨论第二种，即重采样
重采样的思路很简单，它会舍弃权重很小的例子，同时通过重复权重较大的粒子来保持粒子数目不变，复制数量是根据粒子权重大小占比分配的
前面提到，求某种期望问题变成了加权和的形式：

通过重采样之后，期望的表示形式时下面这样的：

对比1、2我们就可以发现，重采样之每个例子的权重都是1/N(1式中的表示重采样前的粒子，2式中的表示重采样之后的粒子)，2式中的表示重采样后粒子重复次数，它可能为0
有了思路之后，具体操作方法请参考遗传算法中的轮盘赌
将重采样的方法放入之前的SIS算法中，便可以得到基本的粒子滤波算法。

至此，粒子滤波的基本算法已经给出来了，下面介绍SIR粒子滤波

Sampling Importance Resampling Filter(SIR)

粒子滤波学习笔记