Probabilistic Robotics读书笔记（一）

转自我的博客http://gongzheng92.net

---

贝叶斯滤波器的推导

模型与方程

首先我们从贝叶斯滤波器谈起。
首先我们需要的是对机器人目前状态（states）的估计，用概率的方式表达为：

$$p(x_t│x_{0:t−1},z_{1:t−1},u_{1:t} )$$
上式可以在诸多假设条件（马尔科夫假设，观测不对环境造成影响等）下化为：
$$p(x_t│x_{t−1},u_t ) \tag{1}$$
同样的我们有测量模型：
$$p(z_t│x_{0:t},z_{1:t−1},u_{1:t} )=p(z_t│x_t ) \tag{2}$$
然而，由于 状态(states)(x)的 不可直接观测，于是我们提出了 置信度（belief）的概念。于是我们希望得到的状态(1)估计便可转化为：
$$\color{red}{bel(x_t )}=p(x_t |z_{1:t},u_{1:t}） \tag{3}$$
注意这里并没有应用马尔科夫假设。

然而这里对$x_t$ 的估计是先完成测量，再进行估计,因此(3)还有一种形式，便是

$$先完成估计，再进行测量$$ ，即
$$\color{blue}{\overline{bel}(x_t )}=p(x_t│z_{1:t−1},u_{1:t} ) \tag{4}$$
于是我们就有了贝叶斯滤波器:
$$\begin{align} Byes&\_filter(bel(x_{t−1} ),u_t,z_t)\\ &\text{for all $x_t$ do}\\ &\color{blue}{\overline{bel}(x_t )}=\int \color{magenta}{p(x_t |u_t,x_{t−1} )}\color{red}{bel(x_{t−1} )}dx\\ &\color{red}{bel(x_t)}=\eta \color{green}{p(z_t|x_t)}\color{blue}{ \overline{bel}(x_t)}\\ &\text{endfor}\\ &\text{return }bel(x_t) \end{align}\tag{5}$$

其实在这里，$\color{magenta}{p(x_t |u_t,x_{t−1) }}$ 就是系统模型，$\color{green}{p(z_t│x_t )}$就是测量模型。

推导过程

那么这个(5)是怎么来的呢？我们从(3)推起：

$$\begin{align} \color{red}{(p(x_t│z_{1:x},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 \color{green}{p(z_t│x_t,z_{1:t−1},u_(1:t) )}\color{blue}{p(x_t│z_{1:t−1},u_{1:t} ) }) \end{align}\tag{6}$$
其中因为 $p(z_t│z_{1:t−1},u_{1:t} )$与我们感兴趣的 $x_t$没有关系，因此可范化为 $\eta$。

$p(x,y)=p(x│y)p(y)=p(x)p(y)$ 记住联合概率（,）的运算级别高于条件概率（|）。
然后在这里推一下书上没推的公式：
Bayes rule: $p(x│y)=\frac{p(y│x)p(x)}{p(y)}$

$$\begin{align} p(x│y,z)&=\frac{p(y,z│x)p(x)}{p(y,z)} \\ &=\frac{p(y,z,x)}{p(y│z)p(z) }\\ &=\frac{p(y│x,z)p(x,z)}{p(y│z)p(z)}\\ &=\frac{p(y│x,z)p(x)}{p(y|z)} \end{align}$$

然后在$\color{green}{p(z_t│x_t,z_{1:t−1},u_{1:t} )}$之中，由于我们假设测量$z_t$只于当前状态有关，与之前的状态与历史控制量均无关系，因此我们有：

$$\color{green}{p(z_t|x_t,z_{1:t−1},u_{1:t} )=p(z_t│x_t )}$$

于是有：

$$\color{red}{p(x_t│z_{1:t},u_{1:t} )}=\eta \color{green}{p(z_t│x_t )}\color{blue}{p(x_t |z_{1:t−1},u_{1:t})} \tag{7}$$

即

$$\color{red}{bel(x_t)}=\eta \color{green}{p(z_t│x_t )}\color{blue}{\overline{bel}(x_t)}$$
但其实这句话很废话，因为就是“测量前的估计乘以测量等于测量后的估计”。
然后：
$$\begin{align} \color{blue}{\overline{bel} (x_t )}&=p(x_t│z_{1:t−1},u_{1:t} )\\ &=\int \color{magenta}{p(x_t│x_{t−1},z_{1:t−1},u_{1:t} )}\color{red}{p(x_{t−1}│z_{1:t−1},u_{1:t} )}dx_{t−1} \end{align}\tag{8}$$

然后由于状态转移假设（其中包括了马尔科夫假设），则

$$\color{magenta}{p(x_t│x_{t−1},z_{1:t−1},u_{1:t} )} = p(x_t|x_{t-1},u_t)$$
再加之忽略红色 $\color{red}{p(x_{t−1}│z_{1:t−1},u_{1:t} )}$部分的 $u_t$ （因为是未来的控制量），于是我们有
$$\color{blue}{\overline{bel}(x_t )}=\int \color{magenta}{p(x_t |u_t,x_{t−1} )}\color{red}{bel(x_{t−1} )}dx$$