转自我的博客http://gongzheng92.net
贝叶斯滤波器的推导
模型与方程
首先我们从贝叶斯滤波器谈起。
首先我们需要的是对机器人目前状态(states)的估计,用概率的方式表达为:
p(xt│x0:t−1,z1:t−1,u1:t)
上式可以在诸多假设条件(马尔科夫假设,观测不对环境造成影响等)下化为:
p(xt│xt−1,ut)(1)
同样的我们有测量模型:
p(zt│x0:t,z1:t−1,u1:t)=p(zt│xt)(2)
然而,由于 状态(states)(x)的 不可直接观测,于是我们提出了 置信度(belief)的概念。于是我们希望得到的状态(1)估计便可转化为:
bel(xt)=p(xt|z1:t,u1:t)(3)
注意这里并没有应用马尔科夫假设。
然而这里对 xt 的估计是先完成测量,再进行估计,因此(3)还有一种形式,便是
先完成估计,再进行测量
,即
bel¯¯¯¯(xt)=p(xt│z1:t−1,u1:t)(4)
于是我们就有了贝叶斯滤波器:
Byes_filter(bel(xt−1),ut,zt)for all xt dobel¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxbel(xt)=ηp(zt|xt)bel¯¯¯¯(xt)endforreturn bel(xt)(5)
其实在这里, p(xt|ut,xt−1) 就是系统模型, p(zt│xt) 就是测量模型。
推导过程
那么这个(5)是怎么来的呢?我们从(3)推起:
(p(xt│z1:x,u1:t)=p(zt│xt,z1:t−1,u1:t)p(xt│z1:t−1,u1:t)p(zt│z1:t−1,u1:t)=ηp(zt│xt,z1:t−1,u(1:t))p(xt│z1:t−1,u1:t))(6)
其中因为 p(zt│z1:t−1,u1:t) 与我们感兴趣的 xt 没有关系,因此可范化为 η 。
p(x,y)=p(x│y)p(y)=p(x)p(y) 记住联合概率(,)的运算级别高于条件概率(|)。
然后在这里推一下书上没推的公式:
Bayes rule: p(x│y)=p(y│x)p(x)p(y)
p(x│y,z)=p(y,z│x)p(x)p(y,z)=p(y,z,x)p(y│z)p(z)=p(y│x,z)p(x,z)p(y│z)p(z)=p(y│x,z)p(x)p(y|z)
然后在
p(zt│xt,z1:t−1,u1:t)
之中,由于我们假设测量
zt
只于当前状态有关,与之前的状态与历史控制量均无关系,因此我们有:
p(zt|xt,z1:t−1,u1:t)=p(zt│xt)
于是有:
p(xt│z1:t,u1:t)=ηp(zt│xt)p(xt|z1:t−1,u1:t)(7)
即
bel(xt)=ηp(zt│xt)bel¯¯¯¯(xt)
但其实这句话很废话,因为就是“测量前的估计乘以测量等于测量后的估计”。
然后:
bel¯¯¯¯(xt)=p(xt│z1:t−1,u1:t)=∫p(xt│xt−1,z1:t−1,u1:t)p(xt−1│z1:t−1,u1:t)dxt−1(8)
然后由于状态转移假设(其中包括了马尔科夫假设),则
p(xt│xt−1,z1:t−1,u1:t)=p(xt|xt−1,ut)
再加之忽略红色 p(xt−1│z1:t−1,u1:t) 部分的 ut (因为是未来的控制量),于是我们有
bel¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dx