非参数贝叶斯滤波——粒子滤波

非参数贝叶斯滤波——粒子滤波

粒子滤波以有限个参数来近似后验。粒子滤波的主要思想是用一系列从后验得到的随机状态采样表示后验$bel(x_t)$bel(xt​)。它可以表示比高斯分布更广泛的分布空间。基于样本表示法的另一个优点就是其建模随机变量的非线性变换的能力。

粒子滤波中，后验分布的样本叫作粒子（particles），有

$X_t:=x_{t}^{[1]}, x_t^{[2]},...,x_t^{[M]}$Xt​:=xt[1]​,xt[2]​,...,xt[M]​

每个粒子$x_t^{m}(1\le m\le M)$xtm​(1≤m≤M)是状态在时刻t的一个具体的实例。换句话说，一个粒子就是根据真实世界状态在时刻t的一种可能假设。到目前为止所讨论的所有其他贝叶斯滤波算法一样，由上一个时间步长的置信度$bel(x_{t-1})$bel(xt−1​)递归地构建置信度$bel(x_{t})$bel(xt​)。

在我看来，粒子就是离散的具有一定概率存在的位置，每个粒子随机（高斯分布）地撒在置信区域，把每个粒子看做是机器人所在的位置，根据代价函数或者适应度函数来确定哪个粒子更接近测量值，通过函数的结果分配每个粒子所在位置的置信度（可能在该位置的概率），该置信度也是机器人在该粒子所在位置的权重。

下面是粒子滤波算法

该算法的输入为前一刻的粒子集$X_{t-1}$Xt−1​、控制$u_t$ut​和测量值$z_t$zt​。算法首先构建一个暂时的粒子集$\overline{X_t}$Xt​​，表示置信度$\overline{bel(x_t)}$bel(xt​)​。该算法通过迭代处理上一刻粒子集中的每一个粒子$x_{t-1}^{[m]}$xt−1[m]​完成。

代码的3-7行是计算每个粒子的权重$w_t^m$wtm​还有此刻粒子置信度

代码第4行代表粒子根据状态转移分布$p(x_t|u_t,x_{t-1})$p(xt​∣ut​,xt−1​)中采样，该处可以用里程计或IMU数据进行估计，这步叫控制更新，由于存在轮胎打滑、地势坑洼等情况造成的误差，所以需要通过测量数据进行校正。

代码第5行是计算权重，权重是根据控制更新后得到的位置来算出此刻的测量概率，这就是前面所说的代价函数或适应度函数，如果在建好的图上进行机器人定位，可以根据测量值进行地图匹配，激光雷达根据距离来计算适应度。根据适应度获得该粒子的权重。

代码6行是计算粒子置信度。通过该时刻的估计位置$x_t^{[m]}$xt[m]​和该时刻的粒子权重$w_t^{[m]}$wt[m]​进行内积获得该粒子的贡献值，通过迭代所以粒子获取它们的贡献值，也就是粒子置信度。如果对他们的贡献值进行累加就是当前时刻的最终估计值。

代码的第8-11行实现了粒子滤波的真实“技巧”——重采样（resampling）

第9、10行通过在$\overline{X_t}$Xt​​中取M个粒子通过权重进行取样给$X_t$Xt​，其抽取概率又其权值给定。