贝叶斯公式推导

贝叶斯单一观测

$$p(x/z)=\frac{p(z/x)p(x)}{p(z)}$$

贝叶斯多种观测

$$p(x/z,u)=\frac{p(z,x,u)}{p(z,u)}=\frac{p(z/x,u)p(x,u)}{p(z,u)}=\frac{p(z/x,u)p(x/u)p(u)}{p(z/u)p(u)}=\frac{p(z/x,u)p(x/u)}{p(z/u)}$$

SLAM中贝叶斯公式推导

$$\begin{array}{rl}Bel(x_t)& =p(x_t/x_0,z_{1:t},u_{1:t})\\ & =\frac{p(z_t/x_t,z_{1:t-1},u_{1:t})p(x_t/x_0,z_{1:t-1},u_{1:t})}{p(z_t/z_{1:t-1},u_{1:t})}(1)\\ & =\eta p(z_t/x_t)p(x_t/x_0,z_{1:t-1},u_{1:t})(2)\\ & =\eta p(z_t/x_t)\int p(x_t/x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}/x_0,z_{1:t-1},u_{1:t})d{x}_{t-1}(3)\\ & =\eta p(z_t/x_t)\int p(x_t/x_{t-1},u_t)p(x_{t-1}/x_0,z_{1:t-1},u_{1:t})d{x}_{t-1}(4)\\ & =\eta p(z_t/x_t)\int p(x_t/x_{t-1},u_t)p(x_{t-1}/x_0,z_{1:t-1},u_{1:t-1})d{x}_{t-1}(5)\\ & =\eta p(z_t/x_t)\int p(x_t/x_{t-1},u_t)Bel(x_{t-1})d{x}_{t-1}\end{array}$$
(1)式到(2)式，分母与x无关，是一个常数，为了方便表示用$\eta$表示，由马尔科夫假设，在已知$x_t$的情况下，$z_t$与{$z_{1:t-1},u_{1:t}$}无关，所以分子第一项化简为$p(z_t/x_t)$

(2)式到(3)式，当前状态$x_t$是基于之前所有状态估计得到的，至少它会受$x_{t-1}$影响，于是按照$x_{t-1}$时刻为条件概率展开:$p(x_{t-1}/x_0,z_{1:t-1},u_{1:t})=p(x_{t-1}/x_0,z_{1:t-1},u_{1:t-1})=Bel(x_{t-1})$

(3)式到(4)式，由马尔科夫假设，当前时刻状态只和上一个时刻有关，所以$\int p(x_t/x_{t-1},z_{1:t-1},u_{1:t})=\int p(x_t/x_{t-1},u_t)$

(4)式到(5)式，考虑到ｔ时刻的输入量$u_t$与 t−1 时刻的状态无关，所以把$u_t$拿掉，$p(x_{t-1}/x_0,z_{1:t-1},u_{1:t})=p(x_{t-1}/x_0,z_{1:t-1},u_{1:t-1})=Bel(x_{t-1})$

总结：从最终的式子可以看出，SLAM中的贝叶斯框架就是：我们用轮速里程计运动数据$u_t$来预测下一时刻机器人的位姿$x_t$，接着用视觉传感器观测数据$z_t$来对预测的位姿$x_t$进行校正，这样就获得了当前时刻状态的后验。其实就是两个过程，状态预测和状态更新。