Particle Filter 粒子滤波

贝叶斯滤波
重要性采样
Sequential Importance Sampling (SIS) Filter
重采样
Sampling Importance Resampling Filter (SIR)

人都喜欢自以为是。心外无事，就没是了。

本文抄自:https://blog.csdn.net/heyijia0327/article/details/40899819

贝叶斯滤波

假设有一个系统,我们知道它的状态方程和测量方程如下
状态方程:
xk=fk(xk−1,vk−1)(1)x_k=f_k(x_{k-1},v_{k-1}) \qquad (1) xk=fk(xk−1,vk−1)(1)
如:xk=xk−12+25xk11+xk−12+8cos(1.2(k−1))+vk−1x_k=\frac{x_{k-1}}{2}+\frac{25x_{k_1}}{1+x^2_{k-1}}+8cos(1.2(k-1))+v_{k-1}xk=2xk−1+1+xk−1225xk1+8cos(1.2(k−1))+vk−1
测量方程:
yk=hk(xk,nk)y_k=h_k(x_k,n_k) yk=hk(xk,nk)
如：yk=xk220+nk(2)y_{k}=\frac{x^2_{k}}{20}+n_{k} \qquad (2) yk=20xk2+nk(2)

其中xxx为系统状态，yyy为测量到的数据，fff,hhh分别是状态转移函数和测量函数，vvv,nnn为过程噪声和测量噪声,噪声都是独立同分布的。

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

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

在处理这些问题时，一般都先假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态xkx_kxk只与上一个时刻的状态xk−1x_{k-1}xk−1有关。同时，假设kkk时刻测量到的数据yky_kyk只与当前的状态xkx_kxk有关，如上面的测量方程2。

假设已知k−1k-1k−1时刻的概率密度函数p(xk−1∣y1:k−1)p(x_{k-1}|y_{1:k-1})p(xk−1∣y1:k−1)

预测：由上一时刻概率密度p(xk−1∣y1:k−1)p(x_{k-1}|y_{1:k-1})p(xk−1∣y1:k−1)得到p(xk∣y1:k−1)p(x_k|y_{1:k-1})p(xk∣y1:k−1),这个公式含有了前面1:k−11:k-11:k−1时刻的测量数据，那么可以预测下一状态xkx_kxk出现的概率。
计算推到如下：
p(xk∣y1:k−1)=∫p(xk,xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1,y1:k−1)p(xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣y1:k−1)dxk−1\begin{aligned} p\left(x_{k} \mid y_{1: k-1}\right) &=\int p\left(x_{k}, x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} \mid x_{k-1}, y_{1: k-1}\right) p\left(x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} \mid x_{k-1}\right) p\left(x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \end{aligned} p(xk∣y1:k−1)=∫p(xk,xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1,y1:k−1)p(xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣y1:k−1)dxk−1
等式的第一行到第二行是贝叶斯公式的应用。第二行得到第三行是由于一阶马尔科夫过程的假设，状态xkx_kxk只由xk−1x_{k-1}xk−1决定。
对上式的理解：
(1) 、p(xk∣xk−1,y1:k−1)=p(xk∣xk−1)p(x_{k}|x_{k-1},y_{1:k-1})=p(x_k|x_{k-1})p(xk∣xk−1,y1:k−1)=p(xk∣xk−1),表明xkx_kxk只由xk−1x_{k-1}xk−1决定
而这里p(xk∣y1:k−1)p(x_k|y_{1:k-1})p(xk∣y1:k−1)表示在已经有了很多测量数据yyy的情况下,那么可以根据已有的经验进行预测，只是猜测xkx_kxk, 而不能决定xkx_kxk。

(2)、公式的最后一行p(xk−1∣y1:k−1)p(x_{k-1}|y_{1:k-1})p(xk−1∣y1:k−1)是已知的，p(xk∣xk−1)p(x_k|x_{k-1})p(xk∣xk−1)是由系统的状态方程决定的，xkx_kxk由xk−1x_{k-1}xk−1和噪声vk−1v_{k-1}vk−1叠加得到的。

更新:由p(xk∣y1:k−1)p(x_k|y_{1:k-1})p(xk∣y1:k−1)得到后验概率p(xk∣y1:k)p(x_k|y_{1:k})p(xk∣y1:k)。这里多了kkk时刻的测量，对上面的预测再进行修正，这就是滤波。这里的后验概率也将是代入到下次的预测，形成递推。
推导:
p(xk∣y1:k)=p(yk∣xk,y1:k−1)p(xk∣y1:k−1)p(yk∣y1:k−1)=p(yk∣xk)p(xk∣y1:k−1)p(yk∣y1:k−1)\begin{aligned} p\left(x_{k} \mid y_{1: k}\right) &=\frac{p\left(y_{k} \mid x_{k}, y_{1: k-1}\right) p\left(x_{k} \mid y_{1: k-1}\right)}{p\left(y_{k} \mid y_{1: k-1}\right)} \\ &=\frac{p\left(y_{k} \mid x_{k}\right) p\left(x_{k} \mid y_{1: k-1}\right)}{p\left(y_{k} \mid 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∣y1:k−1)p(yk∣xk)p(xk∣y1:k−1)
其中归一化常数：
p(yk∣y1:k−1)=∫p(yk∣xk)p(xk∣y1:k−1)dxkp(y_k|y_{1:k-1})=\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_kp(yk∣y1:k−1)=∫p(yk∣xk)p(xk∣y1:k−1)dxk

等式第一行到第二行是因为,yky_kyk只与xkx_kxk有关，p(yk∣xk)p(y_k|x_k)p(yk∣xk)也称之为似然函数，由测量方程决定。

重要性采样

我们的目标是：
E[f(xn)]≈∫f(xn)p^(xn∣y1:k)dxn=1N∑i=1N∫f(xn)δ(xn−xn(i))dxn=1N∑i=1Nf(xn(i))(3)\begin{aligned} E\left[f\left(x_{n}\right)\right] \approx & \int f\left(x_{n}\right) \hat{p}\left(x_{n} \mid y_{1: k}\right) d x_{n} \\ &=\frac{1}{N} \sum_{i=1}^{N} \int f\left(x_{n}\right) \delta\left(x_{n}-x_{n}^{(i)}\right) d x_{n} \\ &=\frac{1}{N} \sum_{i=1}^{N} f\left(x_{n}^{(i)}\right) \qquad (3) \end{aligned} E[f(xn)]≈∫f(xn)p^(xn∣y1:k)dxn=N1i=1∑N∫f(xn)δ(xn−xn(i))dxn=N1i=1∑Nf(xn(i))(3)

当无法从目标分布p(x∣y)p(x|y)p(x∣y)中采样，就从一个已知的可以采样的分布里去采样如q(x∣y)q(x|y)q(x∣y).这样上面的求期望问题就变成了:
E[f(xk)]=∫f(xk)p(xk∣y1:k)q(xk∣y1:k)q(xk∣y1:k)dxk=∫f(xk)p(y1:k∣xk)p(xk)p(y1:k)q(xk∣y1:k)q(xk∣y1:k)dxk=∫f(xk)Wk(xk)p(y1:k)q(xk∣y1:k)dxk(4)\begin{aligned} E\left[f\left(x_{k}\right)\right]=& \int f\left(x_{k}\right) \frac{p\left(x_{k} \mid y_{1: k}\right)}{q\left(x_{k} \mid y_{1: k}\right)} q\left(x_{k} \mid y_{1: k}\right) d x_{k} \\ &=\int f\left(x_{k}\right) \frac{p\left(y_{1: k} \mid x_{k}\right) p\left(x_{k}\right)}{p\left(y_{1: k}\right) q\left(x_{k} \mid y_{1: k}\right)} q\left(x_{k} \mid y_{1: k}\right) d x_{k} \\ &=\int f\left(x_{k}\right) \frac{W_{k}\left(x_{k}\right)}{p\left(y_{1: k}\right)} q\left(x_{k} \mid y_{1: k}\right) d x_{k} \qquad (4) \end{aligned} E[f(xk)]=∫f(xk)q(xk∣y1:k)p(xk∣y1:k)q(xk∣y1:k)dxk=∫f(xk)p(y1:k)q(xk∣y1:k)p(y1:k∣xk)p(xk)q(xk∣y1:k)dxk=∫f(xk)p(y1:k)Wk(xk)q(xk∣y1:k)dxk(4)
其中
Wk(xk)=p(y1:k∣xk)p(xk)q(xk∣y1:k)∝p(xk∣y1:k)q(xk∣y1:k)W_{k}\left(x_{k}\right)=\frac{p\left(y_{1: k} \mid x_{k}\right) p\left(x_{k}\right)}{q\left(x_{k} \mid y_{1: k}\right)} \propto \frac{p\left(x_{k} \mid y_{1: k}\right)}{q\left(x_{k} \mid y_{1: k}\right)} Wk(xk)=q(xk∣y1:k)p(y1:k∣xk)p(xk)∝q(xk∣y1:k)p(xk∣y1:k)
由于：p(y1:k)=∫p(y1:k∣xk)p(xk)dxkp(y_{1:k})=\int p(y_{1:k}|x_k)p(x_k)dx_kp(y1:k)=∫p(y1:k∣xk)p(xk)dxk

所以(3)式可以进一步写成:
E[f(xk)]=1p(y1:k)∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫f(xk)Wk(xk)q(xk∣y1:k)dxk∫p(y1:k∣xk)p(xk)dxk=∫f(xk)Wk(xk)q(xk∣y1:k)dxk∫Wk(xk)q(xk∣y1:k)dxk=Eq(xk∣y1:k)[Wk(xk)f(xk)]Eq(xk∣y1:k)[Wk(xk)](5)\begin{aligned} E\left[f\left(x_{k}\right)\right] &=\frac{1}{p\left(y_{1: k}\right)} \int f\left(x_{k}\right) W_{k}\left(x_{k}\right) q\left(x_{k} \mid y_{1: k}\right) d x_{k} \\ &=\frac{\int f\left(x_{k}\right) W_{k}\left(x_{k}\right) q\left(x_{k} \mid y_{1: k}\right) d x_{k}}{\int p\left(y_{1: k} \mid x_{k}\right) p\left(x_{k}\right) d x_{k}} \\ &=\frac{\int f\left(x_{k}\right) W_{k}\left(x_{k}\right) q\left(x_{k} \mid y_{1: k}\right) d x_{k}}{\int W_{k}\left(x_{k}\right) q\left(x_{k} \mid y_{1: k}\right) d x_{k}} \\ &=\frac{E_{q\left(x_{k} \mid y_{1: k}\right)}\left[W_{k}\left(x_{k}\right) f\left(x_{k}\right)\right]}{E_{q\left(x_{k} \mid y_{1: k}\right)}\left[W_{k}\left(x_{k}\right)\right]} \qquad (5) \end{aligned} E[f(xk)]=p(y1:k)1∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫p(y1:k∣xk)p(xk)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫Wk(xk)q(xk∣y1:k)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=Eq(xk∣y1:k)[Wk(xk)]Eq(xk∣y1:k)[Wk(xk)f(xk)](5)
上面的期望计算都可以通过蒙特卡洛方法来解决它，也就是说，通过采样NNN个样本
{xk(i)}∼q(xk∣y1:k)\left\{ x^{(i)}_k \right\} \sim q(x_k|y_{1:k}){xk(i)}∼q(xk∣y1:k)
,用样本的平均来求它们的期望，所以上面的(4) 式可以近似为:
E[f(xk)]≈1N∑i=1NWk(xk(i))f(xk(i))1N∑i=1NWk(xk(i))=∑i=1NW~k(xk(i))f(xk(i))(6)\begin{aligned} E\left[f\left(x_{k}\right)\right] \approx & \frac{\frac{1}{N} \sum_{i=1}^{N} W_{k}\left(x_{k}^{(i)}\right) f\left(x_{k}^{(i)}\right)}{\frac{1}{N} \sum_{i=1}^{N} W_{k}\left(x_{k}^{(i)}\right)} \\ &=\sum_{i=1}^{N} \tilde{W}_{k}\left(x_{k}^{(i)}\right) f\left(x_{k}^{(i)}\right) \qquad (6) \end{aligned} E[f(xk)]≈N1∑i=1NWk(xk(i))N1∑i=1NWk(xk(i))f(xk(i))=i=1∑NW~k(xk(i))f(xk(i))(6)
其中:
W~k(xk(i))=Wk(xk(i))∑i=1NWk(xk(i))\tilde{W}_{k}\left(x_{k}^{(i)}\right) =\frac{W_{k}\left(x_{k}^{(i)}\right)}{\sum_{i=1}^{N} W_{k}\left(x_{k}^{(i)}\right)} W~k(xk(i))=∑i=1NWk(xk(i))Wk(xk(i))
这就是归一化以后的权重，而之前(4)式中的权重是没有归一化的。
(6)式中，它不再是(3)式中所有的粒子状态直接相加求平均了，而是一种加权和的形式。不同的粒子都有它们相应的权重，如果粒子权重大，说明信任该粒子比较多。

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

对权重www递推推导：

假设重要性概率密度函数q(x0:k∣y1:k)q(x_{0:k}|y_{1:k})q(x0:k∣y1:k)，这里xxx的下标是0:k0:k0:k，也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为:
q(x0:k∣y1:k)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)q(x_{0:k}|y_{1:k})=q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})q(x0:k∣y1:k)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)
后验概率密度函数的递归形式可以表示为:
p(x0:k∣Yk)=p(yk∣x0:k,Yk−1)p(x0:k∣Yk−1)p(yk∣Yk−1)=p(yk∣x0.k,Yk−1)p(xk∣x0.k−1,Yk−1)p(x0.k−1∣Yk−1)p(yk∣Yk−1)=p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)p(yk∣Yk−1)∝p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)\begin{aligned} p\left(x_{0: k} \mid Y_{k}\right) &=\frac{p\left(y_{k} \mid x_{0: k}, Y_{k-1}\right) p\left(x_{0: k} \mid Y_{k-1}\right)}{p\left(y_{k} \mid Y_{k-1}\right)} \\ &=\frac{p\left(y_{k} \mid x_{0 . k}, Y_{k-1}\right) p\left(x_{k} \mid x_{0 . k-1}, Y_{k-1}\right) p\left(x_{0 . k-1} \mid Y_{k-1}\right)}{p\left(y_{k} \mid Y_{k-1}\right)} \\ &=\frac{p\left(y_{k} \mid x_{k}\right) p\left(x_{k} \mid x_{k-1}\right) p\left(x_{0: k-1} \mid Y_{k-1}\right)}{p\left(y_{k} \mid Y_{k-1}\right)} \\ & \propto p\left(y_{k} \mid x_{k}\right) p\left(x_{k} \mid x_{k-1}\right) p\left(x_{0: k-1} \mid Y_{k-1}\right) \end{aligned} p(x0:k∣Yk)=p(yk∣Yk−1)p(yk∣x0:k,Yk−1)p(x0:k∣Yk−1)=p(yk∣Yk−1)p(yk∣x0.k,Yk−1)p(xk∣x0.k−1,Yk−1)p(x0.k−1∣Yk−1)=p(yk∣Yk−1)p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)∝p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)
其中，Y(k)Y(k)Y(k)表示y1:ky_{1:k}y1:k.
粒子权值的递归形式可以表示为：
wk(i)∝p(x0:k(i)∣Yk)q(x0:k(i)∣Yk)=p(yk∣xk(i))p(xk(i)∣xk−1(i))p(x0:k−1(i)∣Yk−1)q(xk(i)∣x0:k−1(i),Yk)q(x0:k−1(i)∣Yk−1)=wk−1(i)p(yk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣x0:k−1(i),Yk)(7)\begin{aligned} w_{k}^{(i)} & \propto \frac{p\left(x_{0: k}^{(i)} \mid Y_{k}\right)}{q\left(x_{0:k}^{(i)} \mid Y_{k}\right)} \\ &=\frac{p\left(y_{k} \mid x_{k}^{(i)}\right) p\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}\right) p\left(x_{0: k-1}^{(i)} \mid Y_{k-1}\right)}{q\left(x_{k}^{(i)} \mid x_{0:k-1}^{(i)}, Y_{k}\right) q\left(x_{0:k-1}^{(i)} \mid Y_{k-1}\right)} \\ &=w_{k-1}^{(i)} \frac{p\left(y_{k} \mid x_{k}^{(i)}\right) p\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}\right)}{q\left(x_{k}^{(i)} \mid x_{0:k-1}^{(i)}, Y_{k}\right)} \qquad (7) \end{aligned} wk(i)∝q(x0:k(i)∣Yk)p(x0:k(i)∣Yk)=q(xk(i)∣x0:k−1(i),Yk)q(x0:k−1(i)∣Yk−1)p(yk∣xk(i))p(xk(i)∣xk−1(i))p(x0:k−1(i)∣Yk−1)=wk−1(i)q(xk(i)∣x0:k−1(i),Yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))(7)

这种权重递推形式的推导是在前面(4)式的形式下进行推导的，也就是没有归一化。而在进行状态估计的公式为∑i=1NW~k(xk(i))f(xk(i))\sum_{i=1}^{N} \tilde{W}_{k}\left(x_{k}^{(i)}\right) f\left(x_{k}^{(i)}\right)∑i=1NW~k(xk(i))f(xk(i))这个公式中的的权重是归一化以后的，所以在实际应用中，递推计算出wkw_kwk后，要进行归一化，才能够代入(6)式中去计算期望。同时，上面(7)式中的分子是不是很熟悉，在上一节贝叶斯滤波中我们都已经做了介绍，p(y∣x)p( y|x )p(y∣x),p(xk∣xk−1)p( x_k|x_{k-1})p(xk∣xk−1)的形状实际上和状态方程中噪声的概率分布形状是一样的，只是均值不同了。因此这个递推的(7)式和前面的非递推形式相比，公式里的概率都是已知的，权重的计算可以说没有编程方面的难度了。权重也有了以后，只要进行稍微的总结就可以得到SIS Filter。

Sequential Importance Sampling (SIS) Filter

在实际应用中我们可以假设重要性分布q()q()q()满足:
q(xk∣x0:k−1,y1:k)=q(xk∣xk−1,yk)q\left(x_{k} \mid x_{0: k-1}, y_{1: k}\right)=q\left(x_{k} \mid x_{k-1}, y_{k}\right) q(xk∣x0:k−1,y1:k)=q(xk∣xk−1,yk)
这个假设说明重要性分布只和前一时刻的状态xk−1x_{k-1}xk−1以及测量yky_kyk有关了，那么(7)式就可以转化为:
wk(i)∝wk−1(i)p(yk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣xk−1(i),yk)w_{k}^{(i)} \propto w_{k-1}^{(i)} \frac{p\left(y_{k} \mid x_{k}^{(i)}\right) p\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}\right)}{q\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}, y_{k}\right)} wk(i)∝wk−1(i)q(xk(i)∣xk−1(i),yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))

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

伪代码：

[{xk(i),wk(i)}i=1N]=SIS⁡({xk−1(i),wk−1(i)}i=1N,Yk){\left[\left\{x_{k}^{(i)}, w_{k}^{(i)}\right\}_{i=1}^{N}\right]=\operatorname{SIS}\left(\left\{x_{k-1}^{(i)}, w_{k-1}^{(i)}\right\}_{i=1}^{N}, Y_{k}\right)} [{xk(i),wk(i)}i=1N]=SIS({xk−1(i),wk−1(i)}i=1N,Yk)

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

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

重采样

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

通常采用有效粒子数来衡量粒子权值的退化程度，即

Neff=N/(1+var⁡(wk∗(i)))wk∗(i)=p(xk(i)∣y1:k)q(xk(i)∣xk−1(i),y1;k)\begin{array}{l}N_{e f f}=N /\left(1+\operatorname{var}\left(w_{k}^{*(i)}\right)\right) \\ w_{k}^{*(i)}=\frac{p\left(x_{k}^{(i)} \mid y_{1: k}\right)}{q\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}, y_{1 ; k}\right)}\end{array}Neff=N/(1+var(wk∗(i)))wk∗(i)=q(xk(i)∣xk−1(i),y1;k)p(xk(i)∣y1:k)

这个式子的含义是，有效粒子数越小，即权重的方差越大，也就是说权重大的和权重小的之间差距大，表明权值退化越严重。在实际计算中，有效粒子数可以近似为:N^eff≈1∑j=1N(wk(i))2\hat{N}_{e f f} \approx \frac{1}{\sum_{j=1}^{N}\left(w_{k}^{(i)}\right)^{2}}N^eff≈∑j=1N(wk(i))21

在进行序贯重要性采样时，若上式小于事先设定的某一阈值，则应当采取一些措施加以控制。克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加,影响计算的实时性。因此，一般采用以下两种途径: (1)选择合适的重要性概率密度函数; (2)在序贯重要性采样之后，采用重采样方法。

对于第一种方法:选取重要性概率密度函数的一个标准就是使得粒子权值的方差最小。关于这部分内容，还是推荐百度文库的那篇文章《粒子滤波理论》，他在这里也引申出来了几种不同的粒子滤波方法。

这里着重讲第二种方法，重采样。

重采样的思路是:既然那些权重小的不起作用了，那就不要了。要保持粒子数目不变，得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制几个?那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，也就是老大分身分得最多，老二分得次多，以此类推。下面以数学的形式来进行说明。
前面已经说明了求某种期望问题变成了这种加权和的形式:

p(xk∣y1:k)=∑i=1Nwk(i)δ(xk−xk(i))(8)p\left(x_{k} \mid y_{1: k}\right)=\sum_{i=1}^{N} w_{k}^{(i)} \delta\left(x_{k}-x_{k}^{(i)}\right) \qquad (8) p(xk∣y1:k)=i=1∑Nwk(i)δ(xk−xk(i))(8)
通过重采样以后，希望表示成:
p~(xk∣y1:k)=∑j=1N1Nδ(xk−xk(j))=∑i=1NniNδ(xk−xk(i))(9)\tilde{p}\left(x_{k} \mid y_{1: k}\right)=\sum_{j=1}^{N} \frac{1}{N} \delta\left(x_{k}-x_{k}^{(j)}\right)=\sum_{i=1}^{N} \frac{n_{i}}{N} \delta\left(x_{k}-x_{k}^{(i)}\right) \qquad (9) p~(xk∣y1:k)=j=1∑NN1δ(xk−xk(j))=i=1∑NNniδ(xk−xk(i))(9)

注意对比(8)和(9)。 xk(i)x_{k}^{(i)}xk(i)是第kkk时刻的粒子。xk(j)x_{k}^{(j)}xk(j)是kkk时刻重采样以后的粒子。其中nin_ini是指粒子
xk(i)x_k^{(i)}xk(i)在产生新的粒子集。xk(j)x_{k}^{(j)}xk(j)时被复制的次数。 (9) 式中第一个等号说明重采样以后，所有的粒子权重一样，都是1/N1/N1/N，只是有的粒子多出现了nin_ini次。

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

在这里用个简单的例子来说明:
假设有333个粒子，在第kkk时刻的时候，他们的权重分别是0.1,0.1,0.80.1, 0.1 ,0.80.1,0.1,0.8, 然后计算他们的概率累计和(matlab中为cumsum0 )得到: [0.1,0.2,1][0.1, 0.2, 1][0.1,0.2,1]。接着，我们用服从[0,1][0,1][0,1]之间的均匀分布随机采样333个值,假设为0.150.150.15, 0.380.380.38和0.540.540.54。也就是说，第二个粒子复制一次，第三个粒子复制两次。
将重采样的方法放入之前的SIS算法中，便形成了基本粒子滤波算法。

标准的粒子滤波算法流程为:

(1)粒子集初始化，k=0k=0k=0:
对于i=1,2,…,Ni=1,2,\dots,Ni=1,2,…,N,由先验p(x0)p(x_0)p(x0)生成采样粒子{x0(i)}i=1N\left\{x_0^{(i)}\right\}^N_{i=1}{x0(i)}i=1N

(2) 对于k=1,2,…k=1,2,\dotsk=1,2,…,循环执行以下步骤：

\qquad 1.1.1. 重要性采样：对于i=1,2,…,Ni=1,2,\dots,Ni=1,2,…,N,从重要性概率密度中生成采样粒子{x~k(i)}i=1N\left\{\tilde{x}_{k}^{(i)}\right\}_{i=1}^{N}{x~k(i)}i=1N,计算粒子权值w~k(i)\tilde{w}_k^{(i)}w~k(i),并进行归一化；

\qquad 2.2.2. 重采样：对粒子集{x~k(i),w~k(i)}\left\{\tilde{x}_{k}^{(i)}, \tilde{w}_{k}^{(i)}\right\}{x~k(i),w~k(i)}进行重采样，重采样后的粒子集为{xk(i),1/N,}\left\{x_k^{(i)},1/N,\right\}{xk(i),1/N,};

\qquad 3.3.3. 输出：计算kkk时刻的状态估计值：x~k=∑i=1Nx~k(i)w~k(i)\tilde{x}_k=\sum _{i=1}^N \tilde{x}_k^{(i)}\tilde{w}_k^{(i)}x~k=∑i=1Nx~k(i)w~k(i)

重采样的思路很简单，但是当你仔细分析权重的计算公式时: .

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

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

Sampling Importance Resampling Filter (SIR)

SIR滤波器很容易由前面的基本粒子滤波推导出来，只要对粒子的重要性概率密度函数做出特定的选择即可。在SIR中，选取:
q(xk(i)∣xk−1(i),yk)=p(xk(i)∣xk−1(i))q\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}, y_{k}\right)=p\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}\right)q(xk(i)∣xk−1(i),yk)=p(xk(i)∣xk−1(i))
p(xk∣xk−1)p(x_k|x_{k-1})p(xk∣xk−1)这是先验概率，在第一章贝叶斯滤波预测部分已经说过怎么用状态方程来得到它。将这个式子代入到SIS推导出的权重公式(7)中:
wk(i)∝wk−1(i)p(yk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣xk−1(i),yk)w_{k}^{(i)} \propto w_{k-1}^{(i)} \frac{p\left(y_{k} \mid x_{k}^{(i)}\right) p\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}\right)}{q\left(x_{k}^{(i)} \mid x_{k-1}^{(i)}, y_{k}\right)} wk(i)∝wk−1(i)q(xk(i)∣xk−1(i),yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))
得到:
wk(i)∝wk−1(i)p(yk∣xk(i))w_{k}^{(i)} \propto w_{k-1}^{(i)} p\left(y_{k} \mid x_{k}^{(i)}\right) wk(i)∝wk−1(i)p(yk∣xk(i))
由之前的重采样我们知道，实际上每次重采样以后，有wk−1(i)=1Nw^{(i)}_{k-1}=\frac{1}{N}wk−1(i)=N1所以上式可以进一步简化为：
wk(i)∝p(yk∣xk(i))w_{k}^{(i)} \propto p\left(y_{k} \mid x_{k}^{(i)}\right)wk(i)∝p(yk∣xk(i))
这里又出来一个概率采样p(yk∣xk(i))p\left(y_{k} \mid x_{k}^{(i)}\right)p(yk∣xk(i)),
实际怎么得到这个概率，程序里面又怎么去采样呢?

先搞清这个概率p(yk∣xk(i))p\left(y_{k} \mid x_{k}^{(i)}\right)p(yk∣xk(i))的含义，它表示在状态xxx出现的条件下，测量y出现的概率。在机器人定位里面就是，在机器人处于位姿xxx时，此时传感器数据y出现的概率。更简单的例子是，我要找到一个年龄是14岁的男孩(状态xxx)，身高为170 (测量yyy) 的概率。要知道yyy出现的概率，需要知道此时yyy的分布。这里以第一篇文章的状态方程为例，由系统状态方程可知，测量是在真实值附近添加了一个高斯噪声。因此，yyy的分布就是以真实测量值为均值，以噪声方差为方差的一个高斯分布。因此，权重的采样过程就是:当粒子处于xxx状态时，能够得到该粒子的测量yyy。要知道这个测量yyy出现的概率，就只要把它放到以真实值为均值，噪声方差为方差的高斯分布里去计算就行了:
w=η(2πΣ)−1/2exp⁡{−12(ytrue−y)Σ−1(ytrue−y)}w=\eta(2 \pi \Sigma)^{-1 / 2} \exp \left\{-\frac{1}{2}\left(y_{\text {true}}-y\right) \Sigma^{-1}\left(y_{\text {true}}-y\right)\right\} w=η(2πΣ)−1/2exp{−21(ytrue−y)Σ−1(ytrue−y)}

到这里，就可以看成SIR只和系统状态方程有关了，不用自己另外去设计概率密度函数，所以在很多程序中都是用的这种方法。

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

[{xk(i),wk(i)}i=1N]=SIR⁡({xk−1(i),wk−1(i)}i=1N,yk){\left[\left\{x_{k}^{(i)}, w_{k}^{(i)}\right\}_{i=1}^{N}\right]=\operatorname{SIR}\left(\left\{x_{k-1}^{(i)}, w_{k-1}^{(i)}\right\}_{i=1}^{N}, y_{k}\right)}[{xk(i),wk(i)}i=1N]=SIR({xk−1(i),wk−1(i)}i=1N,yk)

Fori=1:NFor \quad i=1:NFori=1:N
(1)采样粒子：xk(i)∼p(xk∣xk−1(i))\begin{array}{c}x_{k}^{(i)} \sim p\left(x_{k} \mid x_{k-1}^{(i)}\right) \end{array} xk(i)∼p(xk∣xk−1(i))
（2）计算粒子的权重：
wk(i)=p(yk∣xk(i))w_{k}^{(i)}=p\left(y_{k} \mid x_{k}^{(i)}\right) wk(i)=p(yk∣xk(i))
EndForEnd ForEndFor

(3) 计算粒子权重和，t=sum(w)t=sum(w)t=sum(w)

(4) 对每个粒子，用上面的权重和进行归一化,w=w/tw= w/tw=w/t

(5) 粒子有了，每个粒子权重有了,进行状态估计∑i=1NW~k(xk(i))f(xk(i))\sum_{i=1}^{N} \tilde{W}_{k}\left(x_{k}^{(i)}\right) f\left(x_{k}^{(i)}\right) i=1∑NW~k(xk(i))f(xk(i))

(6) 重采样

在上面算法中，每进行一次，都必须重采样一次，这是由于权重的计算方式决定的。
分析上面算法中的采样，发现它并没有加入测量yky_kyk。只是凭先验知识p(xk∣xk−1)p( x_k|x_{k-1} )p(xk∣xk−1)进行的采样，而不是用的修正了的后验概率。所以这种算法存在效率不高和对奇异点(outliers)敏感的问题。但不管怎样，SIRSIRSIR确实简单易用。