2 概率机器人 Probabilistic Robotics 贝叶斯滤波算法

0 数学基础知识

• 随机变量$X$的值为$x_t$: $p(X=x_t)$ 简写为$p(x_t)$
• 测量值$z$：
  $$z_{t_1:t_2}=z_{t_1},z_{t_1+1},z_{t_1+2},\cdots ,z_{t_2}$$
• 控制值$u$：
  $$u_{t_1:t_2}=u_{t_1},u_{t_1+1},u_{t_1+2},\cdots ,u_{t_2}$$
• 条件概率：
  $$\begin{gathered} p(x_t|z_t)=\frac{p(z_t|x_t)p(x_t)}{p(z_t)} \\ \downarrow \text{在条件中加入}{z}_{1:t-1},u_{1:t},\text{等式依然成立} \\ p(x_t|z_t,z_{1:t-1},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})} \end{gathered}$$
• 观测值$z_t$与之前观测值$z_{1:t-1}$和控制量$u_{1:t}$无关(马尔可夫性)
  $$p(z_t|x_t,z_{1:t-1},u_{1:t})=p(z_t|x_t)$$
• 状态$x_t$仅与前一时刻状态$x_{t-1}$和当前时刻控制量$u_t$有关
  $$p(x_t|x_{t-1},z_{1:t-1},u_{1:t})=p(x_t|x_{t-1},u_t)\text{（状态转移概率）}$$
• 条件概率中，如果有一个参数是$t-1$时刻的，那么$t$时刻的任意参数都可以忽略,因为将来的参数（$t$时刻）不可能对现在的参数（$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})$$

1 贝叶斯滤波算法的公式推导

• 贝叶斯滤波算法是已知$bel(x_{t-1},u_t,z_t)$求$bel(x_t)$
  $$\begin{gathered} \text{预测：}\overline{bel}(x_t)=p(x_t|z_{1:t-1},u_{1:t}) \\ \text{修正：}bel(x_t)=p(x_t|z_{1:t},u_{1:t}) \end{gathered}$$

• 公式推导 1

• p ( x t ) = ∫ p ( x t ∣ x t − 1 ) p ( x t − 1 ) d x t − 1 ↓ b e l ˉ ( x t ) = p ( x t ∣ z 1 : t − 1 , u 1 : t ) = ∫ p ( x t ∣ x t − 1