十分钟搞懂（机器人概率估计）贝叶斯滤波算法和实例

一、贝叶斯滤波算法

贝叶斯滤波（Bayes filter）算法是用来计算置信度的常用算法。该算法的步骤是根据测量数据和控制数据计算置信度分布bel（）。

这里肯定有些同学不明白置信度的含义，别急，往下看。

置信度是概率机器人的一个重要概念。机器人无法直接测量在环境中的真实的状态。比如，一个机器人在全局坐标系中的位姿为$x_t=[151045°]{}^T$ ，但是这个位姿机器人并不能够直接测量，而是需要的可以获得的数据中推测其位姿。
我们一般通过条件概率分布来表示置信度。对于离散情况，每一个可能的假设都会分配一个概率值，对于连续的情况，就为概率密度曲线。
这里用$bel(x_t)$表示状态变量$x_t$的置信度，则后验概率为：
$$bel(x_{z-1})=p(x_t|z_{1:t},u_{1:t})$$
这个后验是在测量数据$z_{1:t}$和控制数据$u_{1:t}$的条件下，时刻t下状态$x_t$的概率分布。
可以得知后验概率是考虑了t时刻下的测量数据$z_t$，在不考虑t时刻测量数据时，该概率表示为：
$$\overline{bel}(x_t)=p(x_t|z_{1:t-1},u_{1:t})$$
该概率被称为预测（prediction）。
$\overline{bel}(x_t)$是基于以前状态的后验，在未结合t时刻的测量数据$z_{1:t}$的条件下，预测了t时刻的状态。由 $\overline{bel}(x_t)$和$z_{1:t}$联合计算$bel(x_t)$称为修正或测量更新。

相信到这里大家都明白置信度的涵义了。接下来介绍贝叶斯滤波算法。

伪代码如下：
1: Bayes_filter($bel(x_{t-1}),u_t,z_t$) :
2: for all $x_t$ do:
3: $\overline{bel}(x_t)=\int p(x_t|u_{1:t},x_{1:t-1})d{x}_{t-1}$
4: $bel(x_t)=$𝛈$p(z_t|x_t)$$\overline{bel}(x_t)$
5: return $bel(x_t)$

贝叶斯滤波是递归的。即时刻t的置信度$bel(x_t)$由时刻t-1的置信度$bel(x_{t-1})$来计算。

。
。
。
有时间再更新～