Ch6 线性离散系统卡尔曼滤波.ppt

单击此处编辑母版标题样式,单击此处编辑母版文本样式,第二级,*,*,*,最优估计,1,第,6,章 线性离散系统卡尔曼滤波,线性离散系统卡尔曼滤波器的推导,带有控制项和测量系统偏差时的卡尔曼滤波器,系统干扰和测量噪声相关时的卡尔曼滤波器,有色噪声下的卡尔曼滤波器,卡尔曼滤波器稳定性和鲁棒性,线性离散系统的最优预测与平滑,2,6.1,引 言,1960,年卡尔曼（,Kalman,）将状态空间分析方法引入滤波理论，得到时域上的递推滤波算法，即卡尔曼滤波。,(1),要求随机过程是平稳的，要求太强；,(2),以单入单出系统推导出的，难以推广到高维的情况；,(3),维纳,-,霍夫方程很难求解，在工程上难以实现；,(4),是批处理方法，不能满足在线快速处理大量数据的需要。,维纳滤波的不足：,与维纳滤波相比，卡尔曼滤波不仅适用于非平稳随机过程，而且可以递推实现。,系统和量测的数学模型,噪声及初值统计特性,状态和估计方差初值,量测数据,系统状态的最优估值,Kalman,Filter:,3,噪声和初值的统计特性,：,离散模型,：,已知：,要求：,零均值，方差阵为对角阵,过程噪声和观测噪声不相关,初值和过程噪声、观测噪声均不相关,离散卡尔曼滤波器公式推导,4,预测,/,时间更新,(,利用系统方程对状态及观测做一步预测,),：,校正,/,测量更新,(,利用新观测对预测状态进行校正,),：,KF,公式,2,：观测预测,KF,公式,3,：状态估计,待定校正增益阵,如何求,？,KF,公式,1,：状态预测,6.2,直观推导法,推导基本步骤：预测，校正。,5,估计误差：,估计误差方差：,6,（,2,）将,J,k,对,K,k,的偏导数为零，即,KF,公式,5,：增益矩阵,估计误差方差矩阵，与,KF,公式,4,等价,KF,公式,4,：估计误差方差矩阵,为了求得,K,k,，（,1,）选择代价函数：,7,KF,公式,6,：预测误差方差矩阵,预测误差：,预测误差方差：,8,卡尔曼滤波的实质,递推估计的过程；,预测,+,校正的过程。,时间更新,z,K,K=k+1,测量更新,初始状态和方差,9,状态预测,方差预测,增益矩阵,状态估值,方差估值,预测,/,时间更新：,校正,/,观测更新：,新息序列,离散卡尔曼滤波器推算方程,10,离散卡尔曼滤波公式公式汇总,11,卡尔曼滤波器结构图,延时,一步,+,-,+,+,当前估计值,上一步估计值,一步预测,12,1.,滤波递推实现，必须给定初值（状态和方差）；,几点说明：,2.,当前状态最优估值：,4.KF,是反馈校正过程，其校正项：，比例因子：,5.,增益：,6.,预测方差：,初始估值可由经验给定，方差需经测量，由统计方法给出；,若滤波是稳定的，滤波将不依赖于初值。,3.,估计方差 的作用：计算增益，但给出了误差分析。,13,最小二乘估计和卡尔曼滤波估计都是无偏估计，都能在得到系统最优状态估计值的同时，还得到估计误差的方差；均适用于状态为向量的情况。,维纳滤波和卡尔曼滤波都是最小方差估计。维纳滤波要求系统的输入是平稳过程，最小二乘和卡尔曼滤波无此要求。,最小二乘估计法可行的条件是只需要系统的测量方程和测量误差方差的信息，有递推算法，最优解是状态值；,维纳滤波需要知道系统的输入和噪声的功率谱密度，以及输入和噪声的互谱密度，属于批处理方法，最优解是滤波器的传递函数或是冲激响应，对状态为向量的情况不容易实现；,卡尔曼滤波可行的条件是不仅需要系统的测量方程，还需要系统的状态方程以及系统干扰和测量误差的均值和方差信息，递推方法，可以实时实现，最优解是系统的状态。,卡尔曼滤波,&,最小二乘,&,维纳滤波,14,卡尔曼滤波的直观解释,卡尔曼滤波的基本思想与舰船组合导航人员作业中对船位推算的逻辑思维方法是吻合的。基本思想都可以归结为,“,预测,+,修正,”,。,卡尔曼滤波过程是：每隔一个滤波周期，通过量测传感器得到量测船位,C,，同时经过状态转移得到预测船位,B,，在量测船位和预测船位之间根据增益,K,k,进行折衷，从而获得最佳估计船位,D,，依此过程不断循环下去。,15,例,6.2,已知离散型线性系统的状态方程和观测方程如下,16,17,应用实例,1,舰船导航,状态向量：,状态方程：,观测方程：,1.,系统模型,18,2.,初始条件,19,3.,仿真,仿真结果,(1),20,仿真结果,(2),21,仿真结果,(3),22,6.3,带有控制项和测量偏差的卡尔曼滤波器,滤波方程：,23,6.4,系统噪声和测量噪声相关时的卡尔曼滤波,系统模型：,相关噪声：,其他条件与,6.2,中相同。,条件改变，不符合,Kalman,基本方程，前面的结果不能直接套用，需要对方程作形式上的修改，化为符合条件的形式，再利用之前结果。,方法：,引入类似于,Lagrange,乘子的待定常数矩阵，适当选取这个矩阵，变非约束方称为约束方程，将约束条件纳入方程，变成符合基本假设条件的形式。,已知,在动态方程右端增加一个恒为零的项：,24,待定矩阵,注意到模型中只有系统方程不同，所以与前相比，只有预测方程不同。,25,一步预测：,一步预测误差：,预测误差方差：,26,噪声,w,k,和,v,k,相关时的卡尔曼滤波递推方程：,27,6.5,有色噪声下的卡尔曼滤波器,若有色噪声可以用白噪声激发的线性系统得到，可通过,增广状态向量,的方法，将噪声作为状态向量的一部分进行估计。,系统模型：,w,1,和,v,为有色噪声，分别对应一个,成型滤波器：,白噪声只是一种理想噪声，实际的噪声总有一定的相关性，只有在相关性较弱时，才可以近似看作白噪声序列。,当噪声的相关性不可忽略时，要考虑,有色噪声,下的卡尔曼滤波。,非白噪声,白噪声,28,白色 有色,成型滤波器,（,forming filter,、,shaping filter,）：,有色噪声 白噪声通过线性系统的输出，,白化,。,29,n+p+m,维增广状态向量：,增广状态方程：,-(6.5.7),w,(,k,),为维的零均值高斯白噪声序列，其协方差矩阵为,：,30,测量方程：,-(6.5.8),(6.5.7),和,(6.5.8),构成的增广状态系统的卡尔曼滤波方程,为,：,增广状态方程：,-(6.5.7),31,两点说明：,（,1,）对状态向量扩维，会增加滤波的运算量；,（,2,）在计算增益矩阵 时，求逆矩阵中可能会因为 过小而产生奇异。为避免这种情况，可在有色中分离出一个白噪声分量 ，其中 对任意,k,都正定。测量方程变为：,32,6.6,卡尔曼滤波器的稳定性和鲁棒性,卡尔曼滤波是递推算法，必需有初值。,若初值选取不当，是否会对滤波产生影响？,卡尔曼滤波要求已知系统模型。,若模型不准，对滤波的影响有多大？,滤波的稳定性问题,初值选取对滤波的影响,滤波的鲁棒性问题,模型误差对滤波的影响,33,6.6.1,卡尔曼滤波的稳定性,1.,滤波的稳定性问题及稳定性定理,知识回顾：三个稳定性概念（稳定、渐进稳定、一致渐进稳定）,三种稳定性之间的关系：,一致渐近稳定性,渐近稳定性,稳定性,34,-(6.6.3),滤波方程：,随机线性离散系统：,-(6.6.2),利用李雅普诺夫第二法则，,有结论：若,-(6.6.5),离散系统的情况,35,相减,取范数,36,连续系统的情况,连续系统滤波方程：,-(6.6.9),-(6.6.10),(6.6.10),的状态转移矩阵,相减,取范数并利用,Schwarz,不等式,37,38,判定一个最优滤波系统是否一致渐近稳定，只需考察这个系统本身是否一致完全能控和一致完全能观，如果满足，则滤波初值和可以任意选取。,滤波方程由系统方程和观测方程推出，滤波的稳定性应该与随机线性系统的结构和参数有关。,卡尔曼等的结论：,如果随机线性系统是一致完全能控和一致完全能观的，那么其线性最优滤波系统是一致渐进稳定的。,（这实际上就是稳定性定理）,39,随机线性系统的能观性：,随机线性系统的能控性：,40,滤波的稳定性定理,41,结论,1,：,对于一致完全能控和一致完全能观的线性系统，当时间充分长以后，其卡尔曼最优滤波值将渐近地不依赖于滤波初值的选取，而且有界的测量输入必有有界的滤波输出。,结论,2,：,当时间充分长以后，它的滤波误差方差阵及滤波增益阵将渐近地不依赖于初始方差阵的选取。,结论,3,：,42,线性定常系统的能控能观性,由上面两式，可以推出线性定常系统一致完全能控与一致完全能观的充要条件为：,43,例：,系统的状态方程和观测方程如下,判定卡尔曼滤波的稳定性。,解：,由系统方程和观测方程知,44,45,系统一致完全能控！,46,系统一致完全能观！,结论：滤波是稳定的。,47,2.,稳态卡尔曼滤波,当滤波达到稳态后，滤波误差方差阵将趋向于一个确定的正定阵，并且可以离线计算，从而避免了大量的在线计算，易于工程实现。,48,它决定着增益矩阵：,黎卡提差分方程,稳态卡尔曼滤波增益阵和滤波误差方差阵：,稳态卡尔曼滤波方程：,49,对卡尔曼滤波的稳定性进行研究的前提是：精确已知系统的数学模型和噪声统计特性。但实际则不然。,由系统建模误差或者系统参数变化而使卡尔曼滤波器保持其稳定的性能称为卡尔曼滤波器的鲁棒性。,为研究卡尔曼滤波的鲁棒性，要研究系统模型存在误差时得到的状态估计值与真实值的误差。,6.6.2,卡尔曼滤波的鲁棒性,50,1.,卡尔曼滤波器模型误差分析,真实模型：,含误差模型：,-(6.6.39),-(6.6.40),51,由含误差的模型得到的滤波递推方程：,52,偏差：,偏差的方差：,53,预测偏差：,预测偏差方差：,递推初值：,54,55,几个结论：,结论,1,：,结论,2,：,结论,3,：,对于一致完全能控能观系统，其方差有上界。,56,1.,卡尔曼滤波的鲁棒性,当系统中的,参数,在一定范围内发生,摄动,或模型化系统具有,建模误差,，卡尔曼滤波器的滤波值仍能保持,不发散,，即,则称该系统的卡尔曼滤波器具有,鲁棒性,。,一般模型：含控制项,-(6.6.56),-(6.6.57),引理,若系统,(6.6.56),和系统,(6.6.57),为一致完全能控和一致完全能观，则有如下结论：,57,证：,58,（,2,）的证明与（,1,）类似。,根据此引理，有如下,5,个结论：,59,结论,1,：,证：,2,范数,矩阵特征值,注意,:,取方差，控制项消失。,60,由系统的渐进稳定性，即,(6.6.59),此外，,61,引理,(1),即：系统的卡尔曼滤波具有鲁棒性。,62,结论,2,：,线性定常系统,具有参数摄动的模型为,若两个系统完全能控能观，,则系统具有抗参数摄动鲁棒性。,结论,3,：,若两个系统完全能控能观，,63,结论,5,：,结论,4,：,若两个系统完全能控能观，,64,6.7,线性离散系统的最优预测与平滑,6.7.1,线性离散系统的最优预测,-(6.7.1),65,由系统模型方程：,-,最优预测方程,以下推导预测误差的协方差矩阵。,66,预测误差：,预测误差方差：,-,最优预测,方差,67,不带求和号的形式：,68,最优预测：,最优预测误差方差：,69,6.7.2,线性离散系统的最优平滑,根据,k,和,j,的具体变化情况，最优平滑分为如下三类：,固定点平滑，固定区间平滑，固定滞后平滑。,1.,固定点平滑,应用：通过观测人造卫星轨道数据来估计其进入轨道时的初始状态。,70,2.,固定区间平滑,应用：常用在惯性导航系统中。,3.,固定滞后平滑,应用：多用于通信系统中。,71,固定点平滑方程的推导,推导方法：将固定点状态作为扩充状态，对基本方程进行扩充，对扩充后状态向量应用原来的滤波方程，最后再将扩充的状态从方程中分离出来。,72,扩充后的状态和观测方程：,一步,预测,：,73,增广误差方差：,74,增益矩阵：,将一步预测、估计误差方差阵和增益矩阵进行分解，分离出固定点平滑方程。,75,应用实例,舰船横摇运动姿态估计,状态方程：,舰船在海上航行时的横摇运动方程,为,：,离散化,平稳高斯随机过程,（,1,）次优估计,76,用角速度陀螺来测量船的横摇运动，其随机测量误差,v,(t,),主要是由于陀螺的随机漂移所致，可视为具有指数相关函数的平稳高斯随机过程，即,式中：,离散系统方程最终表示,为,：,卡尔曼滤波器1：,77,仿真结果,(1),78,仿真结果,(2),从仿真结果可以看出，估计误差较大。,主要原因是海浪干扰和测量噪声均为有色噪声。,79,（,2,）次优估计,将测量噪声白化。,80,仿真条件：有义波高,3.8m,，航速,18,节，浪向角,=90,81,仿真结果,(1),82,仿真结果,(2),对照前一组仿真结果，发现估计效果有提高。,83,（,3,）最优估计,将过程噪声和测量噪声均作白化处理。,扩维后的状态方程：,扩维后的测量方程：,84,仿真条件：有义波高,3.8m,，航速,18,节，浪向角,=90,85,仿真结果,(1),86,仿真结果,(2),与前一组仿真差别不大，原因是测量噪声的有色成分影响大。,87,线性定常系统模型,:,（,4,）姿态预报,88,其向前,l,步的预报估计方程为,:,仿真条件：有义波高,3.8m,，航速,18,节，浪向角,=90,89,仿真结果,(1),90,仿真结果,(2),预测的效果远不如估计的好。,91,对次优估计、最优估计及预报研究的结论,当测量噪声为有色噪声时，如不进行白化处理，则卡尔曼滤波器是不能有效工作的，主要表现为估计误差很大，甚至使滤波发散。,应用卡尔曼次优滤波器,2,进行横摇运动姿态估计的数学模型简单，在线计算量小，可达到比较满意的精度。进一步，可以采用高斯,马尔可夫序列等效海浪干扰的方法对其进行白化处理和状态扩充，从而采用卡尔曼滤波器,3,的最优估计效果优于卡尔曼滤波器,2,，但这是以增加在线计算量为代价的。实际应用时可视具体情况折衷考虑，再选择设计滤波器。,采用卡尔曼滤波器对舰船横摇运动姿态进行预报的结果可供工程实际应用。,本例是以舰船的横摇运动姿态估计为例进行研究的，但其分析问题的思路与研究问题的方法可供研究舰船其它运动姿态以及考虑六个自由度运动之间耦合影响的多变量的估计和预报问题中参考。,92,