粒子滤波算法学习

今天我们来讲一下粒子滤波算法，这也是我在学习过程中的一个笔记，由于有很多的个人理解，有不对的地方欢迎大家批评指正。

另：个人博客 Glooow 开业啦！欢迎各位大驾光临

贝叶斯滤波

贝叶斯滤波是我们理解粒子滤波的基础。假设我们有如下图的隐马尔科夫模型(Hidden Markov Model)，并且有如下的系统方程与观测方程，我们只有观测值 $\boldsymbol{y}_{1:T}$y1:T​，但是现在想要根据观测值估计隐变量 $\boldsymbol{x}_{1:T}$x1:T​，也就是滤波所要做的事情。
$\begin{aligned} \text{System Model}&: x_{k}=f_{k}\left(x_{k-1}, v_{k-1}\right) \\ \text{Observation Model}&: \mathrm{y}_{k}=h_{k}\left(\mathrm{x}_{k}, \mathrm{n}_{k}\right) \end{aligned}$System ModelObservation Model​:xk​=fk​(xk−1​,vk−1​):yk​=hk​(xk​,nk​)​

假设我们现在已经处于 $t_{k-1}$tk−1​，且已经获得了 $p({x}_{k-1}|\boldsymbol{y}_{1:k-1})$p(xk−1​∣y1:k−1​)，这个概率分布的含义是什么呢？我们现在已经有了前 $k-1$k−1 个时刻的观测值 $\boldsymbol{y}_{1:k-1}$y1:k−1​，并且根据这些观测值估计了 $k-1$k−1 时刻隐变量 $x_{k-1}$xk−1​ 的后验概率分布，也即 $p({x}_{k-1}|\boldsymbol{y}_{1:k-1})$p(xk−1​∣y1:k−1​)。那么接下来到 $k$k 时刻，我们又获得了一个观测 $y_k$yk​，要怎么估计新的后验概率分布 $p({x}_{k}|\boldsymbol{y}_{1:k})$p(xk​∣y1:k​) 呢？我们通过预测和更新两个阶段来估计，下面我就解释一下这两个阶段的含义。

1. 预测

前面说了我们有系统模型和观测模型，首先根据系统模型，我们有了 $k-1$k−1 时刻的后验，就可以根据 $x_{k-1}$xk−1​ 到 $x_k$xk​ 的转移模型得到 $x_k$xk​ 对应的概率分布，这个过程就叫做预测(Update)。通过预测阶段，我们可以获得先验概率分布(注意这里我们将 $p({x}_k|\boldsymbol{y}_{1:k-1})$p(xk​∣y1:k−1​) 称为先验概率分布)
$\begin{aligned}p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1}\end{aligned}$p(xk​∣y1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣y1:k−1​)dxk−1​​
也就是说，我们即使没有观测值，也能对 $x_k$xk​ 的分布进行估计，但是由于没有利用观测值 $y_k$yk​ 带来的信息，因此这个估计是不准确的，下面我们就需要用观测值对这个先验概率分布进行纠正，也就是 更新阶段。

2.更新

有了先验，又有了观测，根据贝叶斯公式，我们可以很容易得到后验概率分布 $p\left(x_{k} | \boldsymbol{y}_{1: k}\right)$p(xk​∣y1:k​)。怎么理解下面一个式子呢？我们有似然函数 $p\left(y_{k} | x_{k}\right)$p(yk​∣xk​)，现在有了观测 $y_k$yk​，那么似然值越大，表明对应的 $x_k$xk​ 的概率应该也越大， 就是用似然函数对先验概率进行加权就得到了后验概率。
$\begin{aligned} p\left(x_{k} | \boldsymbol{y}_{1: k}\right)&=\frac{p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)}{p\left(y_{k} | \boldsymbol{y}_{1: k-1}\right)} \\ &\propto p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \\ &= p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \end{aligned}$p(xk​∣y1:k​)​=p(yk​∣y1:k−1​)p(yk​∣xk​,y1:k−1​)p(xk​∣y1:k−1​)​∝p(yk​∣xk​,y1:k−1​)p(xk​∣y1:k−1​)=p(yk​∣xk​)p(xk​∣y1:k−1​)​

怎么理解这个加权呢？举个栗子

比如

总结

总结起来，我们滤波分为两个阶段
$\begin{aligned} \text{Prediction}&: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\ \text{Update}&: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \end{aligned}$PredictionUpdate​:p(xk​∣y1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣y1:k−1​)dxk−1​:p(xk​∣y1:k​)∝p(yk​∣xk​)p(xk​∣y1:k−1​)​
可以看到，上面的预测阶段含有积分，这在实际当中往往是很难计算的，而对于那些非常规的PDF，甚至不能给出解析解。解决这种问题一般有两种思路：

• 建立简单的模型，获得解析解，如卡尔曼滤波(Kalman Filter)及EKF、UKF等；
• 建立复杂的模型，获得近似解，如粒子滤波(Particle Filter)等。

卡尔曼滤波

卡尔曼滤波的思路就是将系统建模线性模型，隐变量、控制变量、控制噪声与观测噪声均为高斯分布，那么观测变量也是随机变量，整个模型中所有的随机变量都是高斯的！高斯分布是我们最喜欢的分布，因为在前面贝叶斯滤波的预测阶段我们可以不用积分了，只需要计算均值和协方差就可以了！根据系统模型和观测模型，我们可以获得滤波的闭式解，这就是卡尔曼滤波的思想。
$\begin{aligned} \text{System Model}&: \mathrm{x}_{k}=A\mathrm{x}_{k-1} + B\mathrm{u}_{k-1} + v_{k-1} \\ \text{Observation Model}&: \mathrm{y}_{k}=C\mathrm{x}_{k}+ \mathrm{n}_{k} \end{aligned}$System ModelObservation Model​:xk​=Axk−1​+Buk−1​+vk−1​:yk​=Cxk​+nk​​
但实际中要求线性系统模型往往是很困难的，对于非线性模型，如果我们还想应用卡尔曼滤波该怎么办呢？线性近似，也就是求一个雅可比矩阵(Jacobi)，这就是扩展卡尔曼滤波(EKF)。

大数定律

粒子滤波是什么是意思呢？我们可以用大量的采样值来描述一个概率分布，当我们按照一个概率分布进行采样的时候，某个点的概率密度越高，这个点被采到的概率越大，当采样数目足够大(趋于无穷)的时候，粒子出现的频率就可以用来表示对应的分布，实际中我们可以理解一个粒子就是一个采样。

但是对于离散型的随机变量还好，可以很容易的求出来状态空间中每个状态的频率。但如果对于连续分布，其实是很不方便的，所幸实际中我们也不需要获得概率分布的全部信息，一般只需要求出期望就可以了。

下面为了公式书写和推导方便，以离散型随机变量为例，假设状态空间为 $\mathcal{Z}=\{1,2,...,K\}$Z={1,2,...,K}，有 $N$N 个采样样本 $z_i \in \mathcal{Z}$zi​∈Z，服从概率分布 $q(\mathbf{z})$q(z)。我们将根据 $N$N 个采样样本 $\boldsymbol{z}_{1:N}$z1:N​ 得到的分布记为经验分布 $\hat{p}(b|\boldsymbol{z}_{1:N}) = \frac{1}{N}\sum_i \mathbb{I}(b-z_i)$p^​(b∣z1:N​)=N1​∑i​I(b−zi​)，其中 $\mathbb{I}(\cdot)$I(⋅) 为指示函数，那么当 $N$N 足够大的时候，经验分布 $\hat{p}(b|\boldsymbol{z}_{1:N})$p^​(b∣z1:N​) 就足够接近真实分布 $q(\mathbf{z})$q(z)。现在我们想估计随机变量 $\mathsf{t}=g(\mathsf{z})$t=g(z) 的期望，即
$\begin{aligned} \mathbb{E}[\mathsf{t}] &= \mathbb{E}[g(\mathsf{z})]=\sum_{b\in\mathcal{Z}}g(b)q(b) \\ &\approx \sum_b g(b)\hat{p}(b|\boldsymbol{z}_{1:N}) \\ &= \sum_b g(b)\frac{1}{N}\sum_i \mathbb{I}(b-z_i)\\ &= \frac{1}{N}\sum_i g(z_i) \end{aligned}$E[t]​=E[g(z)]=b∈Z∑​g(b)q(b)≈b∑​g(b)p^​(b∣z1:N​)=b∑​g(b)N1​i∑​I(b−zi​)=N1​i∑​g(zi​)​
也就是说，我们只需要对样本进行简单求和就可以了！连续型随机变量也是类似的，所以在后面的粒子滤波中，我们利用粒子估计了概率密度分布，要想求期望就只需要进行求和平均就可以了。

粒子滤波简单实例

好了，有了前面的预备知识，我们可以先看一个粒子滤波的简单例子。

假设我们现在有采样好的 $N$N 个粒子 $\{x_{k-1}^i\}_{i=1}^N${xk−1i​}i=1N​，他们服从分布 $p({x}_{k-1}|\boldsymbol{y}_{1:k-1})$p(xk−1​∣y1:k−1​)，那么我们现在如何进行贝叶斯滤波中的预测和更新阶段呢？

1. 预测

回顾一下预测的公式
$\text{Prediction}: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\$Prediction:p(xk​∣y1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣y1:k−1​)dxk−1​
想一下：只要我们让每个粒子 $x_{k-1}^i$xk−1i​ “进化”一步，也就是说按照分布 $p(x_k|x_{k-1})$p(xk​∣xk−1​) 进行采样，使得 $x_k^i \sim p(x_k|x_{k-1}^i)$xki​∼p(xk​∣xk−1i​)，这样我们就获得了 $N$N 个新的粒子 $\{x_k^i\}_{i=1}^N${xki​}i=1N​。很容易验证，如果 $\{x_{k-1}^i\}_{i=1}^N${xk−1i​}i=1N​ 能够很好的表示 $p({x}_{k-1}|\boldsymbol{y}_{1:k-1})$p(xk−1​∣y1:k−1​) 的话，那么 $\{x_k^i\}_{i=1}^N${xki​}i=1N​ 也能很好的表示 $p({x}_k|\boldsymbol{y}_{1:k-1})$p(xk​∣y1:k−1​)(这一部分可以凭直观感觉来理解，这是一个很自然的事情，也可以用前面的经验分布的思路进行公式推导)。

这样做的好处是什么呢？我们避免了积分！只需要对一个已知的分布 $p(x_k|x_{k-1}^i)$p(xk​∣xk−1i​) 进行采样，而这个分布是我们假设的系统模型，可以是很简单的，因此采样也很方便。

2. 更新

通过预测我们获得了 $x_k^i \sim p(x_k|x_{k-1}^i)$xki​∼p(xk​∣xk−1i​)，这 $N$N 个粒子就描述了先验概率分布。接下来就是更新，再回顾一下更新的公式
$\text{Update}: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)$Update:p(xk​∣y1:k​)∝p(yk​∣xk​)p(xk​∣y1:k−1​)
更新是什么呢？前面提到了：更新就是用似然函数对先验概率进行加权就得到了后验概率。如果简单的把 $x_k^i$xki​ 带入到上面的公式里，就可以得到下面的式子
$p\left(x_{k}^i | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}^i\right) p\left(x_{k}^i | \boldsymbol{y}_{1: k-1}\right)$p(xki​∣y1:k​)∝p(yk​∣xki​)p(xki​∣y1:k−1​)
实际上就表示每个粒子有不同的权重。

这里怎么理解呢？想一下前面提到的经验分布，我们只需要用粒子出现的频率来表示对应的概率，实际上就是在统计频率的时候每个粒子的权重都是相同的，出现一次计数就加一。

而这里的区别是什么呢？由于我们有一个观测值 $y_k$yk​，根据 $y_k$yk​ 来反推，某一个粒子 $i_1$i1​ 的导致 $y_k$yk​ 出现的概率更大，那么我们在统计频率的时候，就给这个粒子加一个更大的权重，以表示我们更相信/重视这个粒子。

那么问题来了，前面我们说了滤波过程中要想求期望，只需要简单求和就可以了
$\mathbb{E}[\mathsf{t}] = \frac{1}{N}\sum_i g(z_i)$E[t]=N1​i∑​g(zi​)
现在粒子有了权重怎么办呢，同理，加个权就好了(推导也很简单，相信大家都会)，下面的式子里对权重进行了归一化
$\mathbb{E}[\mathsf{t}] = \sum_i \frac{w_i}{\sum_j w_j}g(z_i)$E[t]=i∑​∑j​wj​wi​​g(zi​)

3. 递推

前面只讲了一次预测和一次更新的过程，注意到我们前面只是从 $k-1$k−1 到 $k$k 时刻的滤波过程，但每一轮循环原理都是相同的。

不过细心的朋友们可能会觉得不太对劲，前面一个阶段的例子里，预测之前我们有采样 $\{x_{k-1}^i\}_{i=1}^N${xk−1i​}i=1N​，推导过程中我们是默认这些粒子的权重都是相同的，然后我们进行了预测和更新，但是更新之后我们给每个粒子加权了呀，到下一个阶段怎么办？！！！也很简单，预测阶段不必要求每个粒子的权重都相同，也加上一个权重就好了。也就是说我们时时刻刻都保存有每个粒子的权重信息。

这样我们就可以得到一个完整的粒子滤波算法了！但是还有问题！

重采样

但是呢，有人发现了上面的算法不好啊！不是因为麻烦，而是滤波到最后会发现只有少数一部分粒子有很大的权重，而其他绝大部分粒子的权重几乎为 0，这就意味着对于那些“不重要”的粒子，他们在我们求期望的时候贡献并不大，但是我们却花费了大量的计算精力来维护这些粒子，这样不好不好！于是就有人提出了重采样。

重采样什么意思呢？你们粒子的权重不是不同吗，那我就采取手段给你们整相同了！简单理解，有两个粒子，粒子 $a$a 的权重是 8，粒子 $b$b 权重是 2，那我就把粒子 $a$a 复制 8 份，粒子 $b$b 复制 2 份，这样得到的 10 个粒子权重就都是 1 了。但是另一个问题是一开始我们只有 2 个粒子，这样弄完我们就有 10 个粒子了，如果一直这么做我们的粒子数会越来越多，算力跟不上。那就从这 10 个粒子里均匀分布地随机抽 2 个！那么粒子 $a$a 的概率就是 $0.8$0.8，这就成了。

至此，加上重采样我们就获得了一个完整的比较好用的粒子滤波算法

粒子滤波原理推导

但是还有一个问题，就是前面我们直接说有 $N$N 个粒子 $x_{k-1}^i \sim p({x}_{k-1}|\boldsymbol{y}_{1:k-1})$xk−1i​∼p(xk−1​∣y1:k−1​)，但是第 1 步(刚开始)的时候我们怎么获得这些粒子来描述 $p(x_1|y_1)$p(x1​∣y1​) 啊？如果是高斯分布还好，我们可以很方便的对高斯分布进行采样，但是如果是一个特别特别特别复杂的分布呢？我们甚至不能写出解析表达式，更没办法按照这个分布进行随机采样，那我们是不是就凉了？不！我们前面不是讲了粒子可以有权重嘛。

假如我们现在有另一个分布 $q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k})$q(x1:k​∣y1:k​)，这个分布是我们自己指定的，可以很简单，而且我们可以按照这个分布来进行采样，那么我们就有 $x_{k-1}^i \sim q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k})$xk−1i​∼q(x1:k​∣y1:k​)，那么我们怎么用这些粒子来表示我们想要得到的真正的分布 $p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k})$p(x0:k​∣y1:k​) 呢？再加个权就行了，就像前面用似然函数进行加权。
$\begin{aligned} p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k})&\approx \sum_{i=1}^{N}w_k^i\delta(\mathbf{x}_{0:k}-\mathbf{x}_{0:k}^i) \\ w_{k}^{i} &\propto \frac{p\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)}{q\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)} \end{aligned}$p(x0:k​∣y1:k​)wki​​≈i=1∑N​wki​δ(x0:k​−x0:ki​)∝q(x0:ki​∣y1:k​)p(x0:ki​∣y1:k​)​​
再考虑前面提到的的预测和更新两个递推进行的阶段，就有
$\begin{aligned}q\left(\mathbf{x}_{0: k} | \mathbf{y}_{1: k}\right)&=q\left(\mathbf{x}_{k} | \mathbf{x}_{0: k-1}, \mathbf{y}_{1: k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\ &= q\left(\mathbf{x}_{k} | \mathbf{x}_{k-1}, \mathbf{y}_{ k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\ w_{k}^{i} &\propto w_{k-1}^{i} \frac{p\left(\mathbf{y}_{k} | \mathbf{x}_{k}^{i}\right) p\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}\right)}{q\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}, \mathbf{y}_{k}\right)} \end{aligned}$q(x0:k​∣y1:k​)wki​​=q(xk​∣x0:k−1​,y1:k​)q(x0:k−1​∣y1:k−1​)=q(xk​∣xk−1​,yk​)q(x0:k−1​∣y1:k−1​)∝wk−1i​q(xki​∣xk−1i​,yk​)p(yk​∣xki​)p(xki​∣xk−1i​)​​
然后我们就得到了一个更加 universal/generic 的粒子滤波算法(下面图片所示算法中没加重采样步骤)。

总结

这篇文章主要是自己学习粒子滤波之后的一些理解，主要参考了文章 A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking，但是这篇文章中是从 general 的情况开始讲，然后举了一些常用的特例，个人感觉不利于初学者理解，因此本文写作过程中的思路是从简单的特例开始再到更一般的情况。

最后一部分 [粒子滤波原理推导](## 粒子滤波原理推导) 其实写的并没有很清楚，主要是因为自己太懒，到最后懒得一个个手敲公式了，如果想更清楚地了解其中的细节，可以阅读上面那篇论文。

Reference

1. M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” in IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174-188, Feb. 2002.