卡尔曼滤波器原理详解

单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,第三级,第四级,第五级,*,单击此处编辑母版标题样式,卡尔曼滤波器原理,林斌,P2011090,26,1,2,3,4,内容提纲,发展概述,算法前提,算法推导,算法总结,卡尔曼滤波,5,算法扩展,Rudolf(Rudy)Emil Klmn（1930）,卡尔曼滤波,工程背景：,1960s,航空航天工程突飞猛进电子计算机又方兴未艾,正式提出：,一种关于线性过滤和预测难题的新方法1960A new approach to linear filtering and prediction problems ，1960,1,发展概述,卡尔曼滤波,1,发展概述,优势：,1,、,采用物理意义较为直观的,时域,状态空间,2、,仅需要,前后两步,的数据,，数据存储量较小,3、,使用比较简单的递推算法，便于在计算机上实现,4、,不仅适用于平稳过程，还可以推广到非平稳随机过程的情况,发展：,卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。,卡尔曼滤波,2,算法前提,随机,离散系统模型,定义随机离散时间过程的,状态向量,，该过程由以下离散随机差分方程描述：,假设系统满足可观性要求，定义,观测向量,，得到观测方程：,随机信号 和 分别表示,过程激励噪声,和,观测噪声,，并假设它们是相互独立并满足正态分布的白噪声。,nx1 nxn nxn nx1 nx1,mx1 mxn mx1,卡尔曼滤波,2,算法前提,即有：,过程激励噪声,观测噪声,实际过程中过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能随着每次迭代计算而变化，但在这里我们假设它们为常数。,另外，状态转移矩阵A，输入矩阵B和测量方程2.2中的矩阵H，在实际过程中都可能随时间变化而变化，但在这儿假设为常数。,卡尔曼滤波,2,算法前提,先验估计和后验估计，及其误差,定义 为根据上一次迭代计算结果而产生的估计值，称为,先验估计,。,定义 为根据当前计算结果而产生的估计值，称为,后验估计,。,定义,先验估计误差,为,定义,后验估计误差,为,nx1 真值,nx1 真值,卡尔曼滤波,2,算法前提,定义,先验估计误差的协方差,为,定义,后验估计误差的协方差,为,卡尔曼滤波,目的描述：,在系统结构已知的情况下，给定k时刻的状态观测向量,，求k时刻的系统状态向量的最优估计 ，使得,最小。,基本思路：,1、根据 计算出k时刻的先验估计,同时也产生了先验估计的协方差矩阵 的递推公式,3,算法推导,卡尔曼滤波,2、根据先验估计 计算出k时刻的观测向量的估计,3、计算实测值 与估计 的差，以此来修正之,前的先验估计 ，得到后验估计,把,带入：,3,算法推导,权重 残差,这里的权重系数也成为卡尔曼增益。,至此卡尔曼滤波计算原型公式基本结束，下面要确定出最优系数K和在最优系数下的后验估计误差的协方差矩阵的递推方法,卡尔曼滤波,推导后验协方差矩阵,按照定义，我们从误差协方差 开始推导如下：,带入,再带入,3,算法推导,卡尔曼滤波,整理测量误差向量，得：,因为噪声项与其他项不相关，协方差=0，所以有：,利用协方差矩阵性质，提出常数矩阵，得：,3,算法推导,卡尔曼滤波,如果记 ，则有：,3,算法推导,卡尔曼滤波,推导最,优卡尔曼增益,最优化K：使后验估计 的协方差 达到最小。,（换一个概念）也是使向量的二范数的,数学期望值最小化的一个过程。,这等同于后验估计的协方差矩阵的迹,最小化,3,算法推导,卡尔曼滤波,首先展开3.4式，得：,记：,上面的式子可以写为：,3,算法推导,卡尔曼滤波,把 对 求导，并令导数=0，则可以得到,取最小值时的最优化 的值。,解得：,3,算法推导,引入常用数学公式？,卡尔曼滤波,化简后验误差协方差公式,在卡尔曼增益等于上面导出的最优值时，计算后验协方差的公式可以进行简化。,对于卡尔曼增益公式,在卡尔曼增益公式两侧同时右乘,得：,把上式带入3.5式，可以消去后面的两项，得：,3,算法推导,3,算法推导,整理，得：,这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用3.5式表示的后验误差协方差公式。,卡尔曼滤波,卡尔曼增益的物理意义,其中：H矩阵为常量；与,过程激励噪声的协方差矩阵Q有关；R为测量噪声的协方差矩阵。,取值范围：,当 R 趋向于零时，有：,当,趋向于零时，有：,3,算法推导,卡尔曼滤波,意义：决定了最优估计组成比例的“调节器”,当 R 趋向于零时，有：,测量噪声 V=0,此时3.,3式改,为,系统表现为完全取测量值作为状态的后验估计值，而系统的先验状态估计完全被抛弃。,反之当,趋向于零时，根据式3.2可知，Q=0,易知，此时系统完全抛弃测量值，取先验估计值,3,算法推导,mx1,nx1,卡尔曼滤波,算法描述,卡尔曼滤波器用反馈控制的方法估计过程状态。估计过程某一时刻的状态，然后以（含噪声）测量值得方式获得反馈，因此卡尔曼滤波器可以分为两个部分：,1、时间更新方程,负责向前推算当前状态向量和误差协方差估计的值，为下一个时间状态构成先验估计。,2、测量更新方程,负责将先验估计和新的测量变量结合已构成改进后的后验估计。,4,算法总结,卡尔曼滤波,时间更新方程,测量更新方程,4,算法总结,卡尔曼滤波,4,算法总结,B,A,H,K,时间更新（预测）,测量更新（校正）,算法框图,1、状态估计向量的运算流程图,卡尔曼滤波,4,算法总结,算法框图,2、误差协方差的运算流程图,A,A,T,I-K,k,H,时间更新（预测）,测量更新（修正）,卡尔曼滤波,滤波器参数调整,在卡尔曼滤波器实际实现时，测量噪声R一般可以观测得到，是滤波器的已知条件。观测测量噪声协方差R可以通过离线试验获取。,而过程激励噪声协方差Q值比较难以确定，因为我们无法直接观测到过程状态 。有时候可以通过Q的手动选择，人为的为过程“注入”一个足够的不确定因素，来产生一个可以接受的结果。,也可以使用一个离线模拟滤波器与在线滤波器进行实时对比的方式来实时优化滤波器的参数（Q和R）。,5,算法扩展,