第ｌ１卷第６期　２０１１年ｌ２月　潍坊学院学报　Ｊｏｕｒｎａｌ　ｏｆ　Ｗｅｉｆａｎｇ［Ｊｎｉｖｅｒｓｉｔｙ　Ｖｏ１．１】Ｎｏ．６　Ｄｅｃ．２０１１　

容错联邦强跟踪卡尔曼滤波算法在组合　

导航中的应用研究　

马云峰　（潍坊学院，山东潍坊２６１０６１）　

摘要：提出了一种改进的联邦强跟踪卡尔曼滤波算法，并将其应用于ＭＳＩＮＳ／ＧＰＳ浅组合中。采用　了三层容错结构，能够有效地检测出数据粗大误差、ＧＰＳ速度跳变以及滤波器误差估计的故障，及时进行　

故障隔离，从而使系统具有良好的容错能力。仿真结果表明，联邦强跟踪卡尔曼滤波算法解决了量测相　关、ＧＰＳ速度跳变和初值选取敏感性等问题，可以提高系统的实时性、鲁棒性和容错性能。　关键词：捷联惯性导航系统；全球定位系统；组合导航系统；卡尔曼滤波；强跟踪；联邦滤波；容错　中图分类号：Ｖ２４９．３２　文献标识码：Ａ　文章编号：１６７１－－４２８８（２０１１）Ｏ６—０ｌ１Ｏ一０６　

１　引言　Ｋａｌｍａｎ滤波技术已经广泛地应用于组合导航系统中，但在ＭＳＩＮＳ／ＧＰＳ浅组合中采用经典的集中式　卡尔曼滤波器还存在着滤波的实时性、滤波稳定性、ＧＰＳ速度跳变等问题。　１９８８年Ｃａｒｌｓｏｎ提出了用于容错系统的联邦卡尔曼滤波器信息融合算法，由于联邦滤波器设计灵活、　计算量小、容错性能好而备受重视［卜　，这种算法的特点是采用不同的信息分配策略可以获得不同容错性　能的组合导航系统，这种化整为零的估计方法十分适合于多导航数据处理器进行并行计算，从而提高整个　

系统的实时性。考虑到ＧＰＳ输出的位置和速度测量值是相关的，而且ＧＰＳ的速度有时会发生突跳，若采　

用集中式卡尔曼滤波器极易造成滤波发散。为解决量测相关并提高系统的容错性，在ＭＳＩＮＳ／ＧＰＳ浅组　合应用中设计一种联邦容错强跟踪卡尔曼滤波器，采用有反馈的融合结构，实现对导航误差的滤波估计，　并能对故障进行隔离和系统重构。　

２联邦强跟踪卡尔曼滤波器的设计　滤波器结构如图１所示。该方案采用两个并行的强跟踪卡尔曼子滤波器，分别对位置和速度进行估　

计，然后通过一个主滤波器融合各子滤波器的状态估计，子滤波器的更新周期和主滤波器的融合周期均为　１　Ｓ；采用了三层故障检测实现容错设计，提高组合导航系统的故障隔离和重构的能力。　

————　图１　联邦强跟踪卡尔曼滤波器的结构　

＊收稿日期：２０１１一Ｏ５—１８　基金项目：山东省自然科学基金项目（Ｙ２００７Ｇ６４）　作者简介：马云峰（１９６７一），男，山东临朐人，潍坊学院信息与控制工程学院教授，工学博士。研究方向：组合导航技术。　

１１０

—　第６期　马云峰：容错联邦强跟踪卡尔曼滤波算法在组合导航中的应用研究　

２．１位置子滤波器　

位置子滤波器的状态变量包括ＭＳＩＮＳ导航计算参数误差、陀螺仪测量误差和加速度计测量误差。状　

态变量Ｘ　（￡）为　ｘｌ（￡）＝［（ｚ）　（ｚ）　乩　￡　￡ｋ　Ｖ　］　

（１）　ＭＳＩＮＳ／ＧＰＳ浅组合的位置子滤波器状态方程与纯惯导系统的状态方程形式相同Ｌ引，即为　

Ｘｌ（￡）一Ｆ，（￡）Ｘ１（￡）＋Ｇ，（ｆ）Ｗ，（￡）　（２）　位置子滤波器的观测方程为　

Ｚｐ（　）一Ｈｐ（￡）Ｘ（　）＋　ｐ（　）　（３）　

式中，　Ｈ　（￡）一Ｅｏ３×６　ｄｉａｇ［－（Ｒ　＋＾）（Ｒ　－￣－ｈ）ｃｏｓＬ　１］０３￣６］　（４）　

（ｆ）＝［Ｐ　Ｐ　Ｐ　］　（５）　

式中，Ｐ　、Ｐ　、Ｐ　为ＧＰＳ接收机沿东北天方向的位置误差。　

２．２速度子滤波器　速度子滤波器与位置子滤波器的状态变量相同，设为Ｘ。（￡）　

Ｘ２（￡）一［（２）　《２）　ｌ２）　乩　ｅ　￡　￡ｋ　］　

（６）　速度子滤波器状态方程与纯惯导系统的状态方程形式也相同，即为　

Ｘ２（￡）一ＦＪ（￡）Ｘ２（ｆ）＋Ｇｊ（￡）Ｗ，（￡）　（７）　

速度子滤波器的量测方程　（￡）为　

（　）一Ｈ　（　）Ｘ（￡）＋７３ｖ（　）　（８）　

式中，　Ｈ　（ｆ）一Ｅｏ３×３　ｄｉａｇＥ１　１　１］０３×９］　（９）　

（ｆ）：［　］　（１０）　

式中，　、　、　为ＧＰＳ接收机沿东北天方向的速度误差。　２．３主滤波器及信息分配　

设主滤波器的状态估计为　估计误差方差阵为Ｐ，；位置、速度子滤波器的状态估计分别为　、　。，　

相应的估计误差方差阵为Ｐ　、Ｐｚ。系统信息Ｐ　、Ｑ　由两个子滤波器共同分享，主滤波器中不分配信息　

（　＋　一１，ｐｍ一０），只执行各子滤波器的数据融合。　

（１）假定初始时刻全局状态估计为叉『Ｕ，其协方差阵为Ｐ，。，系统噪声的协方差矩阵为Ｑ『Ｄ。根据信息　

守恒原理，把这些信息通过信息分配因子　、　分配到各子滤波器。　

』　ｉ＝ｌ，２，ｍ　…）　１Ｘ　（走）一Ｘ，（是）　系统噪声信息按照同样的信息分配原则分配到各子滤波器。　

Ｑ　一　Ｑ，　一１，２，ｍ　

信息分配策略采用文献［４］中的方法，根据局部滤波器的估计精度因子来选定。　

系统信息全部保留在子滤波器中。　

ｆ　＝ｄｉａｇ（￣Ｈ，　２…　）　

ｌ　＝ｄｉａｇ（　１，　ｚ…　）　式中，　（１２）　取　＋　一１，　一０，　

ｐＨ　Ｄ　￣ｌｉ－－　，　一　（　１，２，…　）　

式中，　、　称为局部滤波器的估计精度因子；　，Ｐ　分别代表Ｐ　，Ｐ。的第ｉ行第ｉ列的元素。　（１３）

　潍坊学院学报　２０１１年１２月　

（２）各子滤波器与主滤波器进行时间修正吲　

一１一Ｏｋ肚一ｌ　女一ｌ＋ｎ弓女　Ｐ　一】一Ｌ（志）Ｏｋ／＾一ｌ　Ｐ＾一１　Ｏｋ５＾一ｌ＋ｎＱ女ｒ女　

Ｌ（志）＝ｄｉａｇ［Ａｌ（￣），　２（女），…，．＝Ｉ　（女）］　式中，　

ｆ口　Ｃ　（ｍＣｋ＞１）　ＡＲｋ）一１　１　（口　Ｃ＾≤１）　

ｒ．一！　［　］．　一　＂　∑ａ　Ｍ（点）　

ｆ　Ｅ：ｏ￡　（　一０）　㈤一｛（　，０　＜１）　ｌ　ｌ下．Ｄ　。　

Ｎ（是）一Ｖｏ（　）一　一Ｈ＾Ｑ女一ｌ　Ｈ　Ｍ（　）＝＝０＾　一ｌ　Ｐ＾…ｌ《　一１　Ｈ　Ｈ＾　（３）各子滤波器进行量测修正　

一　ｌ＋Ｋ女￡＾　

Ｐ女一（Ｊ—Ｋ女Ｈ　）Ｐ＾雎一】　（４）主滤波器进行数据融合　把各子滤波器得到的状态估计　（志）、　。（　）及其协方差Ｐ　（七）、Ｐ　（　）进行融合。　

波采用了信息分配原理，子滤波器的结果之间相互独立，主滤波器的数据融合算法为　Ｐ７　一Ｐ　＋Ｐ　（１５）　

（１６）　

（１７）　

（１８）　

（１９）　

（２０）　

（２１）　

（２２）　

（２３）　

（２４）　

因Ｃａｒｌｓｏｎ联邦滤　

Ｐｌ＿　，＝Ｐ　一　义Ｉ＋Ｐ　２　

通常将方差阵的逆Ｐ　称为信息矩阵，它描述了第ｉ个子滤波器的估计所含信息量的大小。　

则第ｉ个子滤波器对估计的精度越差；反之，Ｐｆ越小，第ｉ个子滤波器对估计的精度越高。　２．４系统的三层容错设计　（２５）　

（２６）　Ｐ　越大，　

实际测试表明，ＧＰＳ接收机有时会给出错误的位置、速度数据。因此，如果在ＧＰＳ数据进入卡尔曼滤　

波器之前，不将错误的ＧＰＳ数据剔除，则经反馈校正后。便污染了整个系统。为此，系统进行了三层容错　设计，即数据粗大误差的剔除与隔离、子滤波器误差估计值的故障检测、主滤波器误差估计值的故障检测。　

（１）速度数据粗大误差判断　若根据式（２７）计算出的　大于１０　ｍ／ｓ（假设ＧＰＳ速度最大跳变为５ｍ／ｓ），则ＧＰＳ受到干扰，速度出　

现突变，上标Ｅ、Ｎ、Ｕ表示东、北、天方向。　

Ｖ　＝｛［（　Ｐｓ。一‰）一（　。－＿Ｖ　Ｓ０）］　＋［（　一‰）一（Ｖ　一　）］ｚ　＋［（　Ｐｓ１一‰）一（　ｓ　一　ＳＩ＿Ｉ）］　）　（２７）　

（２）子滤波器的故障检测　

对于各子滤波器，前后两次的误差估值不应发生非常大的突变。如果有大的突变，则认为卡尔曼滤波　器已受到第一层未检测出的故障所污染，其滤波结果不送到主滤波器，并重新初始化该子滤波器。采用残　

差　。检验法［６　进行子滤波器误差估计值的故障检测。　

（３）主滤波器的故障检测　

对于主滤波器，同样前后两次的误差估值不应发生非常大的突变。如果有大的突变，则认为主滤波器　

已受到前两层未检测出的故障所污染，本次融合结果不再反馈到各子滤波器，并且不对ＭＳＩＮＳ进行反馈　校正。同样采用状态　。检验法进行主滤波器误差估计值的故障检测。　

３滤波算法在ＭＳＩＮＳ／ＧＰＳ浅组合中的应用仿真　

３．１　仿真轨迹及仿真条件的设置　设仿真轨迹由平飞、转弯、加速、爬升等状态组成，仿真轨迹中的载体先在高度为１０００米的高空上由　

一１１　２

—　第６期　马云峰：容错联邦强跟踪卡尔曼滤波算法在组合导航中的应用研究　

东北方向匀速飞行，接着加速飞行，然后拐弯向正东方向匀速飞行，最后爬升到一定高度匀速飞行。　．　

仿真条件：系统噪声　是由陀螺仪和加速度计敏感误差的白噪声部分构成的。给定陀螺仪和加速度　

计的参数为：光纤陀螺仪常值漂移１０。　，随机噪声均方差为１ｏ。／ｈ；石英挠性加速度计常值误差０．５ｍｇ，随　

机误差均方差为０．１ｍｇ，ＧＰＳ的位置误差为均方差１５ｍ的白噪声，速度误差为均方差０．１ｍ／ｓ的白噪声，　

两个滤波器的状态初始值均为０。当３００秒至４００秒期间，ＧＰＳ东向速度发生±４ｍ／ｓ的跳变。　

３．２仿真结果　

有东向速度故障情况下的集中卡尔曼滤波估计误差曲线和联邦强跟踪卡尔曼滤波器的滤波误差曲线　

如图２和图３所示。　

０　－ｏ．５　一ｌ　—Ｉ．５　一‘　－２．５　—３　－３．５　－４　－４．５　时Ｉ＇ｕｌ（秒）　ａ．俯仰角误差　

时间（秒）　Ｃ．航向角误差　

ｅ．北向速度误差　时间（秒）　ｂ．横滚角误差　

０　

—０　－０　＿（】　＊—０　．（）　－０　—０　—０　ｄ．东向速度误差　

时间（秒）　ｆ．天向速度误差　

图２有速度跳变故障时集中式卡尔曼滤波误差曲线　４结论　

（１）在正常情况下，联邦强跟踪卡尔曼滤波器的滤波精度与集中式卡尔曼滤波器的滤波精度相当。　

（２）集中式卡尔曼滤波器对初值的选取较敏感，若初值的选取与实际相差较大，将导致滤波器的发散，　而联邦强跟踪卡尔曼滤波器对初值选取敏感度较低。　

（３）当ＧＰＳ速度发生跳变故障后，集中式滤波器立即发散。这说明，在有故障发生时，集中式卡尔曼　

滤波器易受污染。　

一一１１　３—　５　３　５　２　５●

世＊