概率机器人笔记(2)：贝叶斯滤波(递归状态估计)

1.没有控制输入的状态估计

公式推导：
已知贝叶斯公式：
$$P(x|y)=\frac{P(y|x)P(x)}{P(y)}$$
添加了随机变量$z$这个条件之后的贝叶斯公式：
$$P(x|y,z)=\frac{P(y|x,z)P(x|z)}{P(y|z)}$$
接下来做一下变量的替换：
令$y=z_n$
令$z=z_1,z_2,\cdot \cdot \cdot ,z_{n-1}$
带入上式：
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)=\frac{P(z_n|x,z_1,z_2,\cdot \cdot \cdot ,z_{n-1})P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})}{P(z_n|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})}$$
其中，$x$是状态，机器人学中可以认为是机器人的状态，包括机器人的位姿，机器人的线速度或角速度，环境中周围物体的位置和特征等。$z_1,z_2,\cdot \cdot \cdot ,z_n$时间从$1$到$n$环境测量数据的集合。
化简：
根据马尔可夫假设，当状态已知，测量$z_n$便独立于之前的测量，理解起来好比，$x$为门的状态(开或关)，$z$为传感器测量的结果(开或关，传感器带有误差)。
当不知道$x$时，即门的开关状态未知时，$z_n$的结果依赖于之前测量的结果($z_1,z_2,\cdot \cdot \cdot ,z_{n-1}$)，如果之前测量的结果是门开启的概率大，那么$z_n$的结果差不多也是这样，门大概率开着；
当$x$的状态已经知道时，假设门是开着的，那么$z_n$的结果，不在依赖于之前测量的结果，而只与传感器的测量精度有关了。
因此：
$$P(z_n|x,z_1,z_2,\cdot \cdot \cdot ,z_{n-1})=P(z_n|x)$$
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)=\frac{P(z_n|x)P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})}{P(z_n|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})}$$
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)=\eta P(z_n|x)P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})$$
将上式的$n$用$n-1$替换，带入上式
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-1})={\eta }^{\ast }P(z_{n-1}|x)P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-2})$$
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)=\eta {\eta }^{\ast }P(z_n|x)P(z_{n-1}|x)P(x|z_1,z_2,\cdot \cdot \cdot ,z_{n-2})$$
$$\cdot \cdot \cdot \cdot \cdot \cdot$$
从上式我们可以看出递归的影子，继续化简
$$P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)={\eta }_{1\cdot \cdot \cdot n}\left[\prod _{i=1}^{n}P(z_n|x)\right]P(x)$$
这里和上面的$\eta$都是为了保证等式左边以$x$累加之和等于1(${\sum }_{x}P(x|z_1,z_2,\cdot \cdot \cdot ,z_n)=1$)而设置的一个常数，其大小就等于${\sum }_x\left[\left({\prod }_{i=1}^{n}P(z_n|x)\right)P(x)\right]$
例子：

第一次测量：
$P(z_1|open)=0.6$ 当门开着的时候机器人测量结果为$z_1$的概率是0.6
$P(z_1|!open)=0.3$ 当门关着的时候机器人测量结果为$z_1$的概率是0.3
$P(open)=P(!open)=0.5$ 门的状态是开启还是关闭的概率均为0.5
$$P(open|z_1)=\frac{P(z_1|open)P(open)}{P(z_1|open)P(open)+P(z_1|!open)P(!open)}$$
$$=\frac{0.6\cdot 0.5}{0.6\cdot 0.5+0.3\cdot 0.5}\approx 0.67$$
当测量结果为$z_1$时，门是开着的概率是0.67
第二次测量：
$P(z_2|open)=0.5$ 当门开着的时候，测量结果为$z_2$的概率为0.5
$P(z_2|!open)=0.6$ 当门关着的时候，测量结果为$z_2$的概率为0.6
$P(open|z_1)=0.67$ 当测量结果为$z_1$的时候，门是开着的概率为0.67，我们在第一次测量时得到的：
$$P(open|z_1,z_2)=\frac{P(z_2|open)P(open|z_1)}{P(z_2|open)P(open|z_1)+P(z_2|!open)P(!open|z_1)}$$
$$=\frac{0.5\cdot 0.67}{0.5\cdot 0.67+0.6\cdot 0.33}\approx 0.63$$

2.增加控制输入的状态估计

为了表示控制输入$u$对状态的影响，引入状态转移概率：
$$P(x|u,x^{\prime })$$
根据全概率公式：
$$P(x)=\sum _{x^{\prime }}P(x|x^{\prime })P(x^{\prime })$$
增加了条件$u$之后：
$$P(x|u)=\sum _{x^{\prime }}P(x|u,x^{\prime })P(x^{\prime }|u)$$
由于$P(x^{\prime }|u)$是当执行完$u$之后，$x^{\prime }$的概率，即便是执行$u$之前，$x^{\prime }$的概率依然是这个，也就是说，上一时刻的状态不会由下一时刻的动作改变，下一时刻的动作只能改变下一时刻的状态，不能改变过去，因此上一时刻的状态和这一时刻的动作是独立的，$P(x^{\prime }|u)=P(x^{\prime })$，于是就有下面的式子：
$$P(x|u)=\sum _{x^{\prime }}P(x|u,x^{\prime })P(x^{\prime })$$
当让$x$代表门的开关状态为开，$u$代表机器人开门的动作，那么$P(x|u)$是在机器人执行了开门动作之后，门开着的概率。根据全概率公式，执行动作之前门的状态是有开和关两个划分组成，机器人执行开的指令后门开的概率和门之前的状态有关，二者一般不一样。例如当门开着时，执行开门指令后，门开的概率一般大于门关着时执行开门指令后，门开的概率。因此这两个概率应该相加才是真正的执行动作后门开的概率。
例子：
接上例，第三次测量之前，机器人发出了控制关门的指令$u$


$$P(closed|u)=\sum _{x^{\prime }}P(closed|u,x^{\prime })P(x^{\prime })$$
$$=P(closed|u,open)P(open)+P(closed|u,closed)P(closed)$$
$$=0.9\cdot 0.63+1\cdot 0.37=0.937$$
$$P(open|u)=\sum _{x^{\prime }}P(open|u,x^{\prime })P(x^{\prime })$$
$$=P(open|u,open)P(open)+P(open|u,closed)P(closed)$$
$$=0.1\cdot 0.63+0\cdot 0.37=0.063$$

3.贝叶斯滤波

(1)隐马尔可夫模型(HMM)

假设机器人的控制和测量都服从上面的隐马尔科夫模型，可以知道$u_t$代表$t$时刻的控制输入，$z_t$代表$t$时刻的测量，$x_t$代表$t$时刻的隐状态。
测量概率
当想知道$t$时刻的测量值$z_t$的概率时，通过上面的隐马尔科夫模型可以看出，似乎依赖$t$时刻及之前所有的隐状态$x_t,x_{t-1},\cdot \cdot \cdot ,x_0$、控制输入$u_t,u_{t-1},\cdot \cdot \cdot ,u_1$以及观测$z_{t-1},z_{t-2},\cdot \cdot \cdot ,z_1$，但是，隐马尔可夫假设当隐状态已知时，此时的观测独立，即任意时刻的观测只依赖于该时刻的马尔科夫链的状态，与其他观测即状态无关。可以得到下面的条件独立：
$$P(z_t|x_{0:t},z_{1:t-1},u_{1:t})=P(z_t|x_t)$$
状态转移概率
当想知道$t$时刻的状态$x_t$的概率时，通过上面的隐马尔科夫模型可以看出，似乎依赖之前所有的隐状态$x_{t-1},x_{t-2},\cdot \cdot \cdot ,x_0$、控制输入$u_t,u_{t-1},\cdot \cdot \cdot ,u_1$以及观测$z_{t-1},z_{t-2},\cdot \cdot \cdot ,z_1$，但是当$x_{t-1}$时刻的隐状态已知时，$x_t$仅依赖与上一时刻的隐状态$x_{t-1}$和当前时刻的控制输入$u_t$，可以得到下面的条件独立：
$$P(z_t|x_{0:t-1},z_{1:t-1},u_{1:t})=P(x_t|x_{t-1},u_t)$$
置信度
由于机器人的状态不能直接获得，因此往往通过测量数据以及输入控制数据推测出机器人的状态：
$$bel(x_t)=P(x_t|z_{1:t},u_{1:t})$$
即上面的置信度分布是所有可获得的数据为条件的关于状态变量的后验分布，为每一个可能的状态分配了一个概率，下面就要化简这个置信度分布。
我们曾经得到过添加了其他条件$z$之后的贝叶斯公式：
$$P(x|y,z)=\frac{P(y|x,z)P(x|z)}{P(y|z)}$$
上述置信度分布也类似进行分解：
令$x\to x_t$
令$y\to z_t$
令$z\to z_{1:t-1},u_{1:t}$
前面把$z_t$单独取出，是因为可以得到测量概率，一个比较容易获得的量，一般与传感器的精度有关：
$$bel(x_t)=P(x_t|z_{1:t},u_{1:t})=\frac{P(z_t|x_t,z_{1:t-1})P(x_t|z_{1:t-1},u_{1:t})}{P(z_t|z_{1:t-1},u_{1:t})}$$
根据因马尔科夫假设$P(z_t|x_t,z_{1:t-1})=P(z_t|x_t)$，化简得：
$$P(x_t|z_{1:t},u_{1:t})=\eta P(z_t|x_t)P(x_t|z_{1:t-1},u_{1:t})P(z_t|z_{1:t-1},u_{1:t})$$
为了化简$P(x_t|z_{1:t-1},u_{1:t})$这一项，为了凑出递归，需要引入$x_{t-1}$项，但是，在最终结果里面不应该有$x_{t-1}$项，巧妙地利用积分(求和，离散情况)将$x_{t-1}$项消掉，这种全概率思想在这类问题上应用比较多。
$$P(x_t|z_{1:t-1},u_{1:t})={\int }_{x_{t-1}}P(x_t|z_{1:t-1},u_{1:t},x_{t-1})P(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}$$
由于现在的动作不会对以前的状态产生影响，$P(x_{t-1}|z_{1:t-1},u_{1:t})=P(x_{t-1}|z_{1:t-1},u_{1:t-1})$
$$P(x_t|z_{1:t-1},u_{1:t})={\int }_{x_{t-1}}P(x_t|z_{1:t-1},u_{1:t},x_{t-1})P(x_{t-1}|z_{1:t-1},u_{1:t-1})d{x}_{t-1}$$
$$P(x_t|z_{1:t-1},u_{1:t})={\int }_{x_{t-1}}P(x_t|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}$$
$$bel(x_t)=\eta P(z_t|x_t){\int }_{x_{t-1}}P(x_t|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}$$
由此构成了递归，$bel(x_{t-1})$是上一时刻的置信度，乘状态转移概率之后做求和，便是没有测量条件下的状态$x_t$的分布概率，但是$bel(x_t)$是基于以前所有测量和控制的，因此，通过$\eta P(z_t|x_t)$进行了修正，$\eta$为常数，$P(z_t|x_t)$越大，说明在给定状态$x_t$下，$z_t$出现的可能性比较大，因此$z_t$条件下$x_t$的概率也越大，即$bel(x_t)$越大。
例子：

分析上面这个简单的例子：
(a)
机器人在没有打开传感器之前，它在任何一点的概率(置信度)都相同。
(b)
当机器人经过了一次传感器的测量之后，发现自己位于门附近，由于机器人事先知道了该地图，因此机器人认为，在三个门附近的概率较其他地方更大，但是，三个门附近的概率相同，不能确定位于哪个门附近，此时得到了$P(z|x)$。
©
当机器人运动时，由于三个位置不能确定，因此这三个“峰值”也会跟随机器人运动。
运动带来的不确定性使得本次“峰值”比之前更小。
(d)
$bel(x_d)=\eta P(z|x)\ast bel(x_c)$，可以看到五个“峰值”，第二个门处峰值最大也是归一化的结果。
(e)
在接下来的运动中，“峰值”会一直跟随机器人一起运动，但是由于运动的不确定性，“峰值”的高度会越来越小。