贝叶斯滤波算法（实例）

基本的贝叶斯滤波算法

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