一、环境背景
在网格地图中,我们的移动机器人 Orazio 要探索地图。
-
红色移动机器人Orazio
-
蓝色是四个保险杠(bumpers???我以为不知道为什么??)来感知环境
-
黑色格子是墙,不能走
-
白色格子是可以走的路
-
在每个时间点可以给 Orazio 一个向上下左右走的命令
二、原理
1. 动态系统
进一步,transition函数 f 和observation函数 h 详细写成下图:
由初始状态分布
x
0
x_{0}
x0 和
u
0
u_{0}
u0 , transation model
p
(
x
t
∣
x
t
−
1
,
u
t
−
1
)
p(x_{t}|x_{t-1}, u_{t-1})
p(xt∣xt−1,ut−1) 根据先前的状态和控制
来计算当前状态
x
t
x_{t}
xt 的概率。
由此延伸出 贝叶斯动态网络(DBN):
- 随机动态过程的图形表示
- 具有周期性结构的特征
在DBN中,通常我们知道:
- 观测值 z 1 : T z_{1:T} z1:T,因为可以被观测到
- 控制 u 0 : T − 1 u_{0:T-1} u0:T−1,因为控制命令就是人为发布的
以下是一些经典的DBN推断:
2. 原理公式
对于 filtering 公式,经过各种推导(此处省略。。太长了。。) 最后得:
p ( X t ∣ u 0 : t − 1 , z 1 : t ) = η t p ( z t ∣ X t ) ∑ X t − 1 p ( X t ∣ X t − 1 , u t − 1 ) p ( X t − 1 ∣ u 0 : t − 2 , z 1 : t − 1 ) p(X_{t}|u_{0:t-1}, z_{1:t})=\eta_{t}p(z_{t}|X_{t})\sum_{X_{t-1}}p(X_{t}|X_{t-1}, u_{t-1})p(X_{t-1}|u_{0:t-2}, z_{1:t-1}) p(Xt∣u0:t−1,z1:t)=ηtp(zt∣Xt)Xt−1∑p(Xt∣Xt−1,ut−1)p(Xt−1∣u0:t−2,z1:t−1)
其中 η \eta η 是常数:
η = 1 p ( z t ∣ u 0 : t − 1 , z 1 : t − 1 ) \eta = \frac{1}{p(z_{t}| u_{0:t-1}, z_{1:t-1})} η=p(zt∣u0:t−1,z1:t−1)1
最后再简写替代一下:
b ( X t ) = p ( X t ∣ u 0 : t − 1 , z 1 : t ) b(X_{t}) = p(X_{t}|u_{0:t-1}, z_{1:t}) b(Xt)=p(Xt∣u0:t−1,z1:t)
则最终公式为:
b ( X t ) = η t p ( z t ∣ X t ) ∑ X t − 1 p ( X t ∣ X t − 1 , u t − 1 ) b ( X t − 1 ) b(X_{t})=\eta_{t}p(z_{t}|X_{t})\sum_{X_{t-1}}p(X_{t}|X_{t-1}, u_{t-1})b(X_{t-1}) b(Xt)=ηtp(zt∣Xt)Xt−1∑p(Xt∣Xt−1,ut−1)b(Xt−1)
三、代码
公式推导完毕,这是实现代码