Particle Filter

Posted on Edited on In Waline:

对于粒子滤波算法，结合网络上的资源进行整合总结。

粒子滤波

1 Bayesian Filter

假设有一个系统，其状态方程和测量方程为：

xk=fk(xk−1,vk−1)zk=hk(xk,nk)(1.1)x_k = f_k (x_{k-1}, v_{k-1}) \\ z_k = h_k (x_k, n_k) \tag{1.1}

其中 vkv_k 和 nkn_k 为过程噪声和测量噪声。

从贝叶斯理论，状态估计问题就是根据之前一系列的已有数数据 z1:kz_{1:k}（后验知识）递推的计算当前状态 xkx_k 的可信度，即 p(xk∣y1:k)p(x_k | y_{1:k})，通过预测和更新两个步骤来计算：

  • 预测过程就是根据状态方程的先验概率密度对状态进行猜测，即 p(xk∣xk−1)p(x_k | x_{k-1})。
  • 更新过程则利用最新的测量值对先验概率密度进行修正，即对之前的猜测进行修正。

一般假设系统的状态转移服从一阶马尔可夫模型，即当前时刻的状态 xkx_k 至于上一时刻的状态 xk−1x_{k-1} 有关，且当前时刻的测量数据 zkz_k 也至于当前状态 xkx_k 有关。

不妨假设已知 k−1k-1 时刻的概率密度为 p(xK−1∣y1:k−1)p(x_{K-1} | y_{1:k-1})。

1.1 预测过程

由上一时刻的概率密度 p(xk−1∣z1:k−1)p(x_{k-1} | z_{1:k-1}) 得到

p(xk∣z1:k−1)=∫p(xk,xk−1∣z1:k−1)dxk−1=∫p(xk∣xk−1,z1:k−1)p(xk−1∣z1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣z1:k−1)dxk−1(1.2)\begin{aligned} p(x_k | z_{1:k-1}) &= \int p(x_k, x_{k-1} | z_{1:k-1}) d x_{k-1} \\ &= \int p(x_k | x_{k-1}, z_{1:k-1}) p(x_{k-1} | z_{1:k-1}) d x_{k-1} \\ &= \int p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) d x_{k-1} \end{aligned} \tag{1.2}

式中，p(xk∣z1:k−1)p(x_k | z_{1:k-1}) 是假设已知的，而 p(xk∣xk−1)p(x_k | x_{k-1}) 由系统状态方程决定，其概率分布形式和过程噪声 vk−1v_{k-1} 相同，因此实际上 xk=Constant(xk−1)+vk−1x_k = \mathrm{Constant}(x_{k-1}) + v_{k-1}，即 xkx_k 由一个通过 xk−1x_{k-1} 计算得到的常量叠加噪声得到。

1.2 更新过程

由 p(xk∣y1:k−1)p(x_k | y_{1:k-1}) 得到后验概率 p(xk∣z1:k)p(x_k | z_{1:k})。这一步是对前面预测的再次修正（增加了 kk 时刻的测量），即进行了滤波。具体推导过程如下：

p(xk∣z1:k)=p(zk∣z1:k−1,xk)p(xk∣z1:k−1)p(zk∣z1:k−1)=p(zk∣xk)p(xk∣z1:k−1)p(zk∣z1:k−1)=p(zk∣xk)p(xk∣z1:k−1)∫p(zk∣z1:k−1,xk)p(xk∣z1:k−1)dxk=p(zk∣xk)p(xk∣z1:k−1)∫p(zk∣xk)p(xk∣z1:k−1)dxk(1.3)\begin{aligned} p(x_k | z_{1:k}) &= \frac{p(z_k | z_{1:k-1}, x_k) p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})} \\ &= \frac{p(z_k | x_k) p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})} \\ &= \frac{p(z_k | x_k) p(x_k | z_{1:k-1})}{\int p(z_k | z_{1:k-1}, x_k) p(x_k | z_{1:k-1}) d x_k} \\ &= \frac{p(z_k | x_k) p(x_k | z_{1:k-1})}{\int p(z_k | x_k) p(x_k | z_{1:k-1}) d x_k} \end{aligned} \tag{1.3}

等式第一行到第二行是测量 zkz_k 只与状态 xkx_k 有关（由测量方程得知），p(zk∣xk)p(z_k | x_k) 也为似然函数，由测量方程决定：同前面的推理，zk=h(xk)+nkz_k = h(x_k) + n_k，xkx_k 部分是常数，因此 p(zk∣xk)p(z_k | x_k) 只与测量噪声 nkn_k 的分布有关。

2 Monte Carlo Sampling

2.1 采样思想

假设能从一个目标概率分布 p(x)p(x) 中采样到一系列样本（粒子）：x1,...,xNx_1, ..., x_N，那么就能利用这些样本去估计这个目标的某些函数的期望值，如：

E[f(x)]=∫f(x)p(x)dxVar[f(x)]=E[f(x)−E[f(x)]]2=∫(f(x)−E(f(x)))2p(x)dx\begin{aligned} 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{aligned}

而蒙特卡洛采样的思想就是利用平均值来替代上面的积分：

E(f(x))≈f(x1)+⋯+f(xN)N(2.1)E(f(x)) \approx \frac{f(x_1) + \cdots + f(x_N)}{N} \tag{2.1}

可以从大数定理的角度去理解。

2.2 举例

用例子说明蒙特卡洛采样的思想：

  • 假设有一粒质地均匀的骰子，规定一次游戏中连续 4 次掷骰子，至少出现一次 6 点朝上则赢。现估计赢的概率：用 xk(n)x_k^{(n)} 表示第 nn 次游戏中第 kk 次的投掷结果，k=1,2,3,4k = 1, 2, 3, 4。则每次投掷服从均匀分布 x∼U[1,6]x \sim U[1, 6]，但是注意 xx 只取整数。因此目标分布 p(x(n))p(x^{(n)}) 在空间 χ={1,...,6}4\chi = \{1, ..., 6\}^4 均匀分布。

为估计获胜概率，定义一个指示函数：

f(x(n))=I{0<∑k=14I{xk(n)=6}}(2.2)f(x^{(n)}) = \mathbb{I} \{ 0 < \sum_{k=1}^4 \mathbb{I} \{ x_k^{(n)} = 6 \} \} \tag{2.2}

其中 I(x)\mathbb{I}(x) 表示当条件 xx 为 true 时取函数值为 1，否则为 0。由此可以估计游戏中取胜的期望，即取胜的概率：

θ=E[f(x)]≈1N∑n=1Nf(x(n))(2.3)\theta = E[f(x)] \approx \frac{1}{N} \sum_{n=1}^N f(x^{(n)}) \tag{2.3}

当 N→∞N \rightarrow \infty，上式就接近真实取胜概率了。

2.3 在滤波中使用蒙特卡洛采样

在贝叶斯滤波部分，后验概率的计算需要使用积分，为解决积分困难的问题，可以使用蒙特卡洛采样来代替计算后验概率。

假设可以从后验概率中采样到 NN 个样本，则有

p^(xk∣z1:k)=1N∑i=1Nδ(xk−xk(i))≈p(xk∣z1:k)(2.4)\hat{p}(x_k | z_{1:k}) = \frac{1}{N} \sum_{i=1}^N \delta(x_k - x_k^{(i)}) \approx p(x_k | z_{1:k}) \tag{2.4}

定义 f(x)=δ(xk−xk(i))f(x) = \delta(x_k - x_k^{(i)}) 为狄拉克函数（Dirac Delta Function），类似于上述指示函数。

此处已经求得后验概率，则如何使用于图像跟踪或滤波？要进行图像跟踪或滤波，其实就是要知道当前状态的期望值：

E[f(xk)]≈∫f(xk)p^(xk∣z1:k)dxk=1N∑i=1N∫f(xk)δ(xk−xk(i))dxk=1N∑i=1Nf(xk(i))(2.5)\begin{aligned} E[f(x_k)] &\approx \int f(x_k) \hat{p}(x_k | z_{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)}) \end{aligned} \tag{2.5}

也就是用这些采样的粒子的状态值直接平均就得到了期望值，也就是滤波后的值。

2.4 问题

蒙特卡洛采样的思路很简单，就是采样结果的平均值拟合所求后验概率。但是，后验概率是不知道的，如何从后验概率分布中进行采样？因此引入了下面的重要性采样方法。

3 Importance Sampling

3.1 采样方式

无法从目标分布中进行采样，就从一个已知的可采样分布中采样，如 q(xk∣z1:k)q(x_k | z_{1:k})，因此上述期望问题变为：

E[f(xk)]=∫f(xk)p(xk∣z1:k)q(xk∣z1:k)q(xk∣z1:k)dxk=∫f(xk)p(z1:k∣xk)p(xk)p(z1:k)q(xk∣z1:k)q(xk∣z1:k)dxk=∫f(xk)Wk(xk)p(z1:k)q(xk∣z1:k)dxk(3.1)\begin{aligned} E[f(x_k)] &= \int f(x_k) \frac{p(x_k | z_{1:k})}{q(x_k | z_{1:k})} q(x_k | z_{1:k}) d x_k \\ &= \int f(x_k) \frac{p(z_{1:k} | x_k) p(x_k)}{p(z_{1:k}) q(x_k | z_{1:k})} q(x_k | z_{1:k}) d x_k \\ &= \int f(x_k) \frac{W_k(x_k)}{p(z_{1:k})} q(x_k | z_{1:k}) d x_k \end{aligned} \tag{3.1}

其中：

Wk(xk)=p(z1:k∣xk)p(xk)q(xk∣z1:k)∝p(xk∣z1:k)q(xk∣z1:k)W_k(x_k) = \frac{p(z_{1:k} | x_k) p(x_k)}{q(x_k | z_{1:k})} \propto \frac{p(x_k | z_{1:k})}{q(x_k | z_{1:k})}

由于：

p(z1:k)=∫p(z1:k∣xk)p(xk)dxkp(z_{1:k}) = \int p(z_{1:k} | x_k) p(x_k) d x_k

所以：

E[f(xk)]=∫f(xk)Wk(xk)p(z1:k)q(xk∣z1:k)dxk=1p(z1:k)∫f(xk)Wk(xk)q(xk∣z1:k)dxk=∫f(xk)Wk(xk)q(xk∣z1:k)dxk∫p(z1:k∣xk)p(xk)dxk=∫f(xk)Wk(xk)q(xk∣z1:k)dxk∫Wk(xk)q(xk∣z1:k)dxk=Eq(xk∣z1:k)[Wk(xk)f(xk)]Eq(xk∣z1:k)[Wk(xk)](3.2)\begin{aligned} E[f(x_k)] &= \int f(x_k) \frac{W_k(x_k)}{p(z_{1:k})} q(x_k | z_{1:k}) d x_k \\ &= \frac{1}{p(z_{1:k})} \int f(x_k) W_k(x_k) q(x_k | z_{1:k}) d x_k \\ &= \frac{\int f(x_k) W_k(x_k) q(x_k | z_{1:k}) d x_k}{\int p(z_{1:k} | x_k) p(x_k) d x_k} \\ &= \frac{\int f(x_k) W_k(x_k) q(x_k | z_{1:k}) d x_k}{\int W_k(x_k) q(x_k | z_{1:k}) d x_k} \\ &= \frac{E_{q(x_k | z_{1:k})} [W_k(x_k) f(x_k)]}{E_{q(x_k | z_{1:k})} [W_k(x_k)]} \end{aligned} \tag{3.2}

上述期望，仍然可以使用蒙特卡洛采样近似求解，通过采样 NN 个样本 {xk(i)}∼q(xk∣z1:k)\{ x_k^{(i)} \} \sim q(x_k | z_{1:k})，上面近似为：

E[f(xk)]≈1N∑i=1NWk(xk(i))f(xk(i))1N∑i=1NWk(xk(i))=∑i=1NW~k(xk(i))f(xk(i))(3.3)\begin{aligned} E[f(x_k)] &\approx \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)})} \\ &= \sum_{i=1}^N \tilde{W}_k (x_k^{(i)}) f(x_k^{(i)}) \end{aligned} \tag{3.3}

其中：

W~k(xk(i))=Wk(xk(i))∑i=1NWk(xk(i))\tilde{W}_k(x_k^{(i)}) = \frac{W_k(x_k^{(i)})}{\sum_{i=1}^N W_k(x_k^{(i)})}

即为归一化的权重。

注意 (3.3) 式不再是 (2.5) 式中所有采样的简单平均，而是采用了加权平均。因此不同粒子有不同的权重，权重大的粒子说明这个粒子更加可信。

到这里解决了不能从后验概率中采样的问题。但是这种采样方法对于每新增加一个采样，p(xk∣z1:k)p(x_k | z_{1:k}) 都需要重新计算，并且这个式子的计算并不容易，因此需要想办法避免这个问题。最佳的形式就是能够递推的方式去计算这个权重，因此引入了序贯重要性采样（SIS）。

3.2 递推权重

设重要性概率密度函数 q(x0:k∣z1:k)q(x_{0:k} | z_{1:k})）（注意下标），即粒子滤波是估计过去所有时刻的状态的后验，假设其可分解为：

q(x0:k∣z1:k)=q(x0:k−1∣z1:k−1)q(xk∣x0:k−1,z1:k)q (x_{0:k} | z_{1:k}) = q(x_{0:k-1} | z_{1:k-1}) q(x_k | x_{0:k-1}, z_{1:k})

后验概率密度函数的地推形式可以表示为（Zk=z1:kZ_k = z_{1:k}）：

p(x0:k∣Zk)=p(zk∣x0:k,Zk−1)p(x0:k∣Zk−1)p(zk∣Zk−1)=p(zk∣x0:k,Zk−1)p(xk∣x0:k−1,Zk−1)p(x0:k−1∣Zk−1)p(zk∣Zk−1)=p(zk∣xk)p(xk∣xk−1)p(x0:k−1∣Zk−1)p(zk∣Zk−1)∝p(zk∣xk)p(xk∣xk−1)p(x0:k−1∣Zk−1)(3.4)\begin{aligned} p(x_{0:k} | Z_k) &= \frac{p(z_k | x_{0:k}, Z_{k-1}) p(x_{0:k} | Z_{k-1})}{p(z_k | Z_{k-1})} \\ &= \frac{p(z_k | x_{0:k}, Z_{k-1}) p(x_k | x_{0:k-1}, Z_{k-1}) p(x_{0:k-1} | Z_{k-1})}{p(z_k | Z_{k-1})} \\ &= \frac{p(z_k | x_k) p(x_k | x_{k-1}) p(x_{0:k-1} | Z_{k-1})}{p(z_k | Z_{k-1})} \\ &\propto p(z_k | x_k) p(x_k | x_{k-1}) p(x_{0:k-1} | Z_{k-1}) \end{aligned} \tag{3.4}

粒子权值的递推形式可以表示为：

wk(i)∝p(x0:k(i)∣Zk)q(x0:k(i)∣Zk)∝p(zk∣xk(i))p(xk(i)∣xk−1(i))p(x0:k−1(i)∣Zk−1)q(x0:k−1(i)∣Zk−1)q(xk(i)∣x0:k−1(i),Zk)=wk−1(i)p(zk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣x0:k−1(i),Zk)(3.5)\begin{aligned} w_k^{(i)} &\propto \frac{p(x_{0:k}^{(i)} | Z_k)}{q(x_{0:k}^{(i)} | Z_k)} \\ &\propto \frac{p(z_k | x_k^{(i)}) p(x_k^{(i)} | x_{k-1}^{(i)}) p(x_{0:k-1}^{(i)} | Z_{k-1})}{q(x_{0:k-1}^{(i)} | Z_{k-1}) q(x_k^{(i)} | x_{0:k-1}^{(i)}, Z_{k})} \\ &= w_{k-1}^{(i)} \frac{p(z_k | x_k^{(i)}) p(x_k^{(i)} | x_{k-1}^{(i)})}{q(x_k^{(i)} | x_{0:k-1}^{(i)}, Z_{k})} \end{aligned} \tag{3.5}

这种权重递推形式是采用了前面 (3.1) 式的形式下进行推导，即没有进行归一化。而 (3.3) 中所使用的权重是归一化的，所以实际中，wkw_k 计算出来以后需要进行归一化才能带入 (3.3) 式。

(3.5) 中结果的分子部分，在贝叶斯滤波部分已经介绍过，两个概率都是假设已知的，且与噪声有相同的概率分布形状，只是均值不同。有了递推公式，经过整理，就可以得到 SIS 滤波器。

4 Sequential Importance Sampling (SIS) Filter

4.1 算法

实际中，可以假设重要性分布 qq 满足：

q(xk∣x0:k−1,z1:k)=q(xk∣xk−1,zk)q(x_k | x_{0:k-1}, z_{1:k}) = q(x_k | x_{k-1}, z_k)

那么 (3.5) 式可以转化为：

wki∝wk−1(i)p(zk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣xk−1(i),zk)(4.1)w_k^{i} \propto w_{k-1}^{(i)} \frac{p(z_k | x_k^{(i)}) p(x_k^{(i)} | x_{k-1}^{(i)})}{q(x_k^{(i)} | x_{k-1}^{(i)}, z_k)} \tag{4.1}

由此，可以得到 SIS 滤波算法：

[{xk(i),wk(i)}i=1N]=SIS({xk−1(i),wk−1(i)}i=1N,Zk)For i=1:N    (1) Sampling: xk(i)∼q(xk(i)∣xk−1(i),zk−1);    (2) Iterative Computing Weights: wki∝wk−1(i)p(zk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣xk−1(i),zk)EndFor\begin{aligned} &[\{ x_k^{(i)}, w_k^{(i)} \}_{i=1}^N] = \mathrm{SIS} ({\{ x_{k-1}^{(i)}, w_{k-1}^{(i)} \}_{i=1}^N}, Z_k) \\ &\pmb{\mathrm{For}}\ i = 1 : N \\ &\ \ \ \ \text{(1) Sampling: } x_k^{(i)} \sim q(x_k^{(i)} | x_{k-1}^{(i)}, z_{k-1}); \\ &\ \ \ \ \text{(2) Iterative Computing Weights: } w_k^{i} \propto w_{k-1}^{(i)} \frac{p(z_k | x_k^{(i)}) p(x_k^{(i)} | x_{k-1}^{(i)})}{q(x_k^{(i)} | x_{k-1}^{(i)}, z_k)} \\ &\pmb{\mathrm{End For}} \end{aligned}

将粒子权值归一化，即可由 (3.3) 式对每个粒子的状态进行加权，估计目标的状态。

4.2 问题

SIS 算法就是粒子滤波的前身。但是在实际应用中，又发现了很多问题，如：

  • 粒子权重退化的问题，因此出现了重采样（Resampling），就有了基本的粒子滤波算法。
  • 重要性概率密度 qq 的选择问题。

5 Resampling and Particle Filter

5.1 问题提出

在 SIS 滤波中，存在一个退化问题：

  • 几次迭代后，很多粒子的权重变得很小，只有少数粒子权重变得很大；粒子权重的方差随着时间增长，状态空间中的有效粒子数减少。

通常采用有效粒子数来衡量退化程度：

Neff=N1+Var(wk∗(i))wk∗(i)=p(xk(i)∣z1:k)q(xk(i)∣xk−1(i),z1:k)(5.1)N_{eff} = \frac{N}{1 + \mathrm{Var}(w_k^{*(i)})} \\ w_k^{*(i)} = \frac{p(x_k^{(i)} | z_{1:k})}{q(x_k^{(i)} | x_{k-1}^{(i)}, z_{1:k})} \tag{5.1}

这个式子的含义是，有效粒子数越少，即权重的方差越大，表明权值退化越严重。实际计算中，有效粒子数也可以近似为：

N^eff≈1∑i=1N(wk(i))2(5.2)\hat{N}_{eff} \approx \frac{1}{\sum_{i=1}^N (w_k^{(i)})^2} \tag{5.2}

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

  • 选择合适的重要性概率密度函数；
  • 在序贯重要性采样之后，采用重采样方法。

对于第一种方法：选取重要性概率密度函数的一个标准就是使得粒子权值的方差最小。

而对于重采样，思路是：舍弃那些权重小的不起作用的粒子。为了保持粒子数目不变，用一些新的粒子来取代被舍弃的粒子。

找新粒子最简单的方法就是将权重大的粒子多复制几个，对于复制数量：让粒子根据自己权重所占的比例去分配，即权重大的粒子复制的数量多。以数学形式表达，根据前面提到的求期望问题可以转化为求加权和的形式：

p(xk∣z1:k)=∑i=1Nwk(i)δ(xk−xk(i))(5.3)p(x_k | z_{1:k}) = \sum_{i=1}^N w_k^{(i)} \delta(x_k - x_k^{(i)}) \tag{5.3}

通过重采样后，表示为：

p~(xk∣z1:k)=∑j=1N1Nδ(xk−xk(j))=∑i=1NniNδ(xk−xk(i))(5.4)\tilde{p}(x_k | z_{1:k}) = \sum_{j=1}^N \frac{1}{N} \delta(x_k - x_k^{(j)}) = \sum_{i=1}^N \frac{n_i}{N} \delta(x_k - x_k^{(i)}) \tag{5.4}

注意两个式子中，xk(i)x_k^{(i)} 表示第 kk 时刻的粒子，xkjx_k^{j} 表示第 kk 时刻重采样后的粒子，nin_i 是指粒子 xk(i)x_k^{(i)} 在产生新的粒子集 xk(j)x_k^{(j)} 时被复制的次数。式子 (5.4) 第一个等号说明在重采样后每个粒子 xk(j)x_k^{(j)} 的权重相等，第二个等号则表示了有的粒子其实是来自 xk(i)x_k^{(i)} 复制了 nin_i 次。

5.2 粒子滤波算法

  • 参考文献《On Resampling Algorithms for Particle Filters》。

算法：

(1) 粒子集初始化，k=0k=0：
  对于 i=1,2,...,Ni=1,2,...,N，由先验 p(x0)p(x_0) 生成采样粒子 {x0(i)}i=1N\{ x_0^{(i)} \}_{i=1}^N
(2) 对于 k=1,2,...k=1,2,...，循环执行以下步骤：
  ① 重要性采样：对于 i=1,2,...,Ni=1,2,...,N，从重要性概率密度中生成采样粒子 {x~k(i)}i=1N\{ \tilde{x}_k^{(i)} \}_{i=1}^N，计算粒子权重 w~k(i)\tilde{w}_k^{(i)}，并进行归一化；
  ② 重采样：对粒子集 {x~k(i),w~k(i)}\{ \tilde{x}_k^{(i)}, \tilde{w}_k^{(i)} \} 进行重采样，得到粒子集 {xk(i),1N}\{ x_k^{(i)}, \frac{1}{N} \}；
  ③ 输出：计算 kk 时刻的状态估计值：x^k=∑i=1Nx~k(i)w~k(i)\hat{x}_k = \sum_{i=1}^N \tilde{x}_k^{(i)} \tilde{w}_k^{(i)}

5.3 提出问题

分析权重的计算公式时，通过

Wk(xk)=p(z1:k∣xk)p(xk)q(xk∣z1:k)∝p(xk∣z1:k)q(xk∣z1:k)W_k(x_k) = \frac{p(z_{1:k} | x_k) p(x_k)}{q(x_k | z_{1:k})} \propto \frac{p(x_k | z_{1:k})}{q(x_k | z_{1:k})}

**会有疑问，权重大的就多复制几次，这一定是正确的吗？**权重大，如果是分子大，即后验概率大，那说明确实应该在后验概率大的地方多放几个粒子。但权重大也有可能是分母小造成的，这时候的分子也可能小，也就是实际的后验概率也可能小，这时候的大权重可能就没那么优秀了。何况，这种简单的重采样会使得粒子的多样性丢失，到最后可能都变成了只剩一种粒子的分身。粒子滤波里也有专门的方法：正则粒子滤波。

至此，粒子滤波算法已经明朗，但实际应用中还有一个问题，就是重要性概率密度的选择，即 q(⋅)q(\cdot) 的选择。

6 Sampling Importance Resampling Filter

6.1 基本方法

SIR 滤波由前面的粒子滤波推导而来，只要对粒子的重要性概率密度函数做出特定的选择即可。在 SIR 中，选择：

q(xk(i)∣xk−1(i),zk)=p(xk(i)∣xk−1(i))(6.1)q(x_k^{(i)} | x_{k-1}^{(i)}, z_k) = p(x_k^{(i)} | x_{k-1}^{(i)}) \tag{6.1}

注意等式右边为已知的先验概率，在第一章贝叶斯滤波部分已经解释了如何计算。将上式代入 (3.5) 式的权重公式中：

wk(i)∝wk−1(i)p(zk∣xk(i))p(xk(i)∣xk−1(i))q(xk(i)∣x0:k−1(i),Zk)=wk−1(i)p(zk∣xk(i))(6.2)\begin{aligned} w_k^{(i)} &\propto w_{k-1}^{(i)} \frac{p(z_k | x_k^{(i)}) p(x_k^{(i)} | x_{k-1}^{(i)})}{q(x_k^{(i)} | x_{0:k-1}^{(i)}, Z_{k})} \\ &= w_{k-1}^{(i)} p(z_k | x_k^{(i)}) \end{aligned} \tag{6.2}

由重采样部分介绍的内容，发现每一次重采样后，粒子的权重都会被更新为 1N\frac{1}{N}，即 wk−1(i)=1Nw_{k-1}^{(i)} = \frac{1}{N}，因此 (6.2) 式可以进一步简化为：

wk(i)∝p(zk∣xk(i))(6.3)w_k^{(i)} \propto p(z_k | x_k^{(i)}) \tag{6.3}

此处出现了一个概率采样 p(zk∣xk(i))p(z_k | x_k^{(i)})，实际中的计算方法如何？

6.2 概率采样

p(zk∣xk(i))p(z_k | x_k^{(i)}) 在机器人定位中，表示机器人处于位姿 xk(i)x_k^{(i)} 时传感器数据 zkz_k 出现的概率。由第一章的测量方程可知，测量值是在真实值附近添加了高斯噪声，因此 zkz_k 的分布就是以真实值 h(xk)h(x_k) 为均值，方差为噪声方差的高斯分布：

w=η(2π∣Σ∣)−12exp⁡{−12(z−ztrue)TΣ−1(z−ztrue)}(6.4)w = \eta (2 \pi |\Sigma|)^{-\frac{1}{2}} \exp \{ -\frac{1}{2} (z - z_{true})^T \Sigma^{-1} (z - z_{true}) \} \tag{6.4}

至此，就可以看出，SIR 至于系统的状态转移方程和测量方程有关。

6.3 SIR 滤波算法

伪码：

[{xk(i),wk(i)}i=1N]=SIR({xk−1(i),wk−1(i)}i=1N,zk)[\{ x_k^{(i)}, w_k^{(i)} \}_{i=1}^N] = \mathrm{SIR} (\{ x_{k-1}^{(i)}, w_{k-1}^{(i)} \}_{i=1}^N, z_k)
 For i=1:Ni = 1 : N
  (1) 采样粒子：x~k(i)∼p(xk∣xk−1(i))\tilde{x}_k^{(i)} \sim p(x_k | x_{k-1}^{(i)})
  (2) 计算粒子权重：w~k(i)=p(zk∣xk(i))\tilde{w}_k^{(i)} = p(z_k | x_k^{(i)})
 END For
 计算粒子权重和：sumw=∑i=1Nw~k(i)sum_w = \sum_{i=1}^N \tilde{w}_k^{(i)}
 权重归一化：w~k(i)=w~k(i)sumw\tilde{w}_k^{(i)} = \frac{\tilde{w}_k^{(i)}}{sum_w}|
 状态估计：x^k=∑i=1Nw~k(i)x~k(i)\hat{x}_k = \sum_{i=1}^N \tilde{w}_k^{(i)} \tilde{x}_k^{(i)}
 重采样

7 SIR 粒子滤波应用

假设有状态方程和测量方程：

xk=xk−12+25xk−11+xk−12+8cos⁡(1.2(k−1))+vkzk=xk220+nkx_k = \frac{x_{k-1}}{2} + \frac{25 x_{k-1}}{1 + x_{k-1}^2} + 8 \cos (1.2(k-1)) + v_k \\ z_k = \frac{x_k^2}{20} + n_k

有 MatLab 代码为：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
%% 参见博客http://blog.csdn.net/heyijia0327/article/details/40899819
clear all
close all
clc
%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 过程噪声的协方差  (由于是一维的，这里就是方差)
x_R = 1; % 测量噪声的协方差
T = 75;  % 共进行75次迭代
N = 100; % 粒子数，越大效果越好，计算量也越大
 
% initilize our initial, prior particle distribution as a gaussian around
% the true initial value
 
V = 2; % 初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
    x_P(i) = x + sqrt(V) * randn;
end
 
z_out = [x^2 / 20 + sqrt(x_R) * randn];  %实际测量值
x_out = [x]; % the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
 
for t = 1 : T
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2 * (t - 1)) + sqrt(x_N) * randn; 
    z = x^2 / 20 + sqrt(x_R) * randn;
    for i = 1 : N
        % 从先验 p(x(k)|x(k-1)) 中采样
        x_P_update(i) = 0.5 * x_P(i) + 25 * x_P(i) / (1 + x_P(i)^2) + 8 * cos(1.2 * (t - 1)) + sqrt(x_N) * randn;
        % 计算采样粒子的值，为后面根据似然去计算权重做铺垫
        z_update(i) = x_P_update(i)^2 / 20;
        % 对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
        P_w(i) = (1 / sqrt(2 * pi * x_R)) * exp(-(z - z_update(i))^2 / (2 * x_R));
    end
    % 归一化.
    P_w = P_w / sum(P_w);
  
    %% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的
    for i = 1 : N
        x_P(i) = x_P_update(find(rand <= cumsum(P_w), 1));   % 粒子权重大的将多得到后代
    end                                                     % find( ,1) 返回第一个符合前面条件的数的下标
    
    % 状态估计，重采样以后，每个粒子的权重都变成了1/N
    x_est = mean(x_P);
    
    % Save data in arrays for later plotting
    x_out = [x_out, x];
    z_out = [z_out, z];
    x_est_out = [x_est_out, x_est];
    
end
 
t = 0 : T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r', 'linewidth', 3);
set(gca, 'FontSize', 12); set(gcf, 'Color', 'White');
xlabel('time step');
ylabel('flight position');
legend('True flight position', 'Particle filter estimate');

附录

Appendix.1 Dirac Delta Function

函数表示为：

δ(x)=0,(x≠0)∫−∞∞δ(x)dx=1\delta(x) = 0, (x \ne 0) \\ \int_{-\infty}^{\infty} \delta(x) dx = 1

上述表达式不规定 δ\delta 函数在 0 点的取值，是因为这个值无法严谨地表述出来，不能笼统的定义为正无穷，并且函数取值的“大小”是由第二个积分式决定的，因此只需限定取值为零的区域即可。如果函数不在 0 点取非零值，而在其他地方，可定义：

δa(x)=δ(x−a)=0,(x≠a)∫−∞∞δa(x)dx=1\delta_a(x) = \delta(x - a) = 0, (x \ne a) \\ \int_{-\infty}^{\infty} \delta_a(x) dx = 1

参考