转载,原文地址,请支持:http://www.cnblogs.com/ycwang16/p/5999034.html
认知计算,还要从贝叶斯滤波的基本思想讲起。这一部分,我们先回顾贝叶斯公式的数学基础,然后再来介绍贝叶斯滤波器。
(一). 概率基础回顾
我们先来回顾一下概率论里的基本知识:
1. X X: 表示一个随机变量,如果它有有限个可能的取值 {x1,x2,⋯,xn} {x1,x2,⋯,xn}.
2. p(X=xi) p(X=xi):表示变量 X X的值为 xi xi的概率。
3. p(⋅) p(⋅):称为概率质量函数(probability mass function).
例如:一个家里有3个房间,机器人在各个房间的概率为 p(room)={0.1,0.3,0.6} p(room)={0.1,0.3,0.6}.
4. 如果 X X在连续空间取值, p(x) p(x)称为概率密度函数(probability density function),
图1. 概率密度函数曲线示例
5. 联合概率: p(X=x and Y=y)=p(x,y) p(X=x and Y=y)=p(x,y),称为联合概率密度分布。如果 X X和 Y Y是相互独立的随机变量, p(x,y)=p(x)p(y) p(x,y)=p(x)p(y)。
6. 条件概率: p(X=x|Y=y) p(X=x|Y=y) 是在已知 Y=y Y=y的条件下,计算 X=x X=x的概率。
如果 x x和 y y相互独立,则:
7. 全概率公式:
离散情况下:
连续情况下:
(二). 贝叶斯公式
2.1 贝叶斯公式
基于条件概率公式和全概率公式,我们可以导出贝叶斯公式:
- 这里面 x x一般是某种状态; y y一般是代表某种观测。
- 我们称 P(y|x) P(y|x)为causal knowledge,意即由 x x的已知情况,就可以推算 y y发生的概率,例如在图2的例子中,已知如果门开着,则 z=0.5m z=0.5m的概率为0.6;如果门关着,则 z=0.5m z=0.5m的的概率为0.3。
- 我们称 P(x) P(x)为prior knowledge,是对 x x的概率的先验知识。例如在图2的例子中,可设门开或关的概率各占 50% 50%.
- P(x|y) P(x|y)是基于观测对状态的诊断或推断。贝叶斯公式的本质就是利用causal knowledge和prior knowledge来进行状态推断或推理。
例1::
在图2所示的例子中,机器人根据观测的到门的距离,估算门开或关的概率,若测量到门的距离为 z=0.5m z=0.5m,则可用条件概率描述门开着的概率:
图 2.机器人根据观测计算门开或关的概率
2.2 贝叶斯公式的计算
可以看到贝叶斯公式的分母项 P(y) P(y),同 P(x|y) P(x|y)无关,所以可以把它作为归一化系数看待:
所以基于causal knowledge和prior knowledge进行条件概率计算的过程如下:
Algorithm:
∀x:auxx|y=P(y|x)P(x)η=1∑xauxx|y∀x:P(x|y)=ηauxx|y ∀x:auxx|y=P(y|x)P(x)η=1∑xauxx|y∀x:P(x|y)=ηauxx|y
2.3 贝叶斯公式中融合多种观测
在很多应用问题中,我们会用多种观测信息对一个状态进行猜测和推理,贝叶斯公式中是如何融合多种观测的呢?
我们简单推导一下:
所以有:
2.4 贝叶斯递推公式
由此,我们来推导贝叶斯滤波的递推公式:
P(x|z1,…,zn)=? P(x|z1,…,zn)=?
我们把 zn zn看做 y y,把 z1,…,zn−1 z1,…,zn−1看做 z z,代入上面的公式:
再由Markov属性,在 x x已知的情况下, zn zn同 {z1,…,zn–1} {z1,…,zn–1}无关,所以:
从而我们得到贝叶斯的递推公式:
例2:在例1的基础上,如果机器人第二次测量到门的距离仍然为0.5米, 计算门开着的概率。
P(open|z2,z1)=P(z2|open)P(open|z1)P(z2|open)P(open|z1)+P(z2|¬open)P(¬open|z1)=0.6⋅230.6⋅23+0.3⋅13=0.40.5=0.8 P(open|z2,z1)=P(z2|open)P(open|z1)P(z2|open)P(open|z1)+P(z2|¬open)P(¬open|z1)=0.6⋅230.6⋅23+0.3⋅13=0.40.5=0.8
所以,第二次z=0.5m的观测增大了对门开着的概率的置信程度。
(三). 如何融入动作?
在实际问题中,对象总是处在一个动态变化的环境中,例如:
- 机器人自身的动作影响了环境状态
- 其它对象,比如人的动作影响了环境状态
- 或者就是简单的环境状态随着时间发生了变化。
如何在Bayes模型中来描述动作的影响呢?
- 首先,动作所带来的影响也总是具有不确定性的
- 其次,相比于观测,动作一般会使得对象的状态更为模糊(或更不确定)。
我们用 u u来描述动作,在 x′ x′状态下,执行了动作 u u之后,对象状态改变为 x x的概率表述为:
动作对状态的影响一般由状态转移模型来描述。如图3所示,表示了“关门”这个动作对状态影响的转移模型。这个状态转移模型表示:关门这个动作有0.1的失败概率,所以当门是open状态时,执行“关门”动作,门有0.9的概率转为closed状态,有0.1的概率保持在open状态。门是closed的状态下,执行“关门”动作,门仍然是关着的。
图3. “关门”动作的状态转移模型
执行某一动作后,计算动作后的状态概率,需要考虑动作之前的各种状态情况,把所有情况用全概率公式计算:
- 连续情况下:
- 离散情况下:
例3:在例2的基础上,如果按照图3所示的状态转移关系,机器人执行了一次关门动作, 计算动作后门开着的概率?
所以,执行一次关门动作后,门开着的概率变为了0.08.
(四). 贝叶斯滤波算法
4.1 算法设定
由上述推导和示例,我们可以给出贝叶斯滤波的算法,算法的输入输出设定如下。
- 系统输入
- 1到 t t时刻的状态观测和动作: dt={u1,z1…,ut,zt} dt={u1,z1…,ut,zt}
- 观测模型: P(z|x) P(z|x)
- 动作的状态转移模型: P(x|u,x′) P(x|u,x′)
- 系统状态的先验概率分布 P(x) P(x).
- 期望输出
- 计算状态的后延概率,称为状态的置信概率: Bel(xt)=P(xt|u1,z1…,ut,zt) Bel(xt)=P(xt|u1,z1…,ut,zt)
4.2 算法基本假设
贝叶斯滤波的基本假设:
1. Markov性假设: t t时刻的状态由 t−1 t−1时刻的状态和 t t时刻的动作决定。 t t时刻的观测仅同 t t时刻的状态相关,如图4所示:
图4. Markov模型
p(zt|x0:t,z1:t,u1:t)=p(zt|xt)
p(zt|x0:t,z1:t,u1:t)=p(zt|xt)
p(xt|x1:t−1,z1:t,u1:t)=p(xt|xt−1,ut)
p(xt|x1:t−1,z1:t,u1:t)=p(xt|xt−1,ut)
2. 静态环境,即对象周边的环境假设是不变的
3. 观测噪声、模型噪声等是相互独立的
4.3 Bayes滤波算法
基于上述设定和假设,我们给出贝叶斯滤波算法的推导过程:
Bel(xt)=P(xt|u1,z1…,ut,zt) Bel(xt)=P(xt|u1,z1…,ut,zt)
=ηP(zt|xt,u1,z1,…,ut)P(xt|u1,z1,…,ut) <—Bayes =ηP(zt|xt,u1,z1,…,ut)P(xt|u1,z1,…,ut) <—Bayes
=ηP(zt|xt)P(xt|u1,z1,…,ut) <—Markov =ηP(zt|xt)P(xt|u1,z1,…,ut) <—Markov
=ηP(zt|xt)∫P(xt|u1,z1,…,ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1) <—TotalProb. =ηP(zt|xt)∫P(xt|u1,z1,…,ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1) <—TotalProb.
=ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1)<—Markov =ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,ut)dxt−1)<—Markov
=ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,zt−1)dxt−1)<—Markov =ηP(zt|xt)∫P(xt|ut,xt−1)P(xt−1|u1,z1,…,zt−1)dxt−1)<—Markov
=ηP(zt|xt)∫P(xt|ut,xt−1)Bel(xt−1)dxt−1 =ηP(zt|xt)∫P(xt|ut,xt−1)Bel(xt−1)dxt−1
其中第一步采用贝叶斯公式展开,第二步使用Markov性质( zt zt仅由 xt xt决定);第三步使用全概率公式对 xt−1 xt−1进行展开;第四步继续使用Markov性质( xt xt仅由 xt−1 xt−1和 ut ut决定);第五步继续使用Markov性质,因为 xt−1 xt−1同 ut ut无关,最终得到 Bel(xt) Bel(xt)的递推公式。
可见递推公式中分为两个步骤, ∫P(xt|ut,xt−1)Bel(xt−1)dxt−1 ∫P(xt|ut,xt−1)Bel(xt−1)dxt−1部分是基于 xt−1,ut xt−1,ut预测 xt xt的状态; ηP(zt|xt) ηP(zt|xt)部分是基于观测 zt zt更新状态 xt xt.
4.3 Bayes滤波算法流程
所以,Bayes滤波的算法流程图如图5所示。如果 d d是观测,则进行一次状态更新,如果 d d是动作,则进行一次状态预测。
图5. Bayes滤波的算法流程
我们看到,在进行状态预测时,需要对所有可能的 x′ x′状态进行遍历,使得基本的Bayes模型在计算上成本是较高的。
4.3 Bayes滤波算法的应用
Bayes滤波方法是很多实用算法的基础,例如:
- Kalman滤波
- 扩展Kalman滤波
- 信息滤波
- 粒子滤波
虽然Kalman滤波器已经被广泛使用,也有很多的教程,但我们在Bayes滤波器的框架上,来深入理解Kalman滤波器的设计,对理解采用Gaussian模型来近似状态分布的多高斯滤波器(Guassian Multi-Hyperthesis-Filter)等都有帮助。
一. 背景知识回顾
1.1 Bayes滤波
首先回顾一下Bayes滤波. Bayes滤波分为两步:1.状态预测;和 2.状态更新
1. 状态预测,基于状态转移模型:
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1 bel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1
2. 状态更新,基于新的观测
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt) bel(xt)=ηp(zt|xt)bel¯(xt)
我们可以看到,我们的目的是计算 xt xt的后验概率,如果 bel(xt) bel(xt)是任意分布,我们需要在 xt xt的所有可能取值点上,计算该取值的概率,这在计算上是难于实现的。这一计算问题可以有多种方法来近似,比如利用采样的方法,就是后面要讲的粒子滤波和无迹Kalman滤波。
这节要说的近似方法是,当假设 bel(xt) bel(xt)服从Gauss分布,那么我们只需要分布的均值和方差就可以完全描述 bel(xt) bel(xt),而无需在 xt xt的每个可能取值点上进行概率计算。这也是用高斯分布来近似 bel(xt) bel(xt)的好处,因为我们在每一个时刻,只需要计算均值 μt μt和方差 Σt Σt这两个数值,就可以对 bel(xt) bel(xt)完全描述,所以我们就可以推导出这两个数值的递推公式,从而在每个时刻由这两个数值的递推公式完全获得状态估计,这就是The Kalman Filter的基本思想。
1.2 正态分布(Guassian Distribution)
然后我们再回顾一下正态分布的基础知识。正态分布是一种特殊的概率分布,分布的形态完全由二阶矩决定。一元高斯分布表述如下:
其中,一阶矩为均值 μ μ表示期望值,二阶矩为方差 σ σ表示分布的不确定程度。
多元高斯分布的表达式为:
同样,一阶矩为 μ μ表示各元变量的期望值,二阶矩为方差矩阵 Σ Σ表示各元变量的不确定程度。
1.3 正态分布的特点
在线性变换下,一旦高斯,代代高斯。
首先,高斯变量线性变换后,仍为高斯分布,均值和方差如下:
X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT) X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)
然后,两个高斯变量线性组合,仍为高斯分布,均值和方差如下:
X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ21+σ22+2ρσ1σ2) X1∼N(μ1,σ12)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ12+σ22+2ρσ1σ2)
最后,两个相互独立的高斯变量的乘积,仍然为高斯分布,均值和方差如下:
X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ21+σ22μ1+σ21σ21+σ22μ2,σ21σ22σ21+σ22) X1∼N(μ1,σ12)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ12+σ22μ1+σ12σ12+σ22μ2,σ12σ22σ12+σ22)
正因为高斯分布有这些特点,所以,在Bayes滤波公式中的随机变量的加法、乘法,可以用解析的公式计算均值和方差,这使得Bayes滤波的整个计算过程非常简便,即Kalman滤波器的迭代过程。
二. Kalman滤波
2.1 Kalman滤波的模型假设
Kalman滤波所解决的问题,是对一个动态变化的系统的状态跟踪的问题,基本的模型假设包括:1)系统的状态方程是线性的;2)观测方程是线性的;3)过程噪声符合零均值高斯分布;4)观测噪声符合零均值高斯分布;从而,一直在线性变化的空间中操作高斯分布,状态的概率密度符合高斯分布。
- 状态方程
xt=Atxt−1+Btut+εt xt=Atxt−1+Btut+εt - 观测方程
zt=Htxt+δt zt=Htxt+δt
其中过程噪声 εt εt假设符合零均值高斯分布;观测噪声 δt δt假设符合零均值高斯分布。对于上述模型,我们可以用如下参数描述整个问题:
2.2 Kalman滤波器的模型
- xt xt, n n维向量,表示 t t时刻观测状态的均值。
- Pt Pt, n∗n n∗n方差矩阵,表示 t t时刻被观测的 n n个状态的方差。
- ut ut, l l维向量,表示 t t时刻的输入
- zt zt, m m维向量,表示 t t时刻的观测
- At At, n∗n n∗n矩阵,表示状态从 t−1 t−1到 t t在没有输入影响时转移方式
- Bt Bt, n∗n n∗n矩阵,表示 ut ut如何影响 xt xt
- Ht Ht, m∗n m∗n矩阵,表示状态 xt xt如何被转换为观测 zt zt
- Rt Rt, n∗n n∗n矩阵,表示过程噪声 εt εt的方差矩阵
- Qt Qt, m∗m m∗m矩阵,表示观测噪声 δt δt的方差矩阵
图1. 在没有观测情况下,系统状态的从 t−1 t−1到 t t的转移方式
图1给出了在没有观测,仅有输入 ut ut时,状态变量的均值和方差从 t−1 t−1到 t t的转移方式,可见均值和方差的计算,完全是基于高斯分布的线性变化的方法来算的。
图2. Kalman 滤波解决在收到t时刻的输入 ut ut和观测 zt zt的情况下,更新状态 xt xt的问题
图2给出了Kalman滤波所解决的问题,即在获得t时刻的输入和观测的情况下,如何更新 xt xt的均值和方差的问题。当然 ut ut和 zt zt也并不是每一个时刻都需要同时获得,就像贝叶斯滤波一样,可以在获得 ut ut时就做一次状态预测,在获得 zt zt时做一次状态更新。
2.3 Kalman滤波算法
Kalman滤波整体算法如下:
Kalman Filter ( xt−1,Pt−1,ut,zt xt−1,Pt−1,ut,zt)
|
- 第一行基于转移矩阵和控制输入,预测 t t时刻的状态
- 第二行是预测方差矩阵
- 第三行计算Kalman增益,Kt
- 第四行基于观测的新息进行状态更新
- 第五行计算更新状态的方差矩阵。
可以看到算法的所有的精妙之处都在于第三行和第四行。我们可以这样来理解:
- (HtP¯¯¯¯tHTt+Qt) (HtP¯tHtT+Qt)代表对状态进行观测时,观测的不确定程度,它与Kalman增益Kt成反比,表示观测的可能噪声越大的时候,Kalman增益Kt越小。
- 再看第四行,
xt
xt的更新是在
x¯¯¯t
x¯t上加一个
Kt
Kt 乘以
(zt−Htx¯¯¯t)
(zt−Htx¯t)。
(zt−Htx¯¯¯t)
(zt−Htx¯t)代表的是预测的值与观测之间的差异,这个差异当预测和观测都比较接近于真实值时比较小。当观测不准,或者预测不准时都会比较大。而前面的乘子Kt是在观测噪声大的时候比较小,所以整个
Kt(zt−Htx¯¯¯t)
Kt(zt−Htx¯t)这个修正量,表示利用观测对预测结果的修正量。
- 当观测噪声比较小,预测误差比较大时修正幅度比较大
- 当观测噪声比较小预测误差比较小的时候,或者观测噪声比较大的时候,修正误差的幅度也比较小,从而起到了一种平滑的作用。
- 利用较准确的观测修正预测误差,不准确的观测修正量也较小,所以在误差较大的时候能快速修正,而在误差较小时能逐渐收敛。
2.4 Kalman滤波算法的推导
这里我们用Bayes公式,给出Kalman Filter是如何导出的。
1. 系统的初始状态是:
bel(x0)=N(μ0,P0) bel(x0)=N(μ0,P0)
2. 预测过程的推导
状态转移模型是线性函数
xt=Atxt−1+Btut+εt xt=Atxt−1+Btut+εt
所以,由 xt−1 xt−1到 xt xt状态转移的条件概率为:
p(xt|ut,xt−1)=N(Atxt−1+Btut,Rt) p(xt|ut,xt−1)=N(Atxt−1+Btut,Rt)
回顾Bayes公式,计算预测状态的分布,需要考虑所有可能的 xt−1 xt−1:
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1 bel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1
这正是计算两个高斯分布的卷积的过程,参考文献[2]:
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1⇓⇓∼N(Atμt−1+Btut,Rt)∼N(μt−1,Pt−1)⇓bel¯¯¯¯¯¯(xt)=η∫exp{−12(xt−Atxt−1−Btut)TR−1t(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TP−1t−1(xt−1−μt−1)}dxt−1bel¯¯¯¯¯¯(xt)={μ¯t=Atμt−1+BtutP¯¯¯¯t=AtPt−1ATt+Rt bel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1⇓⇓∼N(Atμt−1+Btut,Rt)∼N(μt−1,Pt−1)⇓bel¯(xt)=η∫exp{−12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TPt−1−1(xt−1−μt−1)}dxt−1bel¯(xt)={μ¯t=Atμt−1+BtutP¯t=AtPt−1AtT+Rt
所以Kalman滤波器的预测过程,正是基于两个高斯分布的卷积计算得到的解析表达式。
3. 观测更新过程的推导
观测方程也是线性方程,并且噪声是高斯噪声
zt=Htxt+δt zt=Htxt+δt
所以 p(zt|xt) p(zt|xt)的条件概率是高斯分布的线性变换计算:
p(zt|xt)=N(Htxt,Qt) p(zt|xt)=N(Htxt,Qt)
再考虑贝叶斯公式的状态更新步骤
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt) bel(xt)=ηp(zt|xt)bel¯(xt)
这正是两个高斯分布的乘积的问题,参考文献[2]
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)⇓⇓∼N(zt;Htxt,Qt)∼N(xt;μ¯¯¯t,P¯¯¯¯t)⇓bel(xt)=ηexp{−12(zt−Htxt)TQ−1t(zt−Htxt)}exp{−12(xt−μ¯t)TP¯−1t(xt−μ¯t)} bel(xt)=ηp(zt|xt)bel¯(xt)⇓⇓∼N(zt;Htxt,Qt)∼N(xt;μ¯t,P¯t)⇓bel(xt)=ηexp{−12(zt−Htxt)TQt−1(zt−Htxt)}exp{−12(xt−μ¯t)TP¯t−1(xt−μ¯t)}
所以,基于求高斯变量乘积的分布的方法,可以导出结果仍然是高斯分布,用它的二阶矩表示:
bel(xt)={μt=μ¯t+Kt(zt−Htμ¯t)Pt=(I−KtHt)P¯¯¯¯twithKt=P¯¯¯¯tHTt(HtP¯¯¯¯tHTt+Qt)−1 bel(xt)={μt=μ¯t+Kt(zt−Htμ¯t)Pt=(I−KtHt)P¯twithKt=P¯tHtT(HtP¯tHtT+Qt)−1
所以状态更新中的Kalman增益,均值和方差的更新公式,都是这样导出的。
2.5 Kalman滤波算法的举例
图3和图4通过一维高斯分布的例子,给出在预测和更新过程中状态变量的概率密度分布是如何变化的。
图3. 预测过程的举例,蓝色曲线表示 xt−1 xt−1的pdf,紫色曲线表示 x¯¯¯t x¯t的pdf.
图4. 更新过程举例,紫色为预测后的pdf, 黄色为更新后的pdf,青色为观测的结果
从这个例子中可以值得注意的是,在预测部分高斯分布的卷积一般会使状态估计的方差加大;在观测部分高斯分布的乘积一般会将估计的方差收窄。
2.6 Kalman滤波的代码实现
Kalman滤波算法可以非常方便的用矩阵计算方法实现,其迭代更新过程的Matlab实现的代码仅有如下几行:
2.7 Kalman滤波的效果示例
通过实现一个简单的Kalman滤波器,我们可以直观的看一下Kalman滤波器的提高跟踪准确性的效果。
图5. Kalman滤波器的实验效果示例,其中红色实线是真值;蓝色点是观测;绿色线是滑动平均的结果;紫色曲线是Kalman滤波的结果。
图6. 比较Kalman滤波的跟踪结果和滑动平均的跟踪结果
图6给出了直观的跟踪结果与真实值之间的最小二乘误差的比较,课件Kalman滤波算法相比滑动平均等,提供了更高的跟踪准确性。
2.8 Kalman滤波的算法特点
- Kalman滤波计算快速,计算复杂度为 O(m2.376+n2) O(m2.376+n2),其中 m m是观测的维数; n n是状态的个数。
- 对于线性系统,零均值高斯噪声的系统,Kalman是理论上无偏的,最优滤波器。
- Kalman滤波在实际使用中,要注意参数 R R和 Q Q的调节,这两者实际上是相对的,表示更相信观测还是更相信预测。具体使用时, R R可以根据过程噪声的幅度决定,然后 Q Q可以相对 R R来给定。当更相信观测时,把 Q Q调小,不相信观测时,把 Q Q调大。
- Q Q越大,表示越不相信观测,这是系统状态越容易收敛,对观测的变化响应越慢。 Q Q越小,表示越相信观测,这时对观测的变化响应快,但是越不容易收敛。
参考文献
[1]. Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, 2002, The MIT Press.
[2]. P.A. Bromiley, Products and Convolutions of Gaussian Probability Density Functions, University of Manchester