卡尔曼滤波器原理详解实用教案

1、1234内容内容(nirng)提纲提纲发展(fzhn)概述算法(sun f)前提算法推导算法总结卡尔曼滤波5算法扩展第1页/共25页第一页，共26页。Rudolf (Rudy) Emil Klmn（1930 ）卡尔曼滤波工程背景：1960s航空航天工程突飞猛进电子计算机又方兴未艾正式提出：一种关于线性过滤(gul)和预测难题的新方法1960A new approach to linear filtering and prediction problems ， 19601发展(fzhn)概述第2页/共25页第二页，共26页。卡尔曼滤波1发展(fzhn)概述优势：1、采用物理意义较为直观的时域状态

2、空间2、仅需要前后两步的数据，数据存储量较小3、使用(shyng)比较简单的递推算法，便于在计算机上实现4、不仅适用于平稳过程，还可以推广到非平稳随机过程的情况发展：卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。第3页/共25页第三页，共26页。卡尔曼滤波2算法(sun f)前提随机(su j)离散系统模型定义随机(su j)离散时间过程的状态向量 ，该过程由以下离散随机(su j)差分方程描述：111kkkkwBuAxxkkkvHxzmkRz nkRx 1 . 2假设(jish)系统满足可观性要求，定义观测向量 ，得到观测方程：2 . 2随机信号 和 分别表示过程激励噪

3、声和观测噪声，并假设它们是相互独立并满足正态分布的白噪声。kvkwnx1 nxn nxn nx1 nx1mx1 mxn mx1第4页/共25页第四页，共26页。卡尔曼滤波2算法(sun f)前提即有：过程激励噪声观测噪声实际过程中过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能随着每次迭代(di di)计算而变化，但在这里我们假设它们为常数。另外，状态转移矩阵A，输入矩阵B和测量方程2.2中的矩阵H，在实际过程中都可能随时间变化而变化，但在这儿假设为常数。3 . 24 . 2), 0(RNvk), 0(QNwk第5页/共25页第五页，共26页。卡尔曼滤波2算法(sun f)前提先验估计和后验

4、估计，及其误差定义 为根据上一次迭代(di di)计算结果而产生的估计值，称为先验估计。定义 为根据当前计算结果而产生的估计值，称为后验估计。定义先验估计误差为定义后验估计误差为nkRx nkRx kkkxxenx1 真值5 . 2kkkxxenx1 真值6 . 2第6页/共25页第六页，共26页。卡尔曼滤波2算法(sun f)前提定义(dngy)先验估计误差的协方差为定义(dngy) 后验估计误差的协方差为)cov(,kkkkeeeEPT)cov(,EkkkkeeePT7 . 28 . 2第7页/共25页第七页，共26页。卡尔曼滤波目的描述：在系统结构已知的情况下，给定k时刻的状态观测向量

5、，求k时刻的系统状态向量的最优估计 ，使得(sh de) 最小。基本思路：1、根据 计算出k时刻的先验估计 同时也产生了先验估计的协方差矩阵 的递推公式3算法(sun f)推导kzkx kP1kxkx 11kkkBuxAx1 . 3kPQAPAPTkk12 . 3第8页/共25页第八页，共26页。卡尔曼滤波2、根据先验(xin yn)估计 计算出k时刻的观测向量的估计3、计算实测值 与估计 的差，以此来修正之前的先验(xin yn)估计 ，得到后验估计把 带入：3算法(sun f)推导)(kkkkkxHzKxx3 . 3kx kkxHz kzkz kx kx 权重(qun zhn) 残差这里的

6、权重系数也成为卡尔曼增益。至此卡尔曼滤波计算原型公式基本结束，下面要确定出最优系数K和在最优系数下的后验估计误差的协方差矩阵的递推方法)(kkkkkzzKxxkz 第9页/共25页第九页，共26页。卡尔曼滤波推导后验协方差矩阵(j zhn)按照定义，我们从误差协方差 开始推导如下：带入 再带入3算法(sun f)推导kP)cov(kkkxxPkP)(cov(kkkkkkxHzKxxP)(kkkkkxHzKxxkkkvHxz)(cov(kkkkkkkxHvHxKxxP第10页/共25页第十页，共26页。卡尔曼滤波整理测量误差向量，得：因为噪声(zoshng)项与其他项不相关，协方差=0，所以有：

7、利用协方差矩阵性质，提出常数矩阵，得：3算法(sun f)推导)(cov(kkkkkkvKxxHKIP)cov()(cov(kkkkkkvKxxHKIPTkkkTkkkkkKvKHKIxxHKIP)cov()(cov()(第11页/共25页第十一页，共26页。卡尔曼滤波如果(rgu)记 ，则有：3算法(sun f)推导kkkPxx)cov(TkkTkkkkRKKHKIPHKIP)()(4 . 3第12页/共25页第十二页，共26页。卡尔曼滤波推导最优卡尔曼增益(zngy)最优化K：使后验估计 的协方差 达到最小。（换一个概念）也是使向量的二范数的数学期望值最小化的一个过程。这等同于后验估计的协

8、方差矩阵的迹最小化3算法(sun f)推导kKkx kPkkxx|2kkxxEkkkPtrxxP|2第13页/共25页第十三页，共26页。卡尔曼滤波首先展开3.4式，得：记：上面的式子(sh zi)可以写为：3算法(sun f)推导TkTkkTkTkkkkkKRHPHKKHPPHKPP)(RHPHSTkkTkkkTkTkkkkkKSKKHPPHKPP5 . 3第14页/共25页第十四页，共26页。卡尔曼滤波把 对 求导，并令导数=0，则可以(ky)得到 取最小值时的最优化 的值。解得：3算法(sun f)推导)(kPtrTTCBdABACdtr)(引入常用数学公式？kK)(kPtrkK022)

9、()()()(kkTkkkTTkkTkTkkkSKHPSKKSHPPHdKPdtr11)(RHPHHPSHPKTkTkkTkk6 . 3第15页/共25页第十五页，共26页。卡尔曼滤波化简后验误差协方差公式在卡尔曼增益等于上面导出的最优值时，计算后验协方差的公式可以进行简化。对于卡尔曼增益公式在卡尔曼增益公式两侧(lin c)同时右乘得：把上式带入3.5式，可以消去后面的两项，得：3算法(sun f)推导1kTkkSHPKTkkKSTkTkTkkkKHPKSKkkkkPHKPP第16页/共25页第十六页，共26页。3算法(sun f)推导整理，得：这个公式的计算比较简单，所以(suy)实际中总

10、是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用3.5式表示的后验误差协方差公式。kkkPHKIP)(7 . 3第17页/共25页第十七页，共26页。卡尔曼滤波卡尔曼增益的物理(wl)意义其中：H矩阵为常量； 与过程激励噪声的协方差矩阵Q有关；R为测量噪声的协方差矩阵。取值范围：当 R 趋向于零时，有：当 趋向于零时，有：3算法(sun f)推导0lim0kPKkkPRHPHHPKTkTkkkP10lim HKkR, 01HKk第18页/共25页第十八页，共26页。

11、卡尔曼滤波意义：决定了最优估计组成(z chn)比例的“调节器”当 R 趋向于零时，有：测量噪声 V=0此时3.3式改为系统表现为完全取测量值作为状态的后验估计值，而系统的先验状态估计完全被抛弃。反之当 趋向于零时，根据式3.2可知，Q=0易知，此时系统完全抛弃测量值，取先验估计值3算法(sun f)推导kP10lim HKkRmx1kkkkkzHxHzHxx11)(nx1第19页/共25页第十九页，共26页。卡尔曼滤波算法描述卡尔曼滤波器用反馈控制的方法估计过程状态。估计过程某一时刻的状态，然后以（含噪声）测量值得方式获得(hud)反馈，因此卡尔曼滤波器可以分为两个部分：1、时间更新方程负责

12、向前推算当前状态向量和误差协方差估计的值，为下一个时间状态构成先验估计。2、测量更新方程负责将先验估计和新的测量变量结合已构成改进后的后验估计。4算法(sun f)总结第20页/共25页第二十页，共26页。卡尔曼滤波时间(shjin)更新方程测量更新方程4算法(sun f)总结11kkkBuxAx1 . 42 . 41)(RHPHHPKTkTkk3 . 44 . 4)(kkkkxHzKxxQAPAPTkk 15 . 4kkkPHKIP)(第21页/共25页第二十一页，共26页。卡尔曼滤波4算法(sun f)总结BAHKkx kx kz1kx1kukz 时间更新（预测） 测量更新（校正）算法框图

13、1、状态估计(gj)向量的运算流程图第22页/共25页第二十二页，共26页。卡尔曼滤波4算法(sun f)总结算法框图2、误差(wch)协方差的运算流程图AATI -KkHkPQ1kP时间更新（预测）测量更新（修正）kP第23页/共25页第二十三页，共26页。卡尔曼滤波滤波器参数调整在卡尔曼滤波器实际实现时，测量噪声R一般可以观测得到，是滤波器的已知条件。观测测量噪声协方差R可以通过离线试验获取。而过程激励噪声协方差Q值比较难以确定，因为我们无法直接观测到过程状态 。有时候可以通过Q的手动选择，人为的为过程“注入”一个足够的不确定因素，来产生(chnshng)一个可以接受的结果。也可以使用一个离线模拟滤波器与在线滤波器进行实时对比的方式来实时优化滤波器的参数（Q和R）。5算法(sun f)扩展kx第24页/共25页第二十四页，共26页。谢谢您的观看(gunkn)！第25页/共25页第二十五页，共26页。NoImage内