贝叶斯滤波和粒子滤波

贝叶斯滤波，状态估计，频率学派，一系列统称，

粒子滤波是贝叶斯滤波的特例（实现方式）、粒子耗散问题（不能解决）、FastSLAM

一、贝叶斯滤波

1.数学概念

2.特性

• 估计的是概率分布，不是具体数值；

​ 极大似然估计、极大后验估计

• 是一大类方法的统称；
• 是一个抽象的表达形式——对于不同问题有不同的实现方式（卡尔曼家族、粒子滤波）；
• 迭代估计形式

3.贝叶斯估计

$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)\overline{bel}(x_t)=\eta p(z_t|x_t)\begin{array}{r}\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})\mathrm{d}{x}_{t-1}\end{array}\text{\hspace{1em}(1-1)}$$

• $bel(x_t)$是后验概率——观测更新
• $\overline{bel}(x_t)$是预测分布——运动预测

功能：已知状态量t-1时刻的概率分布，在给定t时刻的观测数据$(z_t,u_t)$的情况下估计出状态量在t时刻的概率分布

4.推导

• 目标：在已知$p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})、u_t、z_t$的情况下，得到$p(x_t|z_{1:t},u_{1:t})$的表达式。

$$\begin{gathered} p(x_t|z_{1:t},u_{1:t})=\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1,u_{1:t}})} \\ =\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t}) \end{gathered}$$

其中：

$p(z_t|x_t,z_{1:t-1},u_{1:t})=p(z_t|x_t)$

$p(x_t|z_{1:t-1},u_{1:t})=\begin{array}{r}\int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})\mathrm{d}{x}_{t-1}\end{array}$

$p(x_t|x_{t-1},z_{1:t-1},u_{1:t})=p(x_t|x_{t-1},u_t)$

$p(x_{t-1}|z_{1:t-1},u_{1:t})=p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})$

令：

$bel(x_t)=p(x_t|z_{1:t},u_{1:t})$表示$x_t$的后验概率分布

$\overline{bel}(x_t)=p(x_t|z_{1:t},u_{1:t})$表示$x_t$的预测（proposal）概率分布

则：

$bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)$

$\overline{bel}(x_t)=\begin{array}{r}\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})\mathrm{d}{x}_{t-1}\end{array}$

2.粒子滤波

粒子滤波用一系列通过后验概率分布随机采样的状态粒子近似表示后验概率分布，采样得到的状态粒子点的疏密程度与该区域后验概率分布大小成正比，也就是说状态粒子点的枢密程度简介反映了后验概率分布的大小。这样粒子就可以直接参与系统的非线性变换，并利用运动和观测进行重新采样以调整状态粒子点的疏密程度。

粒子算法是一种基于遗传进化的算法，粒子经过运动和观测过程的筛选后，粒子点将逐渐集中到后验概率高的区域。

1.特性

• 贝叶斯估计器的一种实现方式
• 能处理非线性情况
• 能处理多峰分布的情况——全局定位
• 用系列粒子近似概率分布
• 非参滤波器

2.推导

$ X={ {(x_t^i,w_ti)|i=1,2,…,n} } $

$x_t^i$表示一个状态的假设——机器人位姿

$w_t^i$表示假设的权重——跟地图的匹配度

流程

• 用粒子进行状态传播：$x_t^i$~$p(x_t|u_t,x_{t-1}^i)$

• 评估每个粒子的权重： $w_t^i=\eta p(z_t|x_t)$

• 根据权重进行重采样：以 $w_t^i$的概率接受$x_t^i$，权重清零

①状态传播

传播模型：
$$\overline{bel}(x_t)=\begin{array}{r}\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})\mathrm{d}{x}_{t-1}\end{array}$$
已知t-1时刻的概率分布（粒子分布）：
$$p(x_{t-1}|z_{1:t-1,}{u}_{1:t-1})=(x_{t-1}^i,w_{t-1}^i)|i=1,2,...,n$$
根据数据$u_t$预测t-1时刻的概率分布（粒子分布）：

$$\begin{gathered} x_t^i\backsim p(x_t|u_t,x_{t-1}^i) \\ i=1,2,....,n \end{gathered}$$

②权重评估

• 无法知道机器人位姿的实际分布
• 从机器人的预测分布进行采样，联合权重一起近似机器人的后验概率分布
• 权重用来评估实际的预测分布和实际分布的差，差越大，权重越小
• 权重的定义

$$w=\frac{bel(x_t)}{\overline{bel}(x_t)}$$

对于某个粒子：
$$\begin{gathered} bel(x_t)=\eta p(z_t|x_t)p(x_t|x_{t-1},u_t)bel(x_t) \\ \overline{bel}(x_t)=p(x_t|x_{t-1},u_t)bel(x_t) \end{gathered}$$
因此权重为：
$$\begin{gathered} w=\frac{\eta p(z_t|x_t)p(x_t|x_{t-1},u_t)bel(x_t)}{p(x_t|x_{t-1},u_t)bel(x_t)} \\ =\eta p(z_t|x_t) \end{gathered}$$

③重采样

• 到目前为止，新的粒子群是根据proposal分布进行采样的，并且用观测模型计算权重，而最终的目的是用粒子群来近似后验概率分布
• 对粒子群进行重采样，对于某一个粒子$x_i$来说，以$w_i$的概率接受这个粒子
• 生成一个随机数，根据其落在的区间决定接受的粒子，重复N次。

3.算法流程

4.存在的问题

• 粒子耗散问题——粒子多样性的丧失
• 维数灾难
• 当proposal比较差的时候，需要用很多的粒子才能较好的表示机器人的后验概率分布。