Particle Filter Tutorial 粒子滤波：从推导到应用

转载自作者白巧克力亦唯心的文章：粒子滤波

由最基础的贝叶斯估计开始介绍，再引出蒙特卡罗采样，重要性采样，SIS粒子滤波，重采样，基本粒子滤波Generic Particle Filter，SIR粒子滤波

1.贝叶斯滤波

假设有一个系统，我们知道它的状态方程
$$x_k=f_k(x_{k-1},v_{k-1})\cdots \cdots \cdots (1)$$
如
$$x_k=\frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^2}+28\cos (1.2x_{k-1})+v_k$$
测量方程
$$y_k=h_k(x_k,n_k)\cdots \cdots \cdots (2)$$
如：
$$y_k=\frac{x_k^2}{20}+n_k$$

• 其中$x$为系统状态，$f$是状态转移函数，$v$是过程噪声。
• 其中$y$是测量到的数据，$h$是测量函数，$n$是测量噪声.
• 噪声都是独立同分布的。

从贝叶斯理论的观点来看，状态估计问题（目标跟踪、信号滤波）就是根据之前一系列的已有数据$y_{1:k}$（后验知识）递推的计算出当前状态$x_k$的可信度。这个可信度就是概率公式$p(x_k|y_{1:k})$，它需要通过预测和更新两个步奏来递推的计算。(递推即通过$p(x_{k-1}|y_{1:k-1})$推导$p(x_k|y_{1:k})$)

• 预测过程是利用系统模型$x_k=f_k(x_{k-1},v_{k-1})$预测状态的先验概率密度，也就是通过已有的先验知识$x_{k-1}$对未来的状态$x_k$进行猜测，即$p(x_k|x_{k-1})$。
• 更新过程则利用最新的测量值$y_k$对先验概率密度进行修正，得到后验概率密度，也就是对之前的猜测进行修正。

在处理这些问题时，一般都先假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态$x_k$只与上一个时刻的状态$x_{k-1}$有关。这是很自然的一种假设，就像小时候玩飞行棋，下一时刻的飞机跳到的位置只由当前时刻的位置和骰子决定。同时，假设k时刻测量到的数据$y_k$只与当前的状态$x_k$有关，如上面的状态方程2$y_k=h_k(x_k,n_k)$。

为了进行递推，不妨假设已知$k-1$时刻的概率密度函数$p(x_{k-1}|y_{1:k-1})$

预测
由上一时刻的概率密度$p(x_{k-1}|y_{1:k-1})$得到$p(x_k|y_{1:k-1})$，这个公式的含义是既然有了前面$1：k-1$时刻的测量数据，那就可以预测一下状态$x_k$出现的概率。

计算推导如下：

$$\begin{array}{rl}p(x_k|y_{1:k-1})& =\int p(x_k,x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$

等式的第一行到第二行纯粹是贝叶斯公式的应用。第二行得到第三行是由于一阶马尔科夫过程的假设，状态$x_k$只由$x_{k-1}$决定。

楼主看到这里的时候想到两个问题：

• 第一：既然$x_k$都只由$x_{k-1}$决定了，即$p(x_k|x_{k-1})$，在这里弄一个$p(x_k|y_{1:k-1})$是什么意思？这两个概率公式含义是不一样的
  • 前一个是纯粹根据模型进行预测，$x_k$实实在在的由$x_{k-1}$决定
  • 后一个是既然测到的数据和状态是有关系的，现在已经有了很多测量数据y了，那么我可以根据已有的经验对你进行预测，只是猜测$x_k$，而不能决定$x_k$。
• 第二：结论$p(x_k|y_{1:k-1})=\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}$
  • $p(x_{k-1}|y_{1:k-1})$是假设已知
  • $p(x_k|x_{k-1})$由系统状态方程$x_k=f_k(x_{k-1},v_{k-1})$决定，它的概率分布形状和系统的过程噪声$v_{k-1}$形状一模一样。由于$x_k=Constant(x_{k-1})+v_{k-1}$也就是$x_k$由一个通过$x_{k-1}$计算出的常数叠加一个噪声得到。
    
    如果没有噪声，$x_k$完全由$x_{k-1}$计算得到，也就没有概率分布这个概念了，由于出现了噪声，所以$x_k$不好确定，他的分布就如同图中的阴影部分，实际上形状和噪声是一样的，只是进行了一些平移。理解了这一点，对粒子滤波程序中，状态$x_k$的采样的计算很有帮助，要采样$x_k$，直接采样一个过程噪声$v_k$，再叠加上$f(x_k)$这个常数就行了。

更新
由$p(x_k|y_{1:k-1})$得到后验概率$p(x_k|y_{1:k})$。这个后验概率才是真正有用的东西，上一步还只是预测，这里又多了k时刻的测量$y_k$，对上面的预测再进行修正，就是滤波了。这里的后验概率$p(x_k|y_{1:k})$也将是代入到下次的预测，以得到$p(x_{k+1}|y_{1:k+1})$，形成递推。

推导：
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(x_k,y_{1:k})}{p(y_{1:k})}=\frac{p(x_k,y_k|y_{1:k-1})p(y_{1:k-1})}{p(y_k|y_{1:k-1})p(y_{1:k-1})}\\ & =\frac{p(x_k,y_k|y_{1:k-1})}{\int p(y_k,x_k|y_{1:k-1})d{x}_k}\\ & =\frac{p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{\int p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})d{x}_k}\\ & =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\end{array}$$

其中归一化常数：

$\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k$

等式最后是因为测量方程$y_k=h_k(x_k,n_k)$中$y_k$只与$x_k$有关。同上,$h(x_k)$部分是常数，只和量测噪声$n_k$的概率分布有关，注意这个也将为SIR粒子滤波里权重的采样提供编程依据。

贝叶斯滤波到这里就告一段落了。但是，请注意上面的推导过程中需要用到积分，这对于一般的非线性，非高斯系统，很难得到后验概率的解析解。为了解决这个问题，就得引进蒙特卡洛采样。

---

结论
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\\ p(x_k|y_{1:k-1})& =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$
在定位中$x_k$大概是位置，$y_k$大概是激光数据。未来粒子采样可能采$x_k$。

---

其实上面推导，就是求解$p(x_k|y_{1:k})$的过程
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(x_k,y_{1:k})}{p(y_{1:k})}=\frac{p(x_k,y_k|y_{1:k-1})p(y_{1:k-1})}{p(y_k|y_{1:k-1})p(y_{1:k-1})}\\ & =\frac{p(x_k,y_k|y_{1:k-1})}{\int p(y_k,x_k|y_{1:k-1})d{x}_k}\\ & =\frac{p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{\int p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})d{x}_k}\\ & =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\end{array}$$
$$\begin{array}{rl}p(x_k|y_{1:k-1})& =\int p(x_k,x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\\ & =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$

2.蒙特卡洛采样

假设我们能从一个目标概率分布$p(x)$中采样到一系列的样本（粒子）$x_1,x_2,\dots ,x_N$，（至于怎么生成服从$p(x)$分布的样本，这个问题先放一放），那么就能利用这些样本去估计这个分布的某些函数的期望值。譬如：
$$\begin{array}{rl}E(f(x))& =\int f(x)p(x)dx\\ var(f(x))& =E[f(x)-E(f(x)){]}^2=\int [f(x)-E(f(x)){]}^{2}p(x)dx\end{array}$$

上面的式子其实都是计算期望的问题，只是被积分的函数不同。

蒙特卡洛采样的思想就是用平均值来代替积分，求期望：

$$E(f(x))\approx \frac{f(x_1)+f(x_2)+\cdots +f(x_N)}{N}$$

这可以从大数定理的角度去理解它。我们用这种思想去指定不同的$f(x)$以便达到估计不同东西的目的。比如：要估计一批同龄人的体重，不分男女，在大样本中男的有100个，女的有20个，为了少做事，我们按比例抽取10个男的，2个女的，测算这12个人的体重求平均就完事了。注意这里的按比例抽取，就可以看成从概率分布$p(x)$中进行抽样。

下面再看一个稍微学术一点的例子

假设有一粒质地均匀的骰子。规定在一次游戏中，连续四次抛掷骰子，至少出现一次6个点朝上就算赢。现在来估计赢的概率。

• 用$x_k^n$来表示在第$n$次游戏中，第$k$次投掷的结果，$k=1,\cdots ,4$。
• 对于分布均匀的骰子，每次投掷服从均匀分布,即：$x_k\sim u(1,6)$.这里的区间是取整数，1,2,3,4,5,6，代表6个面。
• 由于每次投掷都是独立同分布的，所以这里的目标分布$p(x)$也是一个均匀分布。一次游戏就是$x=\{1,\cdots ,6{\}}^4$空间中的一个随机点。
• 为了估计取胜的概率，在第n次游戏中定义一个指示函数:
  $$f(x^n)=I\{0<\sum _{k=1}^{4}I\{x_k^n=6\}\}$$
  其中，指示函数$I\{A\}$是指，若$A$的条件满足，结果为1，不满足结果为0。
  回到这个问题，这里函数$f$的意义就是单次游戏中，若4次投掷中只要有一个6朝上，$f$的结果就会是1。由此，就可以估计在这样的游戏中取胜的期望，也就是取胜的概率：

$$\theta =E(f(x))\approx \frac{1}{N}\sum _{n=1}^{N}f(x^n)$$

当抽样次数N足够大的时候，上式就逼近真实取胜概率了，看上面这种估计概率的方法，是通过蒙特卡洛方法的角度去求期望达到估计概率的目的。是不是就跟我们抛硬币的例子一样，抛的次数足够多就可以用来估计正面朝上或反面朝上的概率了。

当然可能有人会问，这样估计的误差有多大，对于这个问题，有兴趣的请去查看我最下面列出的参考文献2。（啰嗦一句：管的太多太宽，很容易让我们忽略主要问题。博主就是在看文献过程中，这个是啥那个是啥，都去查资料，到头来粒子滤波是干嘛完全不知道了，又重新看资料。个人感觉有问题还是先放一放，主要思路理顺了再关注细节。）

接下来，回到我们的主线上，在滤波中蒙特卡洛又是怎么用的呢？

滤波中蒙特卡洛

由上面我们知道，蒙特卡洛可以用来估计概率，而在上一节中，贝叶斯后验概率的计算里要用到积分。
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\\ p(x_k|y_{1:k-1})& =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$

为了解决这个积分难的问题，可以用蒙特卡洛采样来代替计算后验概率。

假设可以从后验概率$p(x_k|y_{1:k})$中采样到$N$个样本，那么后验概率的计算可表示为：
$$\hat{p}(x_k|y_{1:k})=\frac{1}{N}\sum _{i=1}^N\delta (x_k-x_k^i)\approx p(x_k|y_{1:k})$$
其中，在这个蒙特卡洛方法中，我们定义,是狄拉克函数(dirac delta function)，跟上面的指示函数意思差不多。

用上面的解释来看：$E(f(x))$中$f(x)=\delta (x_k-x)$，当$x=x_k$时返回1。即求$x=x_k$的期望，即$x=x_k$的概率

看到这里，既然用蒙特卡洛方法能够用来直接估计后验概率$p(x_k|y_{1:k})$，那到底怎么用来做图像跟踪或者滤波呢？要做图像跟踪或者滤波，其实就是想知道当前状态的期望值：
$$\begin{array}{rl}E(f(x_k))& \approx \int f(x_k)\hat{p}(x_k|y_{1:k})d{x}_k\\ & =\frac{1}{N}\sum _{i=1}^N\int f(x_k)\delta (x_k-x_k^i)d{x}_k\\ & =\frac{1}{N}\sum _{i=1}^{N}f(x_k^i)\cdots \cdots \cdots (1)\end{array}$$

也就是用这些采样的粒子的状态值$f(x_k^i)$直接平均就得到了期望值，也就是滤波后的值，这里的$f(x_k)$ 就是每个粒子的状态函数。

这就是粒子滤波了，只要从后验概率$p(x_k|y_{1:k})$中采样很多粒子，用它们的状态求平均就得到了滤波结果。

思路看似简单，但是要命的是，后验概率不知道啊，怎么从后验概率分布中采样！所以这样直接去应用是行不通的，这时候得引入重要性采样这个方法来解决这个问题。

3.重要性采样

无法从目标分布中采样，就从一个已知的可以采样的分布里去采样如$q(x|y)$，这样上面的求期望问题就变成了：
$$\begin{array}{rl}E[f(x_k)]& =\int f(x_k)\frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}q(x_k|y_{1:k})d{x}_k\\ & =\int f(x_k)\frac{p(x_k,y_{1:k})}{p(y_{1:k})q(x_k|y_{1:k})}q(x_k|y_{1:k})d{x}_k\\ & =\int f(x_k)\frac{W_k(x_k)}{p(y_{1:k})}q(x_k|y_{1:k})d{x}_k\cdots \cdots \cdots (2)\end{array}$$

其中

$$W_k(x_k)=\frac{p(x_k,y_{1:k})}{q(x_k|y_{1:k})}=\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})}=\frac{p(x_k|y_{1:k})p(y_{1:k})}{q(x_k|y_{1:k})}\propto \frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}$$

由于：

$$p(y_{i:k})=\int p(y_{i:k}|x_k)p(x_k)d{x}_k$$
所以(2)式可以进一步写成：
$$\begin{array}{rl}E[f(x_k)]& =\int f(x_k)\frac{W_k(x_k)}{p(y_{1:k})}q(x_k|y_{1:k})d{x}_k\\ & =\frac{1}{p(y_{1:k})}\int f(x_k)W_k(x_k)q(x_k|y_{1:k})d{x}_k\\ & =\frac{\int f(x_k)W_k(x_k)q(x_k|y_{1:k})d{x}_k}{\int p(y_{i:k}|x_k)p(x_k)d{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)q(x_k|y_{1:k})d{x}_k}\\ & =\frac{E_{q(x_k|y_{1:k})}[f(x_k)W_k(x_k)]}{E_{q(x_k|y_{1:k})}[W_k(x_k)]}\cdots \cdots \cdots (3)\end{array}$$

上面的期望计算都可以通过蒙特卡洛方法来解决它，也就是说，通过采样N个样本$x_k^i\sim q(x_k|y_{1:k})$,用样本的平均来求它们的期望，所以上面的（3）式可以近似为：
$$\begin{array}{rl}E[f(x_k)]& =\frac{\frac{1}{N}{\sum }_{i=1}^N{W}_k(x_k^i)f(x_k^i)}{\frac{1}{N}{\sum }_{i=1}^N{W}_k(x_k^i)}\\ & =\frac{1}{N}{\tilde{W}}_k(x_k^i)f(x_k^i)\cdots \cdots \cdots (4)\end{array}$$

其中：

$${\tilde{W}}_k(x_k^i)=\frac{W_k(x_k^i)}{{\sum }_{i=1}^N{W}_k(x_k^i)}$$

这就是归一化以后的权重，而${\tilde{W}}_k(x_k^i)$那个权重是没有归一化的。

注意重要性采样(4)式
$$E[f(x_k)]=\frac{1}{N}{\tilde{W}}_k(x_k^i)f(x_k^i)$$
，它不再是蒙特卡洛采样（1）式
$$E[f(x_k)]=\frac{1}{N}\sum _{i=1}^{N}f(x_k^i)$$
中所有的粒子状态直接相加求平均了，而是一种加权和的形式。不同的粒子都有它们相应的权重，如果粒子权重大，说明信任该粒子比较多。

到这里已经解决了不能从后验概率$p(x_k|y_{1:k})$直接采样的问题，但是上面这种每个粒子的权重
$$W_k(x_k)=\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})}\propto \frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}$$
都直接计算的方法，效率低，因为每增加一个采样我的理解，不是增加一个$x_k^{i+1}$的采样，而是$x_{k+1}^i$，$p(x_k|y_{1:k})$都得重新计算，并且还不好计算。所以求权重时能否避开计算$p(x_k|y_{1:k})$。而最佳的形式是能够以递推的方式去计算权重，这就是所谓的序贯重要性采样（SIS），粒子滤波的原型。

4.SIS Filter(Sequential Importance Sampling)

下面开始权重$W$递推形式的推导：

假设重要性概率密度函数$q(x_{0:k}|y_{1:k})$，这里x的下标是$0:k$，也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为：
$$\begin{array}{rl}q(x_{0:k}|y_{1:k})& =q(x_k|x_{0:k-1},y_{1:k})q(x_{0:k-1}|y_{1:k})\\ & =q(x_k|x_{0:k-1},y_{1:k})q(x_{0:k-1}|y_{1:k-1})\end{array}$$

后验概率密度函数的递归形式可以表示为：
$$\begin{array}{rl}p(x_{0:k}|y_{1:k})& =\frac{p(x_{0:k},y_{1:k})}{p(y_{1:k})}\\ & =\frac{p(y_k|x_{0:k},y_{1:k-1})p(x_k|x_{0:k-1},y_{1:k-1})p(x_{0:k-1}|y_{1:k-1})p(y_{1:k-1})}{p(y_k|y_{1:k-1})p(y_{1:k-1})}\\ & =\frac{p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|y_{1:k-1})}{p(y_k|y_{1:k-1})}\\ & \propto p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|y_{1:k-1})\end{array}$$
同时，上面这个式子和上一节贝叶斯滤波对比，$x_k$变成了这里的$x_{0:k}$，就是这个不同，导致贝叶斯估计里需要积分，而这里后验概率的分解形式却不用积分。
$$\begin{array}{rl}p(x_k|y_{1:k})& =\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})d{x}_k}\\ p(x_k|y_{1:k-1})& =\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})d{x}_{k-1}\end{array}$$

粒子权值的递归形式可以表示为:

$$\begin{array}{rl}{W}_k(x_k^i)& =\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})}\propto \frac{p(x_k^i|y_{1:k})}{q(x_k^i|y_{1:k})}\\ W_k^i& \propto \frac{p(x_{0:k}^i|y_{1:k})}{q(x_{0:k}^i|y_{1:k})}\\ & =\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)p(x_{0:k-1}^i|y_{1:k-1})}{q(x_k^i|x_{0:k-1}^i,y_{1:k})q(x_{0:k-1}^i|y_{1:k-1})}\\ & =W_{k-1}^i\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)}{q(x_k^i|x_{0:k-1}^i,y_{1:k})}\cdots \cdots \cdots (5)\end{array}$$
注意，这种权重递推形式的推导没有归一化。而在进行状态估计的公式${\sum }_{i=1}^N{\tilde{W}}_k(x_k^i)f(x_k^i)$的权重是归一化以后的. 所以在实际应用中，递推计算出w(k)后,要进行归一化，才能够代入(4)式中去计算期望。同时，上面(5)式中的分子是不是很熟悉，在上一节贝叶斯滤波中我们都已经做了介绍，$p(y|x),p(x_k|x_{k-1})$的形状实际上和状态方程中噪声的概率分布形状是一样的，只是均值不同了。因此这个递推的(5)式和前面的非递推形式相比，公式里的概率都是已知的，权重的计算可以说没有编程方面的难度了。权重也有了以后，只要进行稍微的总结就可以得到SIS Filter。

总结

在实际应用中我们可以假设重要性分布q()满足：

$$q(x_k|x_{0:k-1},y_{1:k})=q(x_k|x_{k-1},y_k)$$

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

$$W_k^i\propto W_{k-1}^i\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)}{q(x_k^i|x_{k-1}^i,y_k)}$$

在做了这么多假设和为了解决一个个问题以后，终于有了一个像样的粒子滤波算法了，他就是序贯重要性采样滤波。

下面用伪代码的形式给出这个算法：

----------------------pseudo code-----------------------------------

$$[\{x_k^i,W_k^i{\}}_{i=1}^N]=SIS(\{x_{k-1}^i,W_{k-1}^i{\}}_{i=1}^N,y_{1:k})$$

For i=1:N

• (1)采样:$x_k^i\sim q(x_k^i|x_{k-1}^i,y_k)$
• (2)递推计算各个粒子的权重；
  $$W_k^i\propto W_{k-1}^i\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)}{q(x_k^i|x_{k-1}^i,y_k)}$$

End For

粒子权值归一化。粒子有了，粒子的权重有了，就可以由(4)式,对每个粒子的状态进行加权去估计目标的状态了。

-----------------------end -----------------------------------------------

这个算法就是粒子滤波的前身了。只是在实际应用中，又发现了很多问题，如粒子权重退化的问题，因此就有了重采样( resample )，就有了基本的粒子滤波算法。还有就是重要性概率密度q()的选择问题，等等。都留到下一章 去解决。

在这一章中，我们是用的重要性采样这种方法去解决的后验概率无法采样的问题。实际上，关于如何从后验概率采样，也就是如何生成特定概率密度的样本，有很多经典的方法（如拒绝采样，Markov Chain Monte Carlo，Metropolis-Hastings 算法，Gibbs采样），这里面可以单独作为一个课题去学习了，有兴趣的可以去看看《统计之都 的一篇博文》，强烈推荐，参考文献里的前几个也都不错。

5.重采样

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

通常采用有效粒子数来衡量粒子权值的退化程度，即
$$\begin{array}{rl}{N}_{eff}& =\frac{N}{1+var(w_k^{\ast (i)})}\\ w_k^{\ast (i)}& =\frac{p(x_k^{(i)}|y_{1:k})}{q(x_k^{(i)}|x_{k-1}^{(i)},y_{1:k})}\end{array}$$

这个式子的含义是，有效粒子数越小，即权重的方差越大，也就是说权重大的和权重小的之间差距大，表明权值退化越严重。在实际计算中，有效粒子数可以近似为：
$${\hat{N}}_{eff}\approx \frac{1}{{\sum }_{i=1}^N(w_k^i{)}^2}$$
在进行序贯重要性采样时，若上式小于事先设定的某一阈值，则应当采取一些措施加以控制。克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。因此，一般采用以下两种途径：

• (1)选择合适的重要性概率密度函数：选取重要性概率密度函数的一个标准就是使得粒子权值的方差最小。关于这部分内容，还是推荐百度文库的那篇文章《粒子滤波理论》，他在这里也引申出来了几种不同的粒子滤波方法。
• (2)在序贯重要性采样之后，采用重采样方法。重采样的思路是：既然那些权重小的不起作用了，那就不要了。要保持粒子数目不变，得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制几个？那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，也就是老大分身分得最多，老二分得次多，以此类推。下面以数学的形式来进行说明

前面已经说明了求某种期望问题变成了这种加权和的形式:(带权重的贝叶斯滤波)
$$p(x_k|y_{1:k})=\sum _{i=1}^N{w}_k^i\delta (x_k-x_k^i)$$
重采样以后，希望变成
$$\tilde{p}(x_k|y_{1:k})=\sum _{i=1}^M\frac{1}{M}\delta (x_k-x_k^i)=\sum _{j=1}^N\frac{n_j}{N}\delta (x_k-x_k^j)$$
即粒子总数仍然是M，但有很多重复粒子。

思路有了，就看具体的操作方法了。在《On resampling algorithms for particle fi lters》 这篇paper里讲了四种重采样的方法。这四种方法大同小异。如果你接触过遗传算法的话，理解起来就很容易，就是遗传算法中那种轮盘赌的思想。

例子

假设有3个粒子，在第k时刻的时候，他们的权重分别是0.1, 0.1 ,0.8,然后计算他们的概率累计和(matlab 中为cumsum() )得到：[0.1, 0.2, 1]。接着，我们用服从[0,1]之间的均匀分布随机采样3个值，假设为0.15 , 0.38 和 0.54。也就是说，第二个粒子复制一次，第三个粒子复制两次。

在MATLAB中一句命令就可以方便的实现这个过程：
$$[,j]=histc(rand(N,1),[0cumsum(w^{\prime })])$$
关于histc的用法可以点击histc用法。

对于上面的过程，还可以对着下面的图加深理解：

基本的粒子滤波
将重采样的方法放入之前的SIS算法中，便形成了基本粒子滤波算法。

• (1)粒子集初始化,k=0:(这里k可以理解为时序)
  对于$i=1,2,\cdots ,N$，由先验$p(x_0)$生成采样粒子$\{x_0^i{\}}_{i=1}^N$这里N是指粒子个数
• (2)对于$k=1,2,\cdots$，循环执行以下步骤：
  • (1)重要性采样：对于$i=1,2,\cdots ,N$，从重要性概率密度$q(x_k|x_{k-1},y_k)$中生成采样粒子$\{{\tilde{x}}_k^i{\}}_{i=1}^N$，计算粒子权值，并进行归一化；
    $${\tilde{w}}_k^i=w_{k-1}^i\frac{p(y_k|{\tilde{x}}_k^i)p({\tilde{x}}_k^i|x_{k-1}^i)}{q({\tilde{x}}_k^i|x_{k-1}^i,y_k)}$$
  • (2)重采样：对粒子集$\{{\tilde{x}}_k^i,{\tilde{w}}_k^i\}$进行重采样，重采样后的粒子集为$\{x_k^i,1/N\}$
  • (3)输出：计算k时刻的状态估计值${\hat{x}}_k={\sum }_{i=1}^N{\tilde{x}}_k^i{\tilde{w}}_k^i$

重采样的思路很简单，但是当你仔细分析权重的计算公式时:
$$W_k(x_k)=\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})}\propto \frac{p(x_k^i|y_{1:k})}{q(x_k^i|y_{1:k})}$$
会有疑问，权重大的就多复制几次，这一定是正确的吗？权重大，如果是分子大，即后验概率大，那说明确实应该在后验概率大的地方多放几个粒子。但权重大也有可能是分母小造成的，这时候的分子也可能小，也就是实际的后验概率也可能小，这时候的大权重可能就没那么优秀了。何况，这种简单的重采样会使得粒子的多样性丢失，到最后可能都变成了只剩一种粒子的分身。在遗传算法中好歹还引入了变异来解决多样性的问题。当然，粒子滤波里也有专门的方法：正则粒子滤波，有兴趣的可以查阅相关资料。

至此，整个粒子滤波的流程已经清晰明朗了，在实际应用中还有一些不确定的就是重要性概率密度的选择。在下一章中，首先引出 SIR 粒子滤波，接着用SIR滤波来进行实践应用。

6.SIR粒子滤波Sampling Importance Resampling Filter

先前的结论：

---

$$[\{x_k^i,W_k^i{\}}_{i=1}^N]=SIS(\{x_{k-1}^i,W_{k-1}^i{\}}_{i=1}^N,y_{1:k})$$

For i=1:N

• (1)采样:$x_k^i\sim q(x_k^i|x_{k-1}^i,y_k)$
• (2)递推计算各个粒子的权重；
  $$W_k^i\propto W_{k-1}^i\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)}{q(x_k^i|x_{k-1}^i,y_k)}$$

End For

---

SIR滤波器很容易由前面的基本粒子滤波推导出来，只要对粒子的重要性概率密度函数做出特定的选择即可。在SIR中，选取如下。由于$p(x_k^i|x_{k-1}^i)$是先验概率，可以用状态方程$x_k=f_k(x_{k-1},v_{k-1})$得到。
$$q(x_k^i|x_{k-1}^i,y_k)=p(x_k^i|x_{k-1}^i)$$
带入权重公式中：
$$W_k^i\propto W_{k-1}^i\frac{p(y_k|x_k^i)p(x_k^i|x_{k-1}^i)}{q(x_k^i|x_{k-1}^i,y_k)}=>W_k^i\propto W_{k-1}^{i}p(y_k|x_k^i)$$
但每次重采样之后，$w_{k-1}^i=\frac{1}{N}$；这样上式可以简化为：
$$W_k^i\propto W_{k-1}^{i}p(y_k|x_k^i)=>W_k^i\propto p(y_k|x_k^i)$$

对于$p(y_k|x_k^i)$

在机器人定位里面就是，在机器人处于位姿x时，此时传感器数据y出现的概率。

更简单的例子是，我要找到一个年龄是14岁的男孩（状态x），身高为170（测量y）的概率。要知道y出现的概率，需要知道此时y的分布。

这里以第一篇文章的状态方程为例，由系统测量方程$y_k=h_k(x_k,n_k)$可知，测量是在真实值附近添加了一个高斯噪声。因此，y的分布就是以真实测量值为均值，以噪声方差为方差的一个高斯分布。

因此，权重的采样过程就是：当粒子处于x状态时，能够得到该粒子的测量y。要知道这个测量y出现的概率，就只要把它放到以真实值为均值，噪声方差为方差的高斯分布里去计算就行了。对于定位来说，真实值就是真实的雷达数据$y_{true}$，粒子的测量值$y$可能就是当粒子处于位置$x_k$时，同一方向的雷达波束发射出去，在地图上遇到的障碍物的位置
$$w=\eta \frac{1}{\sqrt{2\pi \Sigma }}exp\{\frac{(y_{true}-y{)}^2}{2\Sigma }\}$$
到这里，就可以看成SIR只和系统状态方程有关了，不用自己另外去设计概率密度函数，所以在很多程序中都是用的这种方法。

下面以伪代码的形式给出SIR滤波器：

----------------------SIR Particle Filter pseudo code-----------------------------------
$$[\{x_k^i,W_k^i{\}}_{i=1}^N]=SIS(\{x_{k-1}^i,W_{k-1}^i{\}}_{i=1}^N,y_k)$$

• For i=1:N
  • 采样粒子：$x_k^i\sim p(x_k|x_{k-1}^i)$
  • 计算粒子权重：$w_k^i=p(y_k|x_k^i)$
• End for
• 计算粒子权重和$t=\sum w$
• 对每个粒子，用上面的权重和进行归一化,w = w/t
• 状态估计：${\sum }_{i=1}^N{\tilde{W}}_k(x_k^i)f(x_k^i)$
• 重采样

在上面算法中，每进行一次，都必须重采样一次，这是由于权重的计算方式决定的。

分析上面算法中的采样，发现它并没有加入测量y(k)。只是凭先验知识p( x(k)|x(k-1) )进行的采样，而不是用的修正了的后验概率。所以这种算法存在效率不高和对奇异点(outliers)敏感的问题。但不管怎样，SIR确实简单易用。