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

最新推荐文章于 2024-07-19 10:51:07 发布

GHelpU

最新推荐文章于 2024-07-19 10:51:07 发布

阅读量1w

点赞数 19

分类专栏：信号处理文章标签：粒子滤波 PF 信号处理参数估计（学习）自适应PF

本文链接：https://blog.csdn.net/weixin_42647783/article/details/89058079

版权

信号处理专栏收录该内容

10 篇文章 122 订阅

订阅专栏

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-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）的算法流程了：

输入时刻的所有粒子的值及其相应的权重 $\left \{x_{k-1}^{(i)},W_{k-1}^{(i)} \right \}_{i=1}^{N}$ ，以及时刻的观测值 $y_{k}$
根据以下公式计算时刻所有的粒子的状态值以及相应的权重 $\left \{x_{k}^{(i)},W_{k}^{(i)} \right \}_{i=1}^{N}$

$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 。实现的基本思路是轮盘赌思想，举个简单的粒子：在时刻有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 ，（表示粒子数，粒子数越大，效果越好，但也会使得计算量越大）由分布 $p(x_{0})$ 生成采样粒子 $\{x_{0}^{(i)}\}_{i=1}^{N}$

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

重要性采样：对于，从重要性概率密度函数生成采样粒子 $\{\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$ 计算时刻的状态估计值，计算的公式为

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

3.2 SIR粒子滤波算法

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

（1）输入 k-1 时刻的所有粒子的值及其相应的权重 $\left \{x_{k-1}^{(i)},W_{k-1}^{(i)} \right \}_{i=1}^{N}$ ，以及时刻的观测值 $y_{k}$

（2）根据以下公式计算时刻所有粒子 i=1,2,...,N 的状态值以及相应的权重 $\left \{x_{k}^{(i)},W_{k}^{(i)} \right \}_{i=1}^{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.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滤波。本文假设噪声的均值都为，对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{R}}}_{k}}=\frac{1}{k\cdot N}\sum\limits_{t=1}^{k}{\sum\limits_{j=1}^{N}{\left[ {{\mathbf{z}}_{t}}-h(\mathbf{x}_{t}^{j}) \right] \left[ {{\mathbf{z}}_{t}}-h(\mathbf{x}_{t}^{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

GHelpU

关注

19
点赞
踩
139

收藏

觉得还不错? 一键收藏
3
评论
卡尔曼滤波系列——（三）粒子滤波

1 简介粒子滤波（Partical Filter，PF）就是通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被称为“粒子”，故叫做粒子滤波。粒子滤波(PF: Particle Filter)的思想基蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间...
复制链接

扫一扫

专栏目录