Kalman滤波算法解释与实现
转载，原文地址，请支持：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