一、贝叶斯滤波算法
贝叶斯滤波(Bayes filter)算法是用来计算置信度的常用算法。该算法的步骤是根据测量数据和控制数据计算置信度分布bel()。
这里肯定有些同学不明白置信度的含义,别急,往下看。
置信度是概率机器人的一个重要概念。机器人无法直接测量在环境中的真实的状态。比如,一个机器人在全局坐标系中的位姿为
x
t
=
[
15
10
45
°
]
T
x_{t}=[15\quad10\quad45°]{^T}
xt=[151045°]T ,但是这个位姿机器人并不能够直接测量,而是需要的可以获得的数据中推测其位姿。
我们一般通过条件概率分布来表示置信度。对于离散情况,每一个可能的假设都会分配一个概率值,对于连续的情况,就为概率密度曲线。
这里用
b
e
l
(
x
t
)
bel (x_{t})
bel(xt)表示状态变量
x
t
x_{t}
xt的置信度,则后验概率为:
b
e
l
(
x
z
−
1
)
=
p
(
x
t
∣
z
1
:
t
,
u
1
:
t
)
bel(x_{z-1}) = p(x_{t} | z_{1:t},u_{1:t})
bel(xz−1)=p(xt∣z1:t,u1:t)
这个后验是在测量数据
z
1
:
t
z_{1:t}
z1:t和控制数据
u
1
:
t
u_{1:t}
u1:t的条件下,时刻t下状态
x
t
x_t
xt的概率分布。
可以得知后验概率是考虑了t时刻下的测量数据
z
t
z_t
zt,在不考虑t时刻测量数据时,该概率表示为:
b
e
l
‾
(
x
t
)
=
p
(
x
t
∣
z
1
:
t
−
1
,
u
1
:
t
)
\overline {bel}(x_{t}) = p(x_{t}|z_{1:t-1},u_{1:t})
bel(xt)=p(xt∣z1:t−1,u1:t)
该概率被称为预测(prediction)。
b
e
l
‾
(
x
t
)
\overline{bel} (x_{t})
bel(xt)是基于以前状态的后验,在未结合t时刻的测量数据
z
1
:
t
z_{1:t}
z1:t的条件下,预测了t时刻的状态。由
b
e
l
‾
(
x
t
)
\overline{bel} (x_{t})
bel(xt)和
z
1
:
t
z_{1:t}
z1:t联合计算
b
e
l
(
x
t
)
bel(x_{t})
bel(xt)称为修正或测量更新。
相信到这里大家都明白置信度的涵义了。接下来介绍贝叶斯滤波算法。
伪代码如下:
1: Bayes_filter(
b
e
l
(
x
t
−
1
)
,
u
t
,
z
t
bel(x_{t-1}),u_{t},z_{t}
bel(xt−1),ut,zt) :
2: for all
x
t
x_{t}
xt do:
3:
b
e
l
‾
(
x
t
)
=
∫
p
(
x
t
∣
u
1
:
t
,
x
1
:
t
−
1
)
d
x
t
−
1
\overline {bel}(x_{t}) =\int p(x_{t}|u_{1:t},x_{1:t-1})dx_{t-1}
bel(xt)=∫p(xt∣u1:t,x1:t−1)dxt−1
4:
b
e
l
(
x
t
)
=
bel(x_{t}) =
bel(xt)=𝛈
p
(
z
t
∣
x
t
)
p(z_{t}|x_{t})
p(zt∣xt)
b
e
l
‾
(
x
t
)
\overline {bel}(x_{t})
bel(xt)
5: return
b
e
l
(
x
t
)
bel(x_{t})
bel(xt)
贝叶斯滤波是递归的。即时刻t的置信度 b e l ( x t ) bel(x_{t}) bel(xt)由时刻t-1的置信度 b e l ( x t − 1 ) bel(x_{t-1}) bel(xt−1)来计算。
。
。
。
有时间再更新~