Kalman滤波算法解释与实现
来源:互联网 发布:免费家长控制软件 编辑:程序博客网 时间:2024/06/14 22:07
转载,原文地址,请支持:http://www.cnblogs.com/ycwang16/p/5999034.html
认知计算,还要从贝叶斯滤波的基本思想讲起。这一部分,我们先回顾贝叶斯公式的数学基础,然后再来介绍贝叶斯滤波器。
(一). 概率基础回顾
我们先来回顾一下概率论里的基本知识:
1.
2.
3.
例如:一个家里有3个房间,机器人在各个房间的概率为
4. 如果
图1. 概率密度函数曲线示例
5. 联合概率:
6. 条件概率:
如果
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所示的例子中,机器人根据观测的到门的距离,估算门开或关的概率,若测量到门的距离为
图 2.机器人根据观测计算门开或关的概率
2.2 贝叶斯公式的计算
可以看到贝叶斯公式的分母项
所以基于causal knowledge和prior knowledge进行条件概率计算的过程如下:
Algorithm:
2.3 贝叶斯公式中融合多种观测
在很多应用问题中,我们会用多种观测信息对一个状态进行猜测和推理,贝叶斯公式中是如何融合多种观测的呢?
我们简单推导一下:
所以有:
2.4 贝叶斯递推公式
由此,我们来推导贝叶斯滤波的递推公式:
我们把
再由Markov属性,在
从而我们得到贝叶斯的递推公式:
例2:在例1的基础上,如果机器人第二次测量到门的距离仍然为0.5米, 计算门开着的概率。
所以,第二次z=0.5m的观测增大了对门开着的概率的置信程度。
(三). 如何融入动作?
在实际问题中,对象总是处在一个动态变化的环境中,例如:
- 机器人自身的动作影响了环境状态
- 其它对象,比如人的动作影响了环境状态
- 或者就是简单的环境状态随着时间发生了变化。
如何在Bayes模型中来描述动作的影响呢?
- 首先,动作所带来的影响也总是具有不确定性的
- 其次,相比于观测,动作一般会使得对象的状态更为模糊(或更不确定)。
我们用
动作对状态的影响一般由状态转移模型来描述。如图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).
- 1到
- 期望输出
- 计算状态的后延概率,称为状态的置信概率:
Bel(xt)=P(xt|u1,z1…,ut,zt) Bel(xt)=P(xt|u1,z1…,ut,zt)
- 计算状态的后延概率,称为状态的置信概率:
4.2 算法基本假设
贝叶斯滤波的基本假设:
1. Markov性假设:
图4. Markov模型
2. 静态环境,即对象周边的环境假设是不变的
3. 观测噪声、模型噪声等是相互独立的
4.3 Bayes滤波算法
基于上述设定和假设,我们给出贝叶斯滤波算法的推导过程:
其中第一步采用贝叶斯公式展开,第二步使用Markov性质(
可见递推公式中分为两个步骤,
4.3 Bayes滤波算法流程
所以,Bayes滤波的算法流程图如图5所示。如果
图5. Bayes滤波的算法流程
我们看到,在进行状态预测时,需要对所有可能的
4.3 Bayes滤波算法的应用
Bayes滤波方法是很多实用算法的基础,例如:
- Kalman滤波
- 扩展Kalman滤波
- 信息滤波
- 粒子滤波
虽然Kalman滤波器已经被广泛使用,也有很多的教程,但我们在Bayes滤波器的框架上,来深入理解Kalman滤波器的设计,对理解采用Gaussian模型来近似状态分布的多高斯滤波器(Guassian Multi-Hyperthesis-Filter)等都有帮助。
一. 背景知识回顾
1.1 Bayes滤波
首先回顾一下Bayes滤波. Bayes滤波分为两步:1.状态预测;和 2.状态更新
1. 状态预测,基于状态转移模型:
2. 状态更新,基于新的观测
我们可以看到,我们的目的是计算
这节要说的近似方法是,当假设
1.2 正态分布(Guassian Distribution)
然后我们再回顾一下正态分布的基础知识。正态分布是一种特殊的概率分布,分布的形态完全由二阶矩决定。一元高斯分布表述如下:
其中,一阶矩为均值
多元高斯分布的表达式为:
同样,一阶矩为
1.3 正态分布的特点
在线性变换下,一旦高斯,代代高斯。
首先,高斯变量线性变换后,仍为高斯分布,均值和方差如下:
然后,两个高斯变量线性组合,仍为高斯分布,均值和方差如下:
最后,两个相互独立的高斯变量的乘积,仍然为高斯分布,均值和方差如下:
正因为高斯分布有这些特点,所以,在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
其中过程噪声
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 xtHt Ht,m∗n m∗n矩阵,表示状态xt xt如何被转换为观测zt ztRt Rt,n∗n n∗n矩阵,表示过程噪声εt εt的方差矩阵Qt Qt,m∗m m∗m矩阵,表示观测噪声δt δt的方差矩阵
图1. 在没有观测情况下,系统状态的从
图1给出了在没有观测,仅有输入
图2. Kalman 滤波解决在收到t时刻的输入
图2给出了Kalman滤波所解决的问题,即在获得t时刻的输入和观测的情况下,如何更新
2.3 Kalman滤波算法
Kalman滤波整体算法如下:
Kalman Filter (
- Prediction
x¯¯¯t=Atxt−1+Btut x¯t=Atxt−1+BtutP¯¯¯¯t=AtPt−1ATt+Rt P¯t=AtPt−1AtT+Rt
- Correction
Kt=P¯¯¯¯tHTt(HtP¯¯¯¯tHTt+Qt)−1 Kt=P¯tHtT(HtP¯tHtT+Qt)−1xt=x¯¯¯t+Kt(zt−Htx¯¯¯t) xt=x¯t+Kt(zt−Htx¯t)Pt=(I−KtHt)P¯¯¯¯t Pt=(I−KtHt)P¯t
- 第一行基于转移矩阵和控制输入,预测
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. 系统的初始状态是:
2. 预测过程的推导
状态转移模型是线性函数
所以,由
回顾Bayes公式,计算预测状态的分布,需要考虑所有可能的
这正是计算两个高斯分布的卷积的过程,参考文献[2]:
所以Kalman滤波器的预测过程,正是基于两个高斯分布的卷积计算得到的解析表达式。
3. 观测更新过程的推导
观测方程也是线性方程,并且噪声是高斯噪声
所以
再考虑贝叶斯公式的状态更新步骤
这正是两个高斯分布的乘积的问题,参考文献[2]
所以,基于求高斯变量乘积的分布的方法,可以导出结果仍然是高斯分布,用它的二阶矩表示:
所以状态更新中的Kalman增益,均值和方差的更新公式,都是这样导出的。
2.5 Kalman滤波算法的举例
图3和图4通过一维高斯分布的例子,给出在预测和更新过程中状态变量的概率密度分布是如何变化的。
图3. 预测过程的举例,蓝色曲线表示
图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
- Kalman滤波算法解释与实现
- Kalman滤波算法解释与实现
- Kalman 滤波算法导论
- kalman滤波的通俗解释
- [记录]初学kalman滤波算法
- 参悟Kalman滤波之初步理解与实现
- matlab 实现的kalman滤波
- Kalman滤波实现目标跟踪
- Kalman滤波算法和Condensation算法
- 数字图像算法(一)--Kalman滤波(上)
- 数字图像算法(一)--Kalman滤波(下)
- Kalman滤波算法原理(Matlab/C/C++)
- Kalman滤波
- Kalman滤波
- Kalman滤波
- Kalman滤波
- kalman滤波
- Kalman 滤波
- Android中图片加载框架Glide的使用
- PHP 错误 和 日志 函数
- POJ 2739 Sum of Consecutive Prime Numbers
- python 合并拼接字符串
- 时间序列
- Kalman滤波算法解释与实现
- java中Statement详细用法。
- linux上电自动运行运用程序
- laravel5.1 子查询(Query_Builder)
- 剑指Offer(第二版)面试题7:重建二叉树
- 事务
- Java中的类和对象
- Unity如何在VS中做代码格式化
- 343. Integer Break