基本的贝叶斯滤波算法
Algorithm Bayes_filter(bel(x[t-1], u[t], z[t])):
for all x[t] do:
bel_transition(x[t]) = ∫p(x[t] | u[t], x[t-1]) * bel(x[t-1]) * dx[t-1] # 控制更新
bel(x[t]) = η * p(z[t] | x[t]) * bel_transition(x[t]) # 测量更新，η为归一化常数
return bel(x[t])
实例：利用摄像机来估计门的状态的机器人
假设：门只有开关两种状态，只有机器人能改变门的状态
一、初始置信度
bel(X[0] = open) = 0.5
bel(X[0] = closed) = 0.5
二、测量概率
机器人传感器检测门状态的概率
p(Z[t] = sense_open | X[t] = is_open) = 0.6
p(Z[t] = sense_closed | X[t] = is_open) = 0.4
p(Z[t] = sense_open | X[t] = is_closed) = 0.2
p(Z[t] = sense_closed | X[t] = is_closed) = 0.8
三、状态转移概率
(1)机器人使用操作器把门拉开。如果门已经开了，它就开着；如果门是关着的，机器人有0.8的概率将它打开
p(X[t] = is_open | U[t] = push, X[t-1] = is_open) = 1
p(X[t] = is_closed | U[t] = push, X[t-1] = is_open) = 0
p(X[t] = is_open | U[t] = push, X[t-1] = is_closed) = 0.8
p(X[t] = is_closed | U[t] = push, X[t-1] = is_closed) = 0.2
(2)机器人不使用操作器，世界的状态不会改变
p(X[t] = is_open | U[t] = do_nothing, X[t-1] = is_open) = 1
p(X[t] = is_closed | U[t] = do_nothing, X[t-1] = is_open) = 0
p(X[t] = is_open | U[t] = do_nothing, X[t-1] = is_closed) = 0
p(X[t] = is_closed | U[t] = do_nothing, X[t-1] = is_closed) = 1
四、贝叶斯滤波
（1）机器人没有采取任何动作，检测到门是开着的（U[1] = do_nothing, Z[1] = sense_open）
①控制更新
bel_transition(x[1]) = ∫p(x[1] | u[1], x[0]) * bel(x[0]) * dx[0] = ∑p(x[1] | u[1], x[0]) * bel(x[0])
bel_transition(X[1] = is_open) =
p(X[1] = is_open | U[1] = do_nothing, X[0] = is_open) * bel(X[0] = is_open) +
p(X[1] = is_open | U[1] = do_nothing, X[0] = is_closed) * bel(X[0] = is_closed)
= 1*0.5+0*0.5 = 0.5
bel_transition(x[1] = is_closed) =
p(X[1] = is_closed | U[1] = do_nothing, X[0] = is_open) * bel(X[0] = is_open) +
p(X[1] = is_closed | U[1] = do_nothing, X[0] = is_closed) * bel(X[0] = is_closed)
= 0*0.5+1*0.5 = 0.5
②测量更新
bel(x[1]) = η * p(z[1] = sense_open | x[1]) * bel_transition(x[1])
bel(X[1] = is_open) =
η * p(Z[1] = sense_open | X[1] = is_open) * bel_transition(X[1] = is_open)
= η*0.6*0.5 = 0.3η
bel(X[1] = is_closed) =
η * p(Z[1] = sense_open | X[1] = is_closed)) * bel_transition(X[1] = is_closed))
= η*0.2*0.5 = 0.1η
③归一化
bel(X[1] = is_open) = 0.75
bel(X[1] = is_open) = 0.25
（2）机器人使用操作器把门拉开，检测到门是开着的（U[2] = push, Z[2] = sense_open）
①控制更新
bel_transition(x[2]) = ∫p(x[2] | u[2], x[1]) * bel(x[1]) * dx[1] = ∑p(x[2] | u[2], x[1]) * bel(x[1])
bel_transition(X[2] = is_open) =
p(X[2] = is_open | U[2] = push X[1] = is_open) * bel(X[1] = is_open) +
p(X[2] = is_open | U[2] = push, X[1] = is_closed) * bel(X[1] = is_closed)
= 1*0.75+0.8*0.25 = 0.95
bel_transition(X[2] = is_closed) =
p(X[2] = is_closed | U[2] = push, X[1] = is_open) * bel(X[1] = is_open) +
p(X[2] = is_closed | U[2] = push, X[1] = is_closed) * bel(X[1] = is_closed)
= 0*0.75+0.2*0.25 = 0.05
②测量更新
bel(x[2]) = η * p(z[2] = sense_open | x[2]) * bel_transition(x[2])
bel(X[2] = is_open) =
η * p(Z[2] = sense_open | X[2] = is_open) * bel_transition(X[2] = is_open)
= η*0.6*0.95 = 0.57η
bel(X[2] = is_closed) =
η * p(Z[2] = sense_open | X[2] = is_closed)) * bel_transition(X[2] = is_closed))
= η*0.2*0.05 = 0.01η
③归一化
bel(X[1] = is_open) ≈ 0.983
bel(X[1] = is_open) ≈ 0.017
