卡尔曼滤波及其在INSGPS组合导航组合导航中的应用

编号（）字号本科生毕业设计（论文）题目姓名学号班级二一五年六月卡尔曼滤波及其在INS/GPS组合导航中的应用李滢07113036测绘工程113班中国矿业大学本科生毕业论文姓名李滢学号07113036学院环境与测绘学院专业测绘工程设计题目卡尔曼滤波及其在INS/GPS组合导航中的应用指导教师郑南山职称教授2015年6月徐州中国矿业大学毕业论文任务书学院环测学院专业年级测绘工程2011级学生姓名李滢任务下达日期2015年3月23日毕业论文日期2015年3月23日至2015年6月15日毕业论文题目卡尔曼滤波及其在INS/GPS组合导航中的应用毕业论文专题题目卡尔曼滤波及其在INS/GPS组合导航中的应用毕业论文主要内容和要求主要内容通过学习GPS/INS组合导航数据处理的滤波理论方法，研究KALMAN滤波及其变化形式CKF等滤波性能，并进行分析比较。论文要求参照学校本科毕业设计的模板要求进行撰写。要求有目的、意义、基本理论、研究方法、仿真实验分析、结论与展望等、专业文献翻译、英文摘要。参考文献要有一定数量，不少于15篇，最好有英文参考资料。文章语句通顺、图表清晰。院长签字指导教师签字中国矿业大学毕业论文指导教师评阅书指导教师评语（基础理论及基本技能的掌握；独立解决实际问题的能力；研究内容的理论依据和技术方法；取得的主要成果及创新点；工作态度及工作量；总体评价及建议成绩；存在问题；是否同意答辩等）成绩指导教师签字年月日中国矿业大学毕业论文评阅教师评阅书评阅教师评语（选题的意义；基础理论及基本技能的掌握；综合运用所学知识解决实际问题的能力；工作量的大小；取得的主要成果及创新点；写作的规范程度；总体评价及建议成绩；存在问题；是否同意答辩等）成绩评阅教师签字年月日中国矿业大学毕业论文答辩及综合成绩答辩情况提出问题回答问题正确基本正确有一般性错误有原则性错误没有回答答辩委员会评语及建议成绩答辩委员会主任签字年月日学院领导小组综合评定成绩学院领导小组负责人年月日摘要KALMAN滤波是以最小线性方差估计为估计准则的一种实时递推算法。它能根据上一时刻的状态估计和当前的观测值来更新当前的状态估计，不需要存储大量数据，算法简单方便，得到了广泛应用。本文在在总结KALMAN滤波的基本原理以及介绍INS/GPS组合导航理论的基础上，探究KALMAN滤波INS/GPS组合导航方面的应用。论文的主要工作有在介绍线性离散系统的KALMAN滤波方程并对方程作分析的基础上，探究影响常规KALMAN滤波精度的因素。通过在MATLAB环境下对目标常速度模型进行仿真分析,得出影响滤波精度的因素有系统状态方程准确性、测量方程准确性、过程噪声和测量噪声强度和仿真初始条件设置合理性。在概述CKF原理和算法的基础上，选取典型的一维单变量非平稳模型和常见的四维纯方位跟踪模型，采用参数设置不同的两种UKF、CKF和EKF进行仿真分析，比较UKF、EKF和CKF的估计精度。验证了以下结论一般情况下，特别是系统非线性程度比较高时，UKF和CKF的估计精度都高于EKF。UKF和CKF的滤波精度高低取决于系统维数，当系统维数小于2时，UKF滤波精度高于CKF；当维数大于3时，CKF滤波精度大于UKF。在系统状态方程非线性较强的情况下，UKF参数设置对滤波结果影响较大，参数设置不当可能引起滤波精度下降甚至滤波发散。在介绍INS/GPS组合导航基础理论和构建INS/GPS松组合系统模型的基础上，将EKF用于INS/GPS组合导航系统，仿真结果表明EKF的有效性。关键字KALMAN滤波；滤波精度；CKF；UKF；EKF；INS/GPS组合导航ABSTRACTKALMANFILTERISONEOFREALTIMERECURSIVEALGORITHMBASEDONTHETHEORYOFLINEARMINIMUMVARIANCEESTIMATIONITCANUPDATETHECURRENTSTATEESTIMATIONACCORDINGTOPREVIOUSLYSTATEESTIMATIONANDCURRENTOBSERVATION,WHICHNOTNEEDTORESTORELOTSOFDATAANDITHADBEENWIDELYUTILIZEBECAUSEOFITSSIMPLYALGORITHMTHEPAPERSUMMARIZESTHEBASICTHEORYOFKALMANFILTER,ANDINTRODUCETHEINS/GPSINTEGRATEDNAVIGATIONTHEORY,ANDTRYTODISCUSSTHEAPPLICATIONOFKALMANFILTERININS/GPSINTEGRATEDNAVIGATIONAREATHEMAINCONTENTSOFTHESTUDYAREASFOLLOWSINTRODUCINGANDANALYZINGTHEDISCRETELINEARSTOCHASTICKALMANFILTEREQUATION,ANDDISCUSSINGTHEINFLUENCEELEMENTOFCOMMONKALMANFILTERPRECISIONACCORDINGTOTHEEMULATIONOFTARGETCONSTANTSPEEDMODELINMATLAB,WEKNOWTHATTHEFACTOROFINFLUENTFILTERSPRECISIONHAVETHEACCURACYOFSTATEEQUATION,MEASUREMENTEQUATION,ANDTHERATIONALITYOFPROCESSNOISE,MEASUREMENTNOISEANDEMULATIONSINITIALCONDITIONSETTINGTHEPAPERSELECTTHETYPICALONEDEGREESINGLEVARIABLENONSTABLEMODELANDFOURDEGREEPUREAZIMUTHTRACKINGMODEL,USINGDIFFERENTUKF,CKF,EKFMODELTOEMULATIONANDCOMPARETHEMITVERIFYTHECONCLUSIONLIKEFOLLOWSINGENERALCASE,ESPECIALLYINTHEHIGHNONLINEARDEGREE,THEFILTERPRECISIONOFUKFANDCKFWOULDBETTERTHANEKFWHENTHEDEGREEISLESSTHAN2,UKFBETTERTHANCKFWHILETHEDEGREEIS3,CKFBETTERTHANUKFINTHESTUDYOFNONLINEARCONDITION,UKFWASINFLUENCESERIOUSTHEWRONGPARAMETERSETTINGMAYDECLINETHEPRECISIONAFTERINTRODUCEOFINS/GPSCOMBINATIONNAVIGATIONTHEORYANDINS/GPSLOOSECOMBINATIONSYSTEMMEDEL,USINGEKFFORINS/GPSLOOSEDCOMBINATION,THEEMULATIONSHOWSTHEEKFEFFECTIVENESSKEYWORDSKALMANFILTERTHEPRECISIONOFFILTERCKFUKFEKFINS/GPSCOMBINATIONNAVIGATION目录1绪论111KALMAN滤波理论的发展112INS/GPS组合导航系统概述1121组合导航系统2122INS/GPS组合导航系统2123INS/GPS组合导航系统的发展213KALMAN滤波在组合导航中的应用现状314论文主要内容32KALMAN滤波基础理论521KALMAN滤波的理论基础5211估计与滤波5212线性最小方差估计6213正交投影定理6214白噪声过程722KALMAN滤波的基本方程8221随机线性离散系统的KALMAN滤波方程8222滤波基本方程分析9223算例分析1023离散非线性系统滤波12231EKF13232UKF153UKF及CKF的估计精度比较1731CKF1732仿真分析18321单变量非平稳模型18322纯方位跟踪模型204INS/GPS组合导航理论基础2341全球定位系统23411GPS系统简介2342INS基础理论与导航算法25421惯导系统基本原理和现状25422坐标系统25423INS导航算法26424INS误差模型2943INS/GPS组合导航系统的组合模式30431松组合30432紧组合30433超紧组合305INS/GPS组合导航系统模型的建立3151状态变量的选取31511直接法31512间接法3152INS/GPS松组合结构设计方案3253松组合导航模型32531INS/GPS松组合状态方程32532INS/GPS松组合测量方程3354基于EKF的INS/GPS松组合导航仿真336总结与展望3561总结3562展望35参考文献37附录39翻译部分53英文原文53中文翻译62致谢69中国矿业大学2015届本科生毕业论文第1页1绪论11KALMAN滤波理论的发展KALMAN滤波理论的创立是估计理论发展到一定阶段的结果。最早的估计方法是最小二乘法，是高斯KARLGAUSS于1795年为测定行星运动轨道提出的，由于其计算简单至今被广泛使用。1941年，前苏联科学家戈尔莫克洛夫ANKO1MOGOROV初次解决了离散平稳随机序列的预测和外推问题。次年，维纳NWIENER提出了WIENER滤波理论旨在解决火力控制系统的精确跟踪问题。然而，由于WIENER滤波运算复杂、要求数据存储空间大且使用范围仅限于一维平稳随机过程信号滤波，难以推广和应用。考虑到频域设计法限制了维纳滤波器的推广，人们开始尝试在时域内设计最优滤波器。1960年，KALMAN提出了离散线性系统KALMAN滤波。1961年，他和BUCY合作将KALMAN滤波理论连续线性系统系统中，至此KALMAN滤波理论创立。KALMAN滤波与WIENER滤波估计准则相同，都是基于最小方差估计准则，所不同的是前者是一种时域滤波方法，引入状态空间来描述系统，算法采用递推的方法。因此，相比WIENER滤波，首先它所需的数据存储空间小，其次适用范围增大到多维非平稳随机过程。正是由于KALMAN滤波相较其他滤波方法的优越性，一经提出它被立即用于工程实践中，目前仍作为一种重要的最优估计方法被用于很多领域。理论用于实践总会发现各种不可预计的问题，解决这些问题又促进了理论的发展。KALMAN滤波理论在应用于工程实践中就主要遇到了以下问题1最初提出的KALMAN滤波理论只适用于线性系统且要求观测方程也是线性的，然而实际应用中系统几乎是非线性或近似线性的且不能保证观测方程为线性。那么如何将KALMAN滤波理论应用于非线性系统的状态估计中2KALMAN滤波的递推算法基于计算机，由于计算机处理字长有限会导致数值不定，然而随着科学技术的进步人们又对KALMAN滤波的数值稳定性、计算效率等有更高的要求。那么如何提高KALMAN滤波的数值稳定性，如何提高计算效率3标准KALMAN滤波在模型精确，噪声统计特性已知的情况下推导的。然而，对于一个实际系统往往存在模型不确定性或噪声统计特性不完全已知，这些将影响滤波精度甚至引起滤波发散。如何将KALMAN滤波理论应用于这样的情况为了解决以上问题，KALMAN滤波理论有了很大发展。扩展KALMAN滤波（EKF）和近年来发展的以无迹KALMAN滤波UKF为代表的SIGMA点KALMAN滤波很好地适用于广泛存在的非线性随机动态系统。平方根滤波、UD分解滤波、奇异值分解滤波等一系列数值鲁棒的滤波算法被相继提出用于改善KALMAN滤波的数值稳定性，提高计算效率。近些年来，人们将鲁棒控制的思想引入滤波中，形成了鲁棒滤波理论，解决了系统模型不确定或噪声统计特性不完全已知的问题。此外，在信息融合的趋势下，KALMAN滤波在各个领域得到广泛应用后也逐渐成为多传感器信息融合的主要方法。1988年，为了给容错组合导航系统的设计提供理论方法，CARLSON在分散化滤波的基础上提出了联邦滤波理论。12INS/GPS组合导航系统概述中国矿业大学2015届本科生毕业论文第2页121组合导航系统“组合导航系统就是将两种或两种以上导航子系统按某种适当方式组合为一种导航系统，以达到提高系统精度和改善系统可靠性等目的1。”由于不同的独立导航系统都存在各自的优缺点，在现今对导航性能要求越来越高的情况下，单独使用某种导航系统都难以满足导航性能的需求。组合导航系统能融合各个导航子系统的优点，使整个导航系统性能达到最优。122INS/GPS组合导航系统INS/GPS组合导航系统将惯性导航系统与GPS组合，是目前得到广泛应用的一种组合导航系统。惯性导航系统是一种自主式导航系统，它不需要接收和传出信号，而是通过传感器测量载体的加速度和角速度直接进行连续积分就可获得载体位置、速度和姿态。由于其导航定位的原理，惯性导航系统优点在于它的不依赖外界信息、隐蔽性好、抗干扰能力强、短期内导航定位精度较高，最主要的缺点是误差随时间迅速积累增长，难以控制。GPS是目前应用最广泛的全球定位系统，它能提供长时间内相对稳定的导航定位精度且使用时间和空间范围广，使用成本低，其最主要的缺点抗干扰能力差，动态环境中可靠性差。根据INS和GPS导航功能是互补的,以适当的方法将两者组合,2必定可以提高系统的整体导航精度及导航性能。表11INS、GPS导航功能互补INSGPS自主式导航系统，导航信息不受外界干扰，无信号遮挡问题非自主式导航系统，接收机天线可能被遮挡造成信号暂时丢失定位中断短期内导航定位精度较高，误差随时间迅速积累增长无时间积累误差，长时间内导航定位精度相对稳定的首次定位需较长时间的预热和初始对准周期GPS接收设备首次定位无需预热，定位快速高精度的INS的价格很高GPS接收机成本较低可以通过两种基本方法来实现组合导航系统，第一种是经典的重调法，这种方法用GPS调整惯导的输出，是比较低级的方法现在已基本不使用。第二种是卡尔曼滤波法，用卡尔曼滤波进行信息的融合以实现系统最优估计，又分为松组合、紧组合和超紧组合（深度组合）这3个组合级别。123INS/GPS组合导航系统的发展美国等西方发达国家对INS/GPS组合导航系统的研究开始较早，21世纪初已基本完成研究并广泛应用。美、英等国早在20世纪80年代就开始了对INS/GPS组合导航系统的研究。20世界90年代初期，美国等西方发达国家已经完成了对INS/GPS组合导航系中国矿业大学2015届本科生毕业论文第3页统的研究和开发，并取得了很好的研究成果。但在上世纪90年代之前，组合导航系统以简单的松组合为主，90年代末之后开始有人对紧耦合模型进行研究,随后在飞机得到应用。20世纪末，INS/GPS超紧组合开始形成并快速发展起来。21世纪之后，西方发达国家继续对INS/GPS的组合方法进行了广泛研究，取得了许多重要的研究成果。INS/GPS的组合主要朝着高精度、高可靠性和小型化、低成本两个方向发展3。国内对INS/GPS组合导航系统的研究开始比较晚，而且刚开始时研究力量比较薄弱，对它们的研究仅在少数高校、研究所和研制与生产惯性器件企业中展开。近年来，随着北斗卫星导航系统的研究与发展，INS/GNSS组合导航已成为研究的热点，陆续出现了一些理论研究成果。吴韦华分别使用输出校正和反馈校正进行KALMAN滤波，比较了SINS/北斗组合导航松组合和紧组合4。孙枫等将一种新的滤波算法CKF应用于INS/GPS组合导航中，较之广泛使用的EKF方法改善非线性模型下INS/GPS组合导航精度5。辜道威将H滤波应用于INS/GPS松组合导航中，在噪声特性未知的情况下相比标准KALMAN滤波提高了组合导航精度6。许长辉提出一种根据给定的错误预警率和故障探测率确定抗差参数的抗差模型，该模型能够有效修复单或多异常值误差和缓慢增长误差7。胡高歌提出一种改进的强跟踪UKF算法当系统状态异常时弥补了标准UKF方法缺乏自适应能力的缺陷，提高导航精度8。但是由于精密工业的落后，市场推广的不足，理论研究还没得到广泛应用。紧耦合与超紧耦合模式的组合导航仍在实验室研究阶段，目前组合导航系统主要是简单的松组合方式。13KALMAN滤波在组合导航中的应用现状KALMAN滤波是组合导航的关键技术，从KALMAN滤波理论开始建立到现在，关于如何将滤波理论的最新结果用于组合导航中的问题一直吸引了很多科研工作者的关注。早在1988年，为了给容错组合导航系统提供设计理论，CARLSON就提出了联邦滤波理论。景观KF和EKF已在工程上被广泛应用于组合导航中，它们存在很大的局限性。KF要求系统数学模型必须为线性，而INS/GPS组合导航系统的线性误差状态方程是在假定INS失准角为小量的时候导出的，在惯性器件精度较差时这种线性误差状态方程就实际系统相差太大，用KF的方法会导致滤波发散。EKF方法关于系统模型不确定性的鲁棒性很差。近20年来，随着非线性滤波方法取得很大发展，UKF、CKF等方法被用于组合导航。随着自适应滤波技术和鲁棒滤波技术的发展，它们也被应用于组合导航吸引中。现阶段单种滤波已经不能很好地满足组合导航的精度需要，几种不同种类的滤波方法组合或者滤波和其他方法的组合已经成为趋势。谭兴龙提出了一种神经网络辅助的多重渐消因子自适应SVDUKF算法，在观测值含粗差的情况下能自适应调节，消除滤波中的异常9。杨元喜通过神经网络对状态方程预报值修复，提出一种神经网络辅助的GPS/INS组合导航自适应滤波算法，相比自适应滤波算法导航精度又有提升10。周德新针对非线性滤波算法鲁棒性差的缺陷，提出将强跟踪滤波算法与容积卡尔曼滤波相结合的一种算法并应用于组合导航中,仿真结果显示滤波效果优于CKF11。14论文主要内容中国矿业大学2015届本科生毕业论文第4页论文具体内容安排如下第一章绪论部分，介绍了KALMAN滤波的发展历程，对INS/GPS组合导航系统及其发展做了概述，最后介绍了KALMAN滤波在组合导航中的应用现状。第二章KALMAN滤波基础理论。首先给出了KALMAN滤波理论研究的基础知识，然后详细介绍离散线性随机系统的KALMAN滤波并通过仿真探究影响其滤波性能的因素，最后简要介绍离散非线性系统滤波，详细介绍EKF、UKF思路和算法。第三章UKF和CKF估计精度比较。介绍CKF原理及算法，通过MATLAB仿真将其与CKF和EKF进行估计精度比较。第四章INS/GPS组合导航理论基础。分三个部分，第一部分介绍GPS定位原理，第二部分详细介绍INS包括INS基本原理、INS导航算法及INS误差模型等，第三部分在前两部分的基础上概述INS/GPS组合导航系统的三种组合模式。第五章INS/GPS组合导航系统模型的建立。建立INS/GPS松组合状态方程和测量方程，根据建立的INS/GPS组合导航系统模型用实测数据基于EKF仿真。第六章总结与展望。中国矿业大学2015届本科生毕业论文第5页2KALMAN滤波基础理论21KALMAN滤波的理论基础211估计与滤波1估计、估计准则与最优估计在通信和控制工程中，系统的很多状态变量不能直接测量而且测量信息中往往还带有一些随机的测量噪声。在状态变量不能直接获得的情况下，我们可以选取与状态变量有函数关系的变量作为观测量，间接推算我们所需的状态变量，这种推算的过程就是估计。“估计理论所研究的对象是随机现象，是根据受干扰的量测数据来估计关于随机变量、随机过程或系统某些特征的一种科学方法12。”估计理论大体上分为参数估计和状态估计两个分支13。本文介绍的卡尔曼滤波是状态估计中广泛应用的滤波理论和方法。估计验前信息、估计约束条件和估计准则是估计问题的三个要素。根据这三要素，一个估计问题可以做如下描述若设X为N维未知状态变量，其估计值为XZ为与X有函数关系的M维观测向量，它与X的函数关系表示如下,ZFXV21V为统计规律部分或全部已知的M维观测噪声这三条假设中，第一条给出所要求的未知状态变量，第二条给出约束条件，第三条给出估计先验信息。估计问题可描述为在已知观测向量Z和观测噪声向量V的全部或部分统计规律的情况下，根据选定的估计准则和约束条件21，确定一个函数HZ，使得它成为在选定准则下X的最优估计，即XHZ22下面介绍估计准则和最优估计的概念。估计准则就是为衡量估计的好坏设定的一个判断的标准。估计准则可以是多种多样的，常用的估计准则有最小（加权）均方差估计、极大似然估计、线性最小方差无偏估计、最小二乘估计等。“最优估计就是在某一估计准则下，按照某种统计意义，13使估计达到最优。”因此，最优估计是针对某一准则而言的。2滤波的概念滤波一词起源于通信理论，14它是从含有干扰的接收信号中提取有用信号的一种技术。经典滤波理论认为信号和噪声分布在不同的频带上，可以运用经典滤波网格通过选频的方法过滤掉噪声，保留有用的信号。滤波器从早期的模拟滤波器发展为数字滤波器和统计滤波器的两个分支。模拟滤波器用于对连续模拟信号进行处理，随着集成电路技术的出现我们能够对离散数字信号进行处理，模拟滤波器开始向数字滤波器方向发展。中国矿业大学2015届本科生毕业论文第6页但是，我们所遇到的信号和噪声有时可能是随机的，这样就无法用模拟滤波器亦或是数字滤波器处理，15其特性往往只能从统计的意义上来描述，由此，产生了统计滤波器。从估计问题的角度上来说，滤波平滑和预测的概念略有不同。设,1XTT表示根据1T时刻和1T以前时刻的观测值对T时刻状态X做出的某种估计，若1TT则对,1XTT的估计称为滤波，即依据过去直至现在的观测量来估计现在的状态。当1TT时对,1XTT的估计称为为平滑，1TT时为预测。本文主要讨论滤波问题。212线性最小方差估计线性最小方差估计是在估计量是观测量的线性函数的情况下的最小方差估计。估计量的表示形式如下XZABZ23式中，若X是N维状态向量的估计值，Z为M维观测向量，则A是与N维的随机向量，B是NM维矩阵。记估计误差为XXXZ，12则基于最小方差估计准则就是要选择随机向量A和矩阵B使下式（误差方差）TJXEXABZXABZ24达到最小，此时得到的估计记为LMVXZ。将JX达到最小的A和B记为LA和LB,则LMVLLXZABZ25因此只要求得LA和LB，我们就可由公式（23）得到LMVXZ。我们将JX对A和B求偏导，令偏导等于零即可求出LA和LB。此处推导过程不做详述，最后得到1,LAEXCOVXZVARZEZ26,LBCOVXZVARZ27将式（26）和式（27）代入式（25），得1LMV,XZEXCOVXZVARZZEZ28KALMAN滤波正是基于这种最小线性方差估计准则得出的最优估计。213正交投影定理正交投影定义设X和Z分别为具有二阶矩的N维和M维随机向量，如果存在一个中国矿业大学2015届本科生毕业论文第7页与X同维的随机向量X，满足下列三个条件X可由Z线性表示，即存在非随机的N维向量A和NM矩阵B使得无偏性，即EXEXXX与Z正交，即T0EXXZ则称X是X在Z上的正交投影，记为/XEXZ,此处E代表投影的意思。从线性最小方差估计的讨论可知，基于观测量Z的X的线性最小方差LMVXZ恰好是X在Z上的正交投影。214白噪声过程如果随机过程WT满足0EWT29ATEWTWQT29B那么称WT为白噪声过程，式29B中Q为WT的方差强度。式（210）为WT的自相关函数，表示如下WRTQT210从式210中可以看出，WT的自相关函数与时间间隔T正相关，然而与时间点T无关，所以WT是平稳的过程。无论时间T和靠得多近，只要T，WRT总不为零，那么WT与W就总不相关，所以在时间变化过程中信号会跳变而不是缓慢变化。式124可进一步写成RWQ211因此WT的功率谱为JWWSWQEDQ212若随机序列K满足0KEW213ATKJKKJEWWQ213B则称为白噪声序列。中国矿业大学2015届本科生毕业论文第8页22KALMAN滤波的基本方程221随机线性离散系统的KALMAN滤波方程设随机线性离散系统的方程如下所示,11,11KKKKKKKXXW214AKKKKZHXV214B式中KX是系统的N维状态向量，KZ是系统的M维观测向量，KW是系统的P维过程噪声序列，KV是M维观测噪声序列，,1KK是系统的NN维状态转移矩阵，,1KK是NP维噪声输入矩阵，KH是MN维观测矩阵。假定关于系统过程噪声和观测噪声的统计特性如下T0,KKJKKJEWEWWQT0,KKJKKJEVEVVRT0KJEWV215式中，KQ是系统过程噪声KW的PP维对称非负定方差矩阵，KR是系统观测噪声KV的MM维对称正定方差阵，而KJ是KRONECKER函数。下面不经推导直接给出随机线性离散系统的KALMAN滤波基本方程如下状态一步预测,1,11KKKKKXX216A状态估计,1,1KKKKKKKKXXKZHX216B滤波增益矩阵1TT,1,1KKKKKKKKKKPHHPHR216C一步预测方差矩阵TT,1,11,1,11,1KKKKKKKKKKKKPPQ216D估计误差方差矩阵中国矿业大学2015届本科生毕业论文第9页,1KKKKKPIKHP216EKALMAN滤波的算法过程可以用流程图来表述，如图21所示。KALMAN滤波具有增益计算回路和滤波计算回路这两个计算回路。通过迭代进行增益矩阵和误差方差矩阵的更新，然后通过输入观测量及更新的增益矩阵完成状态向量的更新，由此增益计算回路是独立的而滤波计算回路在增益计算回路的基础上完成。图21KALMAN滤波算法222滤波基本方程分析1递推性从KALMAN滤波及式（216B）所描述的系统状态方程可知，从1K时刻的估计开始，由系统的状态方程可预测K时刻状态向量，再由观测方程和K时刻的观测量，就可以推算出K时刻状态KX的最小方差估计KX。在有了初始值0X的情况下，就可以依次推算估计1X，2X，3X，KX，这就是KALMAN滤波的递推性质。正是因为其递推性质，它在数据的存储方面优于维纳滤波，因为计算无须保存过去的测量数据，只需根据当前时刻的测量数据和前一时刻的状态向量的估计值，即可更新当前时刻状态向量的估计值。运用这种滤波方式，可以在根据当前状态向量的估计值预测下一个时刻的系统状态，也可以通过当前观测量更新当前的状态向量估计值，对前一时刻的预测有一个校正作用。2反馈校正性输入输入计算在此处键入公式。计算计算计算计算输出输出时间更新观测更新增益计算回路滤波计算回路中国矿业大学2015届本科生毕业论文第10页由式（216B）可以看出，第二项是对原有预测值的校正项。因为在获得第K次测量量KZ之前，,1KKX仅是由1KX依据状态方程所推断出来的KX一个初步估计，在获得KZ之后，就需要对,1KKX进行校正，校正项的系数KK称为增益矩阵，根据测量关系，KK由测量误差和前一刻的预测误差共同决定。3增益矩阵的性质如果状态一步预测很准确，即,1KKKXX的差值很小，,1KKP则很小，那么测量KZ的意义就不大，增益KK也不会大，这从式（216C）直接可见。特别地，从极端情况看，若,1KKKXX的差值为零，则无须校正，相应的,1KKP，KK均为零。如果测量KZ很准确，则KR很小。结论是KK与,1KKP成正比与KR成反比。,1KKP随动态过程噪声误差KQ的大小而增减，那么就表明输入系统噪声越强，KK越大，倚重测量；反之，测量噪声越强，KK就越小，自然要倚重系统状态方程。223算例分析通过对目标常速度模型（CV）的MATLAB仿真，分析影响滤波的估计性能的因素。CV模型的离散化状态方程和离散化测量方程如下1122111|1XKXKXKKKWKXKXK12XKZKHKCKVKXK217式中，11|01TKK，10HK，5CK。对噪声的统计特性满足式（219），且1001QK，1RK；取仿真初始条件300|05X，1000|0010P。模拟产生10S的跟踪数据，01T。只考虑被估计状态1XK的滤波结果，如图22所示。图221XK的滤波结果中国矿业大学2015届本科生毕业论文第11页为了分析影响KALMAN滤波的估计性能的因素，考虑下面四种情况,分别在MATLAB环境下仿真，与图22的滤波结果分析比较。探究测量方程准确与否对滤波的影响。假设利用无系统误差的不准确的观测方程来替代真实的观测方程，即KALMAN滤波算法中取0CK，则滤波计算结果如图23所示。探究仿真初始条件对滤波的影响。改变仿真初始条件，取初始值00|05X100|001P。滤波计算结果如图24所示。探究系统方程准确与否对滤波的影响。利用不准确的系统状态方程，此处假设滤波过程中取0901|01KK，则滤波计算结果如图25所示探究过程噪声和观测噪声对滤波的影响。表21给出了不同过程噪声和测量噪声的情况下滤波的误差以及增益矩阵第一个元素11KK，仅取前5次采样。图23测量方程不准确时1XK的滤波结果图24改变仿真初始条件的1XK的滤波结果中国矿业大学2015届本科生毕业论文第12页图25状态方程不准确时1XK的滤波结果表21不同噪声和观测噪声取值下的KALMAN滤波计算结果QI22,52QI22,1Q52I22,12X1的估计误差K11KX1的估计误差K11KX1的估计误差K11K032890286324229183481453303075026330241602319022880918208041019720813108065091740671406507065210653607042009270858403965082100972309634096370964109644图23显示估计偏差在6左右震荡，相比于图22估计偏差误差明显增大。可以看出由于存在明显的系统偏差，影响滤波精度。从图24可以看出，仿真刚开始时估计误差比较大，然后估计误差快速减小，一小段时间后收敛于0。这是由于同时改变仿真初始条件0|0X和0|0P，当0|0X与真实值相差比较大且0|0P取值比较小时，需要多次迭代滤波值才能接近真值。因此，仿真初始值设置不当将会影响开始一小段时间内的滤波精度。从图25可以看出，由于利用不准确的状态方程，滤波结果呈发散趋势，影响滤波的精度。表21中改变过程噪声和观测噪声的统计性质，估计误差没有特别显著的变化，但是对增益矩阵有直接影响。从表21计算结果可知，当R增大时，11KK明显减小，即测量噪声越强，增益矩阵就越小，滤波估计倚重系统状态方程；而当Q增大时，11KK增大，即增益矩阵随系统噪声的增强增大，从而加强测量信息的作用。这正验证了222小节中关于增益矩阵的结论KK与,1KKP成正比与KR成反比。输入系统噪声越强，KK越大，倚重测量；反之，测量噪声越强，KK就越小，自然要倚重系统状态方程。23离散非线性系统滤波前面一小节介绍的最基本的KALMAN滤波只适用于线性系统，并且要求观测方程也必须是线性的。然而，非线性随机动态系统广泛存在于工程实践中，如飞机和舰船的惯性导航系统、火箭制导和控制系统、卫星轨道/姿态的估计系统、组合导航系统等都属于这类系统。并且，系统的观测方差也可能是非线性的。为此，我们需要适用于非线性随机动态系统的滤波方法。对于非线性系统来说，得到精确的最优滤波解很困难，为此提出了大量的次优的近似非线性滤波的方法。这些方法大致分为三类第一类是解析近似的中国矿业大学2015届本科生毕业论文第13页方法；第二类是基于确定性采样的方法；第三类是基于随机采样的方法。具体如表所示表22非线性滤波算法分类近似方法函数近似确定性采样近似随机采样近似泰勒级数展开插值多项式展开典型算法扩展KALMAN滤波DDF/CDF无迹KALMAN滤波粒子滤波改进算法UD分解，奇异值分解LD分解，平方根滤波二阶扩展KALMAN滤波迭代扩展KALMAN滤波平方根DDF高斯混合DDF平方根无迹KALMAN滤波高斯混合无迹KALMAN滤波UPF，正则化粒子滤波MCMC方法RBPF，辅助粒子滤波器其中应用最广泛的是扩展KALMAN滤波（EKF）。其基本思想是将非线性系统线性化，然后运用广义KALMAN滤波技术进行状态估计，其本质还是KALMAN滤波。然而，由于非线性系统线性化存在高阶阶段误差且线性化过程中计算JACOBI矩阵存在不准确性，这些都将影响状态估计效果。近年来无迹KALMAN滤波得到到广泛应用，它们都属于SIGMA点滤波（SPKF）。SIGMA点滤波的基本思想是通过设计少量的SIGMA点，由SIGMA点经由非线性函数传播，计算出随机向量一、二阶统计特性。依无迹KALMAN滤波据变换矩阵的不同，SIGMA点滤波包括无迹KALMAN滤波（UKF）、平方根无迹KALMAN滤波（SRUKF）、中心分布KALMAN滤波（CDKF）、平方根中心分布KALMAN滤波（SRCDKF）等。这一小节主要介绍EKF和UKF的原理。231EKF扩展卡尔曼滤波的本质还是KALMAN滤波，最后的滤波方程都是一致的，不同的是扩展卡尔曼滤波需要通过泰勒级数展开的方法对非线性系统进行线性化，泰勒展开保留一阶项。考虑有如下非线性系统111,1,1KKKKXFXKXKW218A,KKKZHXKV218B式中，KW和KV均为白噪声序列。EKF过程如下将状态方程和观测方程通过泰勒展开线性化由系统状态方程将1,1KFXK围绕滤波值KX进行泰勒展开，只保留一阶项。11111111,1|,1KKKKKKKKXXKFXFXKXXXKWX令11,11|KKKKXXKFX，111111,1|KKKKKXXKFFXKXX中国矿业大学2015届本科生毕业论文第14页则状态方程最终表示为,11111,1KKKKKKKXXXKW219与KALMAN滤波基本方程相比，状态方程增加了非随机的外作用项1K。由系统观测方程将,KHXK围绕滤波值,1KKX进行泰勒展开，只保留一阶项。,1,1,1,|KKKKKKKKKKXXKHZHXKXXVX令,1|KKKKXXKHHX，,1,1,1,|KKKKKKKKXXKHHXKXYX则最终观测方程为KKKKKZHXVY220与KALMAN滤波基本方程相比，测量方程增加了KY。根据KALMAN滤波基本方程得出EKF方程状态一步预测,11,1KKKXFXK221A状态估计,1,1,KKKKKKKXXKZHXK221B滤波增益矩阵1TT,1,1KKKKKKKKKKPHHPHR221C一步预测方差矩阵TT,1,11,1111,1,1KKKKKKKKKKPPXKQXK221D估计误差方差矩阵,1KKKKKPIKHP221E与KALMAN滤波基本方程相比，只有221A和221B不一致，换成了非线性函数计算，其中,1KK和KH为雅克比矩阵需计算求得。中国矿业大学2015届本科生毕业论文第15页232UKFUKF也是基于卡尔曼滤波原理的一种非线性滤波方法，但不同于EKF的处理方式，UKF不对非线性系统的状态方程和观测方程进行线性化，而是利采样点近似的原理，选取一系列近似高斯分布的SIGMA点通过UT变换来进行状态与误差协方差的递推和更新。UT变换与线性化方法的比较如图25所示图25UT变化与线性化方法比较设ZFX,X是N维随机变量。UT变换是根据X的均值和方差设计21N个SIGMA点（N为高斯随机变量的维数），对设计的点计算其通过非线性函数传播的结果I，然后基于I计算Z的均值和方差。考虑如下非线性模型1,KKKKXFXUW222A,KKKZHXV222B式中，KX为N维系统状态，KW为N维系统过程噪声，KV为M系统观测噪声。UKF算法的具体流程如下状态参数初始化，设定初始状态向量0X及协方差矩阵0P,同时给出系统噪声和量测噪声的协方差矩阵Q和R。对于给定的1KX和1KP，用UT变化求状态一步预测,1KKX和一步预测协方差矩阵,1KKP。计算SIGMA点1IK011KKX111,1,2,IKKKXNPIN111,1,2,2IKKKXNPINNN223求状态一步预测,1KKX和一步预测协方差矩阵,1KKP中国矿业大学2015届本科生毕业论文第16页,11,1,2,2IIKKKFIN2242,1,10NMIKKIKKIXW2252T,1,1,1,1,110NCIIKKIKKKKKKKKKIPWXXP226计算观测更新方程,1,1KKKKH2272,1,10NMIKKIKKIZW2282T,1,1,1,10KNCIIIKKKKKKKKZIPWZZ2292T,1,1,1,10KKNCIIXZIKKKKKKKKIPWXZ230滤波更新1KKKKXZZKPP231,1,1KKKKKKKXXKZZ232T,1KKKKKKZPPKPK233中国矿业大学2015届本科生毕业论文第17页3UKF及CKF的估计精度比较31CKFCKF根据SPHERICALRADIALCUBATURE准则,计算出一组具有相同权重的点并直接通过非线性系统方程传播这些点来进行状态估计，16无需对非线性模型线性化,其精度达3阶。考虑如下非线性模型1,KKKKXFXUW31A,KKKZHXV31B式中，KX为N维系统状态，KW为N维系统过程噪声，KV为M系统观测噪声CKF算法的具体流程如下计算出CUBATURE点对应的权I22112,I1,2,2N32时间更新计算CUBATURE点I,K11I133用非线性状态方程传播CUBATURE点I,KI,K134计算一步预测估计和一步预测协方差矩阵2,1,1NKKIIKIXX352TT,1,1,111NKKIIKIKKKKKKIPXXXXQ36测量更新计算CUBATURE点中国矿业大学2015届本科生毕业论文第18页I,K,1I,137用非线性状态方程传播CUBATURE点,I,K38计算量测预计和协方差2,1NKIIKIZZ392TT,1NZZKIIKIKKKKIPZZZZR3102TT,11NXZKIIKIKKKKIPXZXZ311滤波更新,/,312,1313,1,31432仿真分析321单变量非平稳模型17单变量非平稳模型UNGM是一种典型的一维非线性模型，在经济领域广泛使用，其离散时间状态方程和测量方程如下0512511128COS121220315UNGM的状态方程和测量方程都为非线性，且系统状态方程非线性程度较高。式中，0,1且0,1。仿真初始条件为0|00，P0|01,状态初始真实值为005在MATLAB环境下仿真50次。采用如下定义的误差均方值MSE来评价滤波精度。中国矿业大学2015届本科生毕业论文第19页211NKKIMSEXXN316其中，KX是真实值，KX为估计值，N为仿真次数。用以下四种滤波对单变量非平稳模型进行仿真UKF1（1，0，K0）UKF21，2，KN3EKFCKF仿真结果如图31、图32、图33和表31所示。图31三种方法真实值与滤波值对比图32误差值对比中国矿业大学2015届本科生毕业论文第20页图33估计精度对比表31滤波精度统计信息滤波算法MSERMSEUKF134592458815UKF265384780861EKF1751305132337CKF75656386981从仿真结果可以看出，在系统状态方程非线性较强的情况下，UKF1、UKF2和CKF的MSE和RMSE均小于EKF，从而得出UKF和CKF滤波精度均高于EKF。一维下CKF的MSE和RMSE明显大于UKF1，也大于UKF2，因此UKF滤波精度高于CKF。这验证了维数N2时，UKF估计精度高于CKF。17UKF1滤波精度高于UKF2，由此可见，在系统状态方程非线性较强的情况下，UKF参数设置对滤波结果影响较大。322纯方位跟踪模型18双基站纯方位跟踪（BOT）的离散状态方程和测量方程如下,1111212317由式（317）可见BOT模型是典型的四维非线性模型，它的状态方程为线性，测量方程为非线性。式中，TXYXKXKYKVKVK，I1,2表示目标的方位,1,2表示零均值白噪声。I按如下公式计算ARCTAN1,2318中国矿业大学2015届本科生毕业论文第21页式中，II1,2表示基准站1与基准站2的位置。仿真初始条件设置为T0|00000X,0|001011010PDIAG，初始状态真值为T0010。两个基准站位置分别为12和11，2200500005R，T000101KW，采样间隔为001S，在MATLAB环境下采用341小结中的四种滤波方法仿真50次。采用误差均方值MSE和误差均方根RMSE来评价滤波精度。仿真结果如图34、图35、图36及表32所示。图34仿真轨迹及滤波跟踪结果图35位置RMSE图36速度RMSE中国矿业大学2015届本科生毕业论文第22页表32滤波精度统计信息滤波算法位置MSE位置RMSE速度MSE速度