卡尔曼滤波与组合导航原理++第2版+秦永元，张洪钺，汪叔华编著

卡尔曼滤波与组合导航原理++第2版+秦永元，张洪钺，汪叔华编著+西北工业大学出版社再版说明自1998年本书第1版出版以来的13年间,大量的最优估计新理论和新方法涌现了出来。为适应工程应用需要,更好地为国民经济和国防建设服务,笔者在第1版基础上作了修改和增添,完成了本书的编写。本次修订的主要内容如下(1)校正了第1版中的印刷错误。对固定滞后平滑、固定区间平滑、信息滤波、a-B-y滤波和传递对准中姿态信息匹配量构造方法等作了改写。(2)增加了H滤波、UKF非线性滤波、粒子滤波、估计均方误差阵及最佳增益阵同解形式的介绍和推导。本书再版工作由秦永元完成由于水平和时间有限,再版后还会存在不尽如人意之处,敬请广大读者批评指正。编著者2011年12月目录第一章绪论44a··“·········.:4·.·······.···a.····4····1.1卡尔曼滤波所要解决的问题…………1.2卡尔曼滤波理论的发展和工程应用21.3组合导航简介…·。4。要·血第二章几种最优估计和卡尔曼滤波基本方程2.1几种最优估计2.2离散型卡尔曼滤波…2.3连续型卡尔曼滤波…………习题a;.:.···············:·‘4·甲第三章尔曼滤波中的技术处理713.1有色噪声的白化………713.2有色噪声条件下的卡尔曼滤波………793.3序贯处理…844信息滤波863.5卡尔曼滤波发散的抑制873.6平方根滤波…933.7UDU分解滤波咖备··1033.8自适应滤波甲D萨。非B●1183.9次优滤波1213.10卡尔曼滤波误差分析…1293.11H滤波a·.···‘:·················.······"····139习题……………………………………152第四章卡尔曼滤波稳定性介绍………………………………1554.1稳定性定义…………1554.2判别卡尔曼滤波稳定的充分条件1564.3适用于惯导系统的滤波稳定判别条件………………166习题168第五章滤波系统的校正番bp·音?··鲁1695.1概述169卡尔曼滤波与组合导航原理5.2离散系统的分离定理17053连续系统的分离定理…………1745.4离散系统的估计直接反馈控制1755.5连续系统的估计直接反馈控制…d“·179习题179第六章卡尔曼滤波的推广…………………………1816.1最优线性平滑6.2非线性系统滤波之一:EKF……………………………………………………………1986.3非线性系统滤波之二:UKF21264非线性系统滤波之三:粒子滤波∴………∷222习题·鲁·。目·鲁鲁鲁·D鲁音鲁·曾鲁D非··;自自·非··自春自………228第七章容错组合导航的设计理论…●曹·鲁···身··會·鲁●··曾奇看··d●●2297.1概述…………………··曾·兽非··自p。音非·自音鲁非着鲁·血……2297.2联邦滤波器算法原理2307.3系统级故障检测与隔离的原理与方法…鲁■非·曹毒2447.4惯性器件的故障检测与隔离原理…2527.5故障的统计检测原理与风险分析267第八章卡尔曼滤波理论在组合导航系统设计中的应用……2778.1概述■鲁口··……2778.2组合导航系统的设计模式即即学……2788.3组合导航系统设计中一些常用导航子系统的误差模型………………2828.4惯性导航系统的误差模型3068.5卡尔曼滤波理论在惯导系统初始对准中的应用………3258.6应用基本滤波理论设计组合导航系统…3528.7应用联邦滤波理论设计容错组合导航系统……367附录…………………………378参考文献第一章绪论1.1卡尔曼濾波所要解决的问题所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。信号是传递和运载信息的时间或空间函数。有一类信号的变化规律是既定的,如调幅广播中的载波信号、阶跃信号、脉宽固定的矩形脉冲信号等,它们都具有确定的频谱,这类信号称为确定性信号。另一类信号没有既定的变化规律,在相同的初始条件和环境条件下,信号的每次实现都不一样,如陀螺漂移、海浪、作水平飞行的飞机飞越山区时无线电高度表的输出信号、惯导系统的导航输出误差、GPS的SA误差等,它们没有确定的频谱,这类信号称为随机信号。由于确定性信号具有确定的频谱,所以可根据各信号频带的不同,设置具有相应频率特性的滤波器,如低通、高通、带通、带阻滤波器,使有用信号无衰减地通过,使干扰信号受到抑制这类滤波器可用物理方法实现,此即模拟滤波器,也可用计算机通过算法实现,此即数字滤波器。对确定性信号的滤波处理也称常规滤波。随机信号没有确定的频谱,无法用常规滤波提取或抑制信号,但随机信号具有确定的功率谱,所以可根据有用信号和干扰信号的功率谱设计滤波器。维纳滤波是解决此类问题的方法之一。但设计维纳滤波器须作功率谱分解,只有当被处理信号为平稳的,干扰信号和有用信号均为一维,且功率谱为有理分式时,维纳滤波器的传递函数才可用伯特香农设计法较容易地求解出。否则设计维纳滤波器存在着诸多困难。维纳滤波除设计思想与常规滤波不同外,对信号作抑制和选通这一点是相似的。卡尔曼滤波从与被提取信号有关的量测量中通过算法估计出所需信号。其中被估计信号是由白噪声激励引起的随机响应,激励源与响应之间的传递结构(系统方程)已知,量测量与被估计量之间的函数关系(量测方程)也已知。估计过程中利用了如下信息:系统方程、量测方程、白噪声激励的统计特性、量测误差的统计特性。由于所用信息都是时域内的量,所以卡尔曼滤波器是在时域内设计的,且适用于多维情况。这就完全避免了维纳滤波器在频域内设计遇到的限制和障碍,适用范围远比维纳滤波器广。从以上简述中可看出卡尔曼滤波有如下特点:(1)卡尔曼滤波处理的对象是随机信号;(2)被处理信号无有用和干扰之分,滤波的目的是要估计出所有被处理信号(3)系统的白噪声激励和量测噪声并不是需要滤除的对象,它们的统计特性正是估计过程中需要利用的信息。所以确切地说,卡尔曼滤波应称作最优估计理论,此处称谓的滤波与常规滤波具有完全不同的概念和含意卡尔曼滤波与组合导航原理就实现形式而言,卡尔曼滤波器实质上是一套由数字计算机实现的递推算法,每个递推周期中包含对被估计量的时间更新和量测更新两个过程。时间更新由上一步的量测更新结果和设计卡尔曼滤波器时的先验信息确定,量测更新则在时间更新的基础上根据实时获得的量测值确定。因此,量测量可看做卡尔曼滤波器的输入,估计值可看做输出。输入与输出之间由时间更新和量测更新算法联系,这与数字信号处理概念是类似的,所以有些书上称卡尔曼滤波为广义数字信号处理似有一定道理1.2尔曼魂波理论的发展和工程应用随机信号没有既定的变化规律,对它们的估计也不可能完全准确,所谓最优估计也仅仅是指在某一准则下的最优。根据不同的最优准则,可获得随机信号的不同最优估计。使贝叶斯风险达到最小的估计为贝叶斯估计;使关于条件概率密度的似然函数达到极大的估计为极大似然估计;使验后概率密度达到极大的估计为极大验后估计;使估计误差的均方误差达到最小的估计为最小方差估计,若估计具有线性形式,则估计为线性最小方差估计,卡尔曼滤波即属此类估计。卡尔曼滤波理论的创立是科学技术和社会需要发展到一定程度的必然结果。早在1795年,高斯( Karl gauss)为测定行星运动轨道而提出了最小二乘估计法。20世纪40年代,为了解决火力控制系统精确跟踪问题,维纳(N. Weaner)于1942年提出了维纳滤波理论2。维纳根据有用信号和干扰信号的功率谱确定出线性滤波器的频率特性,首次将数理统计理论与线性系统理论有机地联系在一起,形成了对随机信号作平滑、估计或预测的最优估计新理论。比维纳稍早,前苏联科学家戈尔莫克洛夫(A.N, Kolmogorov)于1941年也曾提出过类似理论[3。维纳给出了由功率谱求解维纳滤波器频率特性闭合解的一般方法,包括对功率谱的上、下平面分解及傅里叶变换和反变换,运算繁杂,解析求解十分困难。1950年,伯特和香农给出了在功率谱为有理谱这一特殊条件下,由功率谱直接求取维纳滤波器传递函数的设计方法,这一方法简单易行,具有一定的工程实用价值。维纳滤波的最大缺点是适用范围极其有限,它要求被处理信号必须是平稳的,且是一维的。人们试图将维纳滤波推广到非平稳和多维的情况[5,都因无法突破计算上的困难而难以推广和应用。采用频域设计法是造成维纳滤波器设计困难的根本原因。因此人们还渐转向寻求在时域内直接设计最优滤波器的新方法1。其中卡尔曼的研究最具有代表性,他提出的递推最优估计理论也因此而被称为卡尔曼滤波。由于采用了状态空间法描述系统,算法采用递推形式,所以卡尔曼滤波能处理多维和非平稳的随机过程。卡尔曼滤波理论一经提出,立即受到了工程界的重视,而工程应用中遇到的实际问题又使卡尔曼滤波的研究更深入更完善。1959年起美国太空署即NASA( Nation al aeron autics andSpace Administration)开始研究载人太空船登月方案,当时提出了两个主要问题:①中途导航和制导;②液体燃料助推器大挠度条件下的自动驾驶问题。因这两项研究的工作量都很庞大,无力同时进行,所以选择前者作为重点,即宇宙飞船的测轨问题。导航问题中主要解决对太空船运动状态的估计。量测信息来自三个子系统:飞船装备的惯性测量装置和天文观测仪地面测轨系统,测轨数据经数据链传送至太空船。估计方法曾试图采用递推加权最小二乘和维纳滤波,均因精度满足不了要求和计算过于繁杂而不得不放弃。1960年秋,卡尔曼访问了第一章绪论NASA,提出了卡尔曼滤波算法,立即引起重视并投入研究。由于最初提出的卡尔曼滤波仅适用于线性系统,而实际系统是非线性系统,滤波初值应如何取才合理,这些都迫使卡尔曼作进一步的思考,广义卡尔曼滤波就是在此情况下提出来的。阿波罗计划中的导航系统后由麻省理工学院研制完成[18。卡尔曼滤波早期应用中的另一成功实例为C-5A飞机的多模式导航系统卡尔曼滤波比维纳滤波的应用范围广,设计方法也简单易行得多,但它必须在计算机上执行,而20世纪60年代初,无论是速度、字长、容量,计算机还处于低水平阶段。为了适应当时的技术水平,避免由于字长不够产生的舍入误差引起卡尔曼滤波的计算发散, Bierman,Carlson和 Schmidt等人提出了平方根滤波算法和UDU分解滤波算法,以确保卡尔曼滤波增益回路中的滤波方差阵始终正定15231卡尔曼最初提出的滤波基本理论只适用于线性系统,并且要求量测也必须是线性的。在之后的10多年时间内,Bucy, Sun ahara等人致力于研究卡尔曼滤波理论在非线性系统和非线性量测情况下的推广233,拓宽了卡尔曼滤波理论的适用范围。卡尔曼滤波最成功的工程应用是设计运载体的高精度组合导航系统。20世纪80年代起可供运载体装备的导航系统越来越多,非相似导航子系统的增加使量测信息增多,这对提高组合导航系统的精度十分有利。但是,如果采用集中式卡尔曼滤波器实现组合,则存在两个致命问题:①滤波器计算量以状态维数的三次方剧增,无法满足导航的实时性要求;②导航子系统的增加使故障率也随之增加,只要有一个子系统发生故障又没有及时检测出并隔离掉,则整个导航系统都会被污染。为了解决这一矛盾,1979年至1985年间, Speyer, Bierman和Ker等人先后提出了分散滤波思想30。并行计算技术的成熟为分散滤波的发展创造了有利条件。1988年起, Carlson提出了联邦滤波理论( Federated Filtering)116,旨在为容错组合导航系统提供设计理论。 Carlson在装备运载体的诸多非相似导航子系统中选择导航信息全面、输出速率高、可靠性绝对保证的子系统作为公共参考系统,与其余子系统两两组合,形成若干个子滤波器。各子滤波器并行运行,获得建立在子滤波器局部量测基础上的局部最优估计,这些局部最优估计在第二级滤波器即主滤波器内按融合算法合成,获得建立在所有量测基础上的全局估计。全局估计再按信息守恒原则反馈给各子滤波器。实际设计的联邦滤波器是全局次优的,但是对于自主性要求特别高的重要运载体来说,导航系统的可靠性比精度更为重要。采用联邦滤波结构设计组合导航系统,虽然相对最优损失了少许精度,但换来的却是组合导航系统的高容错能力。目前美国空军已将联邦滤波器确定为新一代导航系统的通用滤波器4,46]1.3组合导航简介将运载体从起始点引导到目的地的技术或方法称为导航。导航系统测量并解算出运载体的瞬时运动状态和位置提供给驾驶员或自动驾驶仪实现对运载体的正确操纵或控制。随着科学技术的发展,可资利用的导航信息源越来越多,导航系统的种类也越来越多。以航空导航为例,目前可供裝备的杋载导航系统有惯性导航系统、GPS导航系统、多卜勒导航系统、罗兰C导航系统等,这些导航系统各有特色,优缺点并存。比如,惯性导航(以下简称惯导)系统的优点是:不需要任何外来信息也不向外辐射任何信息,可在任何介质和任何环境条件下实现导卡尔曼滤波与组合导航原理航,且能输出飞机的位置速度、方位和姿态等多种导航参数,系统的频带宽,能跟踪运载体的任何机动运动,导航输出数据平稳,短期稳定性好。但惯导系统具有固有的缺点:导航精度随时间而发散,即长期稳定性差。GPS导航系统导航精度高,在美国国防部加入SA( SelectiveAvaila bility)误差后,使用C/A码信号的水平和垂直定位精度仍分别可达100m和157m(2a),且不随时间发散,这种高精度和长期稳定性是惯导系统望尘莫及的。但GPS导航系统也有其致命弱点:频带窄,当运载体作较高机动运动时,接收机的码环和载波环极易失锁而丢失信号,从而完全丧失导航能力;完全依赖于GPS卫星发射的导航信息,受制于他人,且易受人为干扰和电子欺骗。其余导航系统也有各自的优缺点。各种导航系统单独使用时是很难满足导航性能要求的,提高导航系统整体性能的有效途径是采用组合导航技术,即用两种或两种以上的非相似导航系统对同一导航信息作测量并解算以形成量测量,从这些量测量中计算出各导航系统的误差并校正之。采用组合导航技术的系统称组合导航系统,参与组合的各导航系统称子系统。实现组合导航有两种基本方法(1)回路反馈法,即采用经典的回路控制方法,抑制系统误差,并使各子系统间实现性能互补。第二次世界大战期间,英国为解决海上巡逻机长时间连续飞行的导航精度问题,率先研究了该型系统。(2)最优估计法,即采用卡尔曼滤波或维纳滤波,从概率统计最优的角度估计出系统误差并消除之。两种方法都使各子系统内的信息互相渗透,有机结合,起到性能互补的功效。但由于各子系统的误差源和量测误差都是随机的,所以第二种方法远优于第一种方法。设计组合导航系统时一般都采用卡尔曼滤波。由于惯导和GPS在性能上正好形成互补,所以采用该两种系统作为组合导航设计中的子系统是世界公认的最佳方案,如图1.3.1所示组合导航系统一般具有以下3种功能(1)协合超越功能。组合导航系统能充分利用INS惯导输出各子系统的导航信息,形成单个子系统不具备的功能和精度。KF(2)互补功能。由于组合导航系统综合利用了GPS各子系统的信息,所以各子系统能取长补短,扩大使用范围。图1.3.1惯导/GPS组合导航系统原理图(3)余度功能。各子系统感测同一信息源,使测量值冗余,提高整个系统的可靠性组合导航系统的发展方向是容错组合导航系统和导航专家系统,这些系统具有故障检测诊断、隔离和系统重构的功能。目前,很多国家都在发展以激光陀螺捷联惯导系统和GPS导航系统为主要子系统并辅以其余子系统的组合导航系统,如 Litton公司的 FLAGSHIP系统和Honeywell公司的 GAINS系统,平均故障时间可达1200小时,可根据不同的使用要求和系统故障情况选择不同的工作模式,都已成功地应用于诸多先进飞机上。第二章几种最优估计和卡尔曼滤波基本方程2.1几种最优佔计所谓估计就是根据测量得出的与状态X(t)有关的数据z(t)=hX(t)]+v(t)解算出X(t)的计算值x(t),其中随机向量v(t)为量测误差,X称为x的估计,Z称为X的量测。因为x(t)是根据z(t)确定的,所以X(t)是Z(t)的函数。若X是Z的线性函数,则X称作X的线性估计设在[t,t1]时间段内的量测为Z,相应的估计为x(t),则当t=t1时,X(t)称为X(t)的估计;当t>t:时,X(t)称为X(t)的预测;当t