1 简介

粒子滤波（Partical Filter，PF）就是通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被称为“粒子”，故叫做粒子滤波。

粒子滤波(PF: Particle Filter)的思想基蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。

2 基本算法

2.1 蒙特卡洛采样方法

假设我们能从一个目标概率分布 $p(x)$ 中采样到一系列的样本（粒子） $x_{1},x_{2},...,x_{N}$ ，就能利用这些样本去估计这个分布的某些函数的期望值。例如：

$E\left[ f(x) \right]=\int_{a}^{b}{f(x)p(x)dx}$

蒙特卡洛的思想就是利用平均值代替积分：

$E\left[ f(x) \right]=\int_{a}^{b}{f(x)p(x)dx}\approx \frac{f({{x}_{1}})+f({{x}_{2}})+...+f({{x}_{N}})}{N}$

在滤波、跟踪时，一般都是希望知道当前状态经过变换后的估计值

$E\left[ f({{x}_{n}}) \right]\approx \int{f({{x}_{n}})\hat{p}({{x}_{n}}|{{y}_{1:n}})d{{x}_{n}}}=\frac{1}{N}\sum\limits_{i=1}^{N}{\int{f({{x}_{n}})\delta ({{x}_{n}}-x_{n}^{(i)})d{{x}_{n}}}}$

$E\left[ f({{x}_{n}}) \right]\approx \frac{1}{N}\sum\limits_{i=1}^{N}{f(x_{n}^{(i)})}$

这里 $x_{n}^{(i)}$ ， $(i=1,2,...,N)$ 就是所谓的“粒子”。

2.2 重要性采样

上一节中给出了对于当前状态经过变换后的估计值的计算公式：

$E\left[ f({{x}_{k}}) \right]\approx \int{f({{x}_{k}})p({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}}$

这里状态的分布 $p(x_{k}|y_{1:k})$ 可能未知，于是可以利用一个已知的可以采样的分布去采样，这里假设该参考采样为 $q(x_{k}|y_{1:k})$ ，于是有

$E[f({{x}_{k}})]=\int{f({{x}_{k}})\cdot }\frac{p({{x}_{k}}|{{y}_{1:k}})}{q({{x}_{k}}|{{y}_{1:k}})}\cdot q({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}$

$E[f({{x}_{k}})]=\int{f({{x}_{k}})\cdot \frac{p({{y}_{1:k}}|{{x}_{k}})\cdot p({{x}_{k}})}{p({{y}_{1:k}})\cdot q({{x}_{k}}|{{y}_{1:k}})}\cdot q({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}}=\int{f({{x}_{k}})\cdot \frac{{{W}_{k}}({{x}_{k}})}{p({{y}_{1:k}})}\cdot q({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}}$

其中 $p({{y}_{1:k}})=\int{p({{y}_{1:k}}|{{x}_{k}})\cdot p({{x}_{k}})d{{x}_{k}}}$ ， ${{W}_{k}}({{x}_{k}})=\frac{p({{y}_{1:k}}|{{x}_{k}})\cdot p({{x}_{k}})}{q({{x}_{k}}|{{y}_{1:k}})}\propto \frac{p({{x}_{k}}|{{y}_{1:k}})}{q({{x}_{k}}|{{y}_{1:k}})}$ ，于是有

$E[f({{x}_{k}})]=\frac{\int{f({{x}_{k}}){{W}_{k}}({{x}_{k}})q({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}}}{\int{{{W}_{k}}({{x}_{k}})\cdot q({{x}_{k}}|{{y}_{1:k}})d{{x}_{k}}}}=\frac{{{E}_{q({{x}_{k}}|{{y}_{1:k}})}}[{{W}_{k}}({{x}_{k}})f({{x}_{k}})]}{{{E}_{q({{x}_{k}}|{{y}_{1:k}})}}[{{W}_{k}}({{x}_{k}})]}$

上面这个式子就可以利用蒙特卡洛方法去计算期望值。由蒙特卡洛方法 $E\left[ f({{x}_{k}}) \right]\approx \frac{1}{N}\sum\limits_{i=1}^{N}{f(x_{k}^{(i)})}$ 代入到上面的式子得到

$E[f({{x}_{k}})]=\frac{\frac{1}{N}\sum\limits_{i=1}^{N}{[{{W}_{k}}(x_{k}^{(i)})f(x_{k}^{(i)})]}}{\frac{1}{N}\sum\limits_{i=1}^{N}{[{{W}_{k}}(x_{k}^{(i)})]}}=\sum\limits_{i=1}^{N}{[{{{\tilde{W}}}_{k}}(x_{k}^{(i)})f(x_{k}^{(i)})]}$

其中的 ${{\tilde{W}}_{k}}(x_{k}^{(i)})$ 表示归一化的权重值

${{\tilde{W}}_{k}}(x_{k}^{(i)})=\frac{{{W}_{k}}(x_{k}^{(i)})}{\sum\limits_{i=1}^{N}{[{{W}_{k}}(x_{k}^{(i)})]}}$

上面的式子表示，对变量的变换之后的值，可以利用通过对一系列与该变量相关的粒子先进行变换，再通过加权求和的方式去估计，接下来的问题就是，如何求解权值。

因为 ${{W}_{k}}({{x}_{k}})\propto \frac{p({{x}_{k}}|{{y}_{1:k}})}{q({{x}_{k}}|{{y}_{1:k}})}$ ，而对 $p(x_{k}|y_{1:k})$ 的直接计算复杂，因此可以通过递推的方式求解：

$p({{x}_{0:k}}|{{y}_{1:k}})=\frac{p({{y}_{k}}|{{x}_{0:k}},{{Y}_{k-1}})\cdot p({{x}_{0:k}},{{Y}_{k-1}})}{p({{y}_{k}}|{{Y}_{k-1}})}=\frac{p({{y}_{k}}|{{x}_{k}})\cdot p({{x}_{k}}|{{x}_{k-1}})\cdot p({{x}_{0:k-1}}|{{Y}_{k-1}})}{p({{y}_{k}}|{{Y}_{k-1}})}$

$p({{x}_{0:k}}|{{y}_{1:k}})\propto p({{y}_{k}}|{{x}_{k}})\cdot p({{x}_{k}}|{{x}_{k-1}})\cdot p({{x}_{0:k-1}}|{{Y}_{k-1}})$

$q({{x}_{0:k}}|{{y}_{1:k}})=q({{x}_{0:k-1}}|{{y}_{1:k-1}})\cdot q({{x}_{k}}|{{x}_{0:k-1}},{{y}_{1:k}})$

上述公式中的 $Y_{k}$ 与 $y_{1:k}$ 是等价的。利用以上公式可以推出

$W_{k}^{(i)}\propto \frac{p(x_{0:k}^{(i)}|{{Y}_{k}})}{q(x_{0:k}^{(i)}|{{Y}_{k}})}=\frac{p(x_{0:k-1}^{(i)},{{Y}_{k-1}})}{q(x_{0:k-1}^{(i)},{{Y}_{k-1}})}\cdot \frac{p({{y}_{k}}|x_{k}^{(i)})\cdot p(x_{k}^{(i)}|x_{k-1}^{(i)})}{q(x_{k}^{(i)}|x_{0:k-1}^{(i)},{{y}_{1:k}})}$

$W_{k}^{(i)}\propto W_{k-1}^{(i)}\cdot \frac{p({{y}_{k}}|x_{k}^{(i)})\cdot p(x_{k}^{(i)}|x_{k-1}^{(i)})}{q(x_{k}^{(i)}|x_{0:k-1}^{(i)},{{y}_{1:k}})}$

这就是粒子的权值的递推公式。

2.3 序贯重要性采样

假设系统在 $k$ 时刻的状态只与 $k-1$ 时刻的状态有关，也就是具有马尔科夫性质，那么应有

$p({{x}_{k}}|{{x}_{0:k-1}},{{y}_{1:k}})=q({{x}_{k}}|{{x}_{k-1}},{{y}_{k}})$

于是粒子权值的递推公式就变成了以下形式

$W_{k}^{(i)}\propto W_{k-1}^{(i)}\cdot \frac{p({{y}_{k}}|x_{k}^{(i)})\cdot p(x_{k}^{(i)}|x_{k-1}^{(i)})}{q(x_{k}^{(i)}|x_{0:k-1}^{(i)},{{y}_{1:k}})}$

这样就能得出序贯重要性采样（Sequential Importance Sampling，SIS）的算法流程了：

输入 $k-1$ 时刻的所有粒子的值及其相应的权重，以及 $k$ 时刻的观测值 $y_{k}$
根据以下公式计算 $k$ 时刻所有的粒子的状态值以及相应的权重

$x_{k}^{(i)}\sim q\left( x_{k}^{(i)}|x_{k-1}^{(i)},{{y}_{k}} \right)$

$W_{k}^{(i)}\propto W_{k-1}^{(i)}\cdot \frac{p({{y}_{k}}|x_{k}^{(i)})\cdot p(x_{k}^{(i)}|x_{k-1}^{(i)})}{q(x_{k}^{(i)}|x_{0:k-1}^{(i)},{{y}_{1:k}})}$

2.4 重采样

在应用SIS滤波的过程中，存在一个粒子退化的问题，也就是说，经过几次迭代后，很多粒子的权重都变的很小，可以忽略掉，而只有少数的粒子的权重比较大。并且粒子权值的方差随着时间增大，状态空间中的有效粒子数较少，这样一来，无效采样粒子数的增加会使得大量的计算浪费在对后验滤波概率几乎不起作用的粒子上，最终使得算法的估计性能下降。

对于这样的粒子退化问题，通常采用有效粒子数来衡量粒子权值的退化程度，即

${{N}_{eff}}=\frac{N}{1+\operatorname{var}(W_{k}^{*(i)})}$

而上式又可以利用以下公式来近似计算

${{\hat{N}}_{eff}}=\frac{1}{\sum\limits_{i=1}^{N}{{{(W_{k}^{(i)})}^{2}}}}$

重采样的思路大致是这样的：舍去权重小的粒子，然后将权重大的粒子分割成若干个，最后使得分割得到的所有粒子的权重一致。基于未重采样的原粒子的后验概率密度为

$p({{x}_{k}}|{{y}_{1:k}})=\sum\limits_{i=1}^{N}{W_{k}^{(i)}\delta ({{x}_{k}}-x_{k}^{(i)})}$

经过重采样后变成

$p({{\tilde{x}}_{k}}|{{y}_{1:k}})=\sum\limits_{j=1}^{N}{\frac{1}{N}\delta ({{x}_{k}}-x_{k}^{(j)})}=\sum\limits_{i=1}^{N}{\frac{{{n}_{i}}}{N}\delta ({{x}_{k}}-x_{k}^{(i)})}$

其中 $x_{k}^{(i)}$ 表示原粒子， $n_{i}$ 表示该粒子应该分割的个数， $x_{k}^{(j)}$ 表示重采样后的粒子，经过重采样后，所有粒子的权值都为 $1/N$ 。实现的基本思路是轮盘赌思想，举个简单的粒子：在 $k$ 时刻有3个粒子，权重分别为0.1，0.1，0.8，则它们的累计概率为 $[0.1,0.2,1]$ ，接着对 $[0,1]$ 上的均匀分布随机采样3个值，假设为0.15，0.38，0.54，那么，就应该让第二个粒子分割一次，第三个粒子分割两次。

利用上面的方法就能完成每一个时刻粒子的重采样。

3 粒子滤波算法

3.1 标准的粒子滤波算法

（1）粒子集的初始化， $k=0$ ：

对于 $i=1,2,...,N$ ，（ $N$ 表示粒子数，粒子数越大，效果越好，但也会使得计算量越大）由分布 $p(x_{0})$ 生成采样粒子 $\{x_{0}^{(i)}\}_{i=1}^{N}$

（2）对于 $k=1,2,...$ ，执行：

重要性采样：对于 $i=1,2,...,N$ ，从重要性概率密度函数生成采样粒子 $\{\tilde{x}_{k}^{(i)}\}_{i=1}^{N}$ ，然后计算粒子的权值并归一化得 $\{\tilde{W}_{k}^{(i)}\}_{i=1}^{N}$
重采样：对粒子集 $\{\tilde{x}_{k}^{(i)}\}_{i=1}^{N}$ 重采样得到新的粒子集 $\{x_{k}^{(i)}\}_{i=1}^{N}$ ，相应的权重为 $W_{k}^{(i)}=1/N$
估计输出：根据粒子集 $\{x_{k}^{(i)}\}_{i=1}^{N}$ 和相应的权重 $W_{k}^{(i)}=1/N$ 计算 $k$ 时刻的状态估计值，计算的公式为

${{\hat{x}}_{k}}=\sum\limits_{i=1}^{N}{x_{k}^{(i)}W_{k}^{(i)}}$

3.2 SIR粒子滤波算法

SIR粒子滤波算法的流程如下：

（1）输入 $k-1$ 时刻的所有粒子的值及其相应的权重，以及 $k$ 时刻的观测值 $y_{k}$

（2）根据以下公式计算 $k$ 时刻所有粒子 $i=1,2,...,N$ 的状态值以及相应的权重

$x_{k}^{(i)}\sim p\left( x_{k}|x_{k-1}^{(i)}\right)$

$W_{k}^{(i)}= p\left( y_{k}|x_{k}^{(i)}\right)$

（3）计算粒子权重的和并归一化

（4）重采样

（5）根据以下公式估计状态

${{\hat{x}}_{k}}=\sum\limits_{i=1}^{N}{f(x_{k}^{(i)})W_{k}^{(i)}}$

其中粒子的权重可以用以下的高斯分布计算

$W_{k}^{(i)}=\eta {{(2\pi \Sigma )}^{-1/2}}\exp \left\{ -\frac{1}{2}{{({{y}_{true}}-y)}^{T}}{{\Sigma }^{-1}}({{y}_{true}}-y) \right\}$

4 实际应用

现在我们假设在海上有一艘正在做匀速直线运动的船只，其相对于传感器的横纵坐标为 $(x;y;v_{x};v_{y})$ 为隐藏状态，无法直接获得，而传感器可以测量得到船只相对于传感器的距离和角度 $(r;\theta)$ ，传感器采样的时间间隔为 $\Delta t$ ，则：

状态向量 $(x;y;v_{x};v_{y})$ ，观测向量 $(r;\theta)$

状态转移方程和观测方程为：

$\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right])+{{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{x}_{k-1}} \\ {{y}_{k-1}} \\ {{v}_{{{x}_{k-1}}}} \\ {{v}_{{{y}_{k-1}}}} \\ \end{matrix} \right]+{{\mathbf{s}}_{k}}$

$\left[ \begin{matrix} {{r}_{k}} \\ {{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {{x}_{k}} \\ {{y}_{k}} \\ {{v}_{xk}} \\ {{v}_{yk}} \\ \end{matrix} \right])+{{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{{{y}_{k}}}{{{x}_{k}}} \\ \end{matrix} \right]+{{\mathbf{v}}_{k}}$

这里给定距离传感器的噪声均值为 $0$ ，方差为 $10$ ；角度传感器的噪声均值为 $0$ ，方差为 $0.001$ （单位弧度）；

粒子数量为 $300$ ，粒子散布范围为 $[2000; 2000; 10; 10]$ ，采样时间点为 $100$ 个；

船只的初始状态为 $(1000;1500;5;-3)$ ，四个状态量的噪声的方差分别为 $(2;2;0.2;0.2)$ 。仿真结果如下：

从仿真结果可以看出，刚开始的一段数据的跟踪效果不好，这是因为初始的粒子是随机地在整个大范围中散布，这样就会有很多粒子给出不利于跟踪准确度的结果，但是随着时间的推移，粒子不断的调整和重采样，粒子滤波的跟踪结果最后开始向目标的真实位置收敛。

相比于EKF滤波，粒子滤波对速度量的跟踪更加准确。在这种情形下，总体来说滤波效果还是不错的，但是在实际应用中，对于船只运动的状态转移噪声的均值 $\mathbf q$ 和协方差矩阵 $\mathbf Q$ ，以及传感器的观测噪声的均值 $\mathbf r$ 和协方差矩阵 $\mathbf R$ ，往往都是未知的，有很多情况都只有观测值而已，这样的情形下，就有必要利用观测值对噪声的统计量参数做出适当的估计（学习）。

5 参数估计（学习）

利用EM算法和极大后验概率估计（MAP），对未知的噪声参数做出估计，再利用估计出的参数进行PF滤波。本文假设噪声的均值都为 $0$ ，对EM算法在粒子滤波中的推导暂时先不给出，之后可能会补充，这里就先给出一种Adaptive-PF算法对协方差矩阵 $\mathbf Q$ 和 $\mathbf R$ 的估计公式。

${{\mathbf{\hat{Q}}}_{k}}=\frac{1}{k\cdot N}\sum\limits_{t=1}^{k}{\sum\limits_{j=1}^{N}{\left[ \mathbf{x}_{t}^{j}-f(\mathbf{x}_{t-1}^{j}) \right]{{\left[ \mathbf{x}_{t}^{j}-f(\mathbf{x}_{t-1}^{j}) \right]}^{T}}}}$

利用序贯的方式分别计算这八项乘积的累加和，再用于计算 ${{\mathbf{\hat{Q}}}_{k}}$ 和 ${{\mathbf{\hat{R}}}_{k}}$

利用以上的Adaptive-PF算法对船只的运动做滤波跟踪，得到的效果如下图所示：

相比于没有做参数估计的PF滤波，可以看出，Adaptive-PF在估计误差上要优于PF滤波，最后对真实值的收敛效果更好。而且，它并不需要指定状态转移噪声和观测噪声的参数，将更有利于在实际中的应用。

6 总结

粒子滤波通过采用参考分布，随机产生大量粒子，近似状态的后验概率密度，从而得到系统的估计。粒子滤波并不需要计算Jacobi矩阵，也不用对系统做局部线性化，即可对状态进行估计，对复杂系统有较好的适用性。但是当对准确度要求很高的时候，粒子滤波所需要的粒子数量就很大，这会带来巨大的计算量，而且对于高维的状态，其计算量与维度呈现指数增长，为EKF的若干阶，而如果减少粒子数，又会使得滤波效果下降。于是就有了产生更优越更精确的粒子的无迹卡尔曼滤波UKF，将在下一篇博文中为读者解读。

7 参考文献

[1] Johansen, A.M., Doucet, A. & Davy, M. Stat Comput (2008) . Particle methods for maximum likelihood estimation in latent variable models.

[2] 卡尔曼滤波系列——（一）标准卡尔曼滤波.

[3] 卡尔曼滤波系列——（二）扩展卡尔曼滤波.

[4] https://max.book118.com/html/2017/0502/103920556.shtm.

[5] https://blog.csdn.net/piaoxuezhong/article/details/78619150.

原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~

如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。

需要代码和有需要相关技术支持的可咨询QQ：297461921

卡尔曼滤波系列——（三）粒子滤波相关推荐

对无迹卡尔曼滤波(UKF)和粒子滤波(PF)的理解
对无迹卡尔曼滤波(UKF)的总结: EKF通过在工作点附近进行一阶泰勒展开来线性化非线性的运动方程和观测方程.这样的线性近似在高度非线性的情况下存在较大误差.而UKF换了一个线性化的思路.1)选取2n ...
卫星轨道的估计问题(Matlab)(三):标准重采样粒子滤波(SIR)对新问题的尝试
SIR滤波器关于粒子滤波的基本知识可以参加下面的博客: 粒子滤波,讲的很通俗易懂基本粒子滤波算法过程: SIR算法伪代码: Matlab代码实现求解如下所示的滤波问题: %PF的应用 clc,c ...
卡尔曼滤波系列——（四）无损卡尔曼滤波
1 简介无损卡尔曼滤波又称无迹卡尔曼滤波(Unscented Kalman Filter,UKF),是无损变换(Unscented Transform,UT)与标准卡尔曼滤波体系的结合,通过UT变换 ...
机器人学习--网友资料系列激光SLAM建图、粒子滤波定位和位姿图优化
一.移动机器人自主导航的前提是在未知环境中先构建地图 (目前市内很多用的2D激光雷达,构建栅格地图,相当于立体空间中的某个水平面高度的切面) 一般用的是2D 激光SLAM算法构建概率栅格占用地图: ...
卡尔曼滤波和粒子滤波
整篇转自:https://blog.csdn.net/zkl99999/article/details/46619771/ 转自http://blog.csdn.net/karen99/article ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（2）
基于B站老王的贝叶斯滤波.卡尔曼滤波.粒子滤波 Bilibili 文章目录第三部分:随机过程的贝叶斯滤波BF 第四部分:卡尔曼滤波KF 第三部分:随机过程的贝叶斯滤波BF 随机过程包含一系列随机变量 ...
从卡尔曼滤波到粒子滤波很详细，很明了。。
转自http://blog.csdn.net/karen99/article/details/7771743 卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了, ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
Particle Filter Tutorial 粒子滤波：从推导到应用（三）
五.重采样在应用SIS 滤波的过程中,存在一个退化的问题.就是经过几次迭代以后,很多粒子的权重都变得很小,可以忽略了,只有少数粒子的权重比较大.并且粒子权值的方差随着时间增大,状态空间中的有效粒子数 ...

卡尔曼滤波系列——（三）粒子滤波

1 简介

2 基本算法

2.1 蒙特卡洛采样方法

2.2 重要性采样

2.3 序贯重要性采样

2.4 重采样

3 粒子滤波算法

3.1 标准的粒子滤波算法

3.2 SIR粒子滤波算法

4 实际应用

5 参数估计（学习）

6 总结

7 参考文献

卡尔曼滤波系列——（三）粒子滤波相关推荐

最新文章

热门文章