新型自适应Kalman滤波算法及其应用

合导航，既可保证滤波的自适应性，又有利于提高系 统的实时性。
２ 自适应Ｋａｌｍａｎ滤波算法的应用
通过在惯导／双星组合导航系统中应用，对本文 提出的自适应Ｋａｌｍａｎ滤波算法与常规Ｋａｌｍａｎ滤
波和Ｓａｇｅ－Ｈｕｓａ自适应滤波作了进一步比较。 ２．１惯导／双星组合系统数学模型
１．２简化的Ｓａｇｅ－－Ｈｕｓａ自适应Ｋａｉｍａｎ滤波
针对式（１）、（２），简化的Ｓａｇｅ－Ｈｕｓａ自适应滤
波算法的数学模型Ⅲ 又（愚）一西（志，ｋ一１）完（惫一１）＋Ｋ（五）可（量） （３）
口（尼）＝Ｚ（点）一Ｈ（忌）Ｘ（志Ｉ惫一１）
（４）
Ｋ（愚）＝Ｐ（志Ｉｋ一１）日Ｔ（是）Ｉ－Ｈ（ｋ）Ｐ（惫Ｉ志一１）×
１ 自适应Ｋａｌｍａｎ滤波的理论
１．１ 问题的提出
考虑一时变离散系统模型嘲
Ｘｔ＝蛾Ｉ卜１Ｘ卜ｌ＋ｎ—ｌＷｔ一１
（１）
Ｚ（志）＝日（惫）Ｘ（忌）＋ｙ（志）
（２）
式中 墨∈Ｒ”为ｋ时刻的系统状态；饥忙。∈Ｒ一×一
为从ｋ一１到ｋ时刻的一步状态转移矩阵；ｎ一。∈
Ｐｎ为五时刻的系统噪声矩阵；Ｈ（愚）∈Ｒ“。“为系统
量测矩阵；ｙ（愚）”为量测噪声向量；Ⅳ。一，∈Ｒｒ为ｋ
时刻的系统噪声；Ｚ（忌）∈Ｒ４为系统ｋ时刻的观测
收稿日期：２００８—０６—１６ 基金项目：航空基础科学基金资助项目（２００７０８５１０１０；２００９０８１８００４） 作者简介：赵龙（１９７６一），男（满族）·内蒙古赤峰市人。副教授·博士，研究方向：精确导航、图形图像匹配导航和视觉导航。
由于我国自主研发的北斗双星定位系统的定位 信息是由地面中心站解算并通过卫星转发给用户， 所以无法得到准确的噪声及误差模型。因此，采用 常规Ｋａｌｍａｎ滤波实现惯导／双星组合导航系统时， 其滤波效果较差，甚至有发散的趋势［１］。Ｓａｇｅ和 Ｈｕｓａ提出了一种自适应滤波算法，可在线计算系统 噪声Ｑ和量测噪声Ｒ［２］。但实际上，Ｓａｇｅ－Ｈｕｓａ自 适应滤波算法并不能同时将Ｑ和Ｒ估计出来，且文 献Ｅ３］已证明了该算法只能在Ｑ已知时估计出Ｒ， 或Ｒ已知时估计出Ｑ。文献［４－Ｉ提出了一种简化的
Ｋｅｙ ｗｏｒｄｓ：ａｄａｐｔｉｖｅ ｆｉｌｔｅｒｉｎｇ；Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ；ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ
在动态系统的数学模型和噪声统计特性已知 的情况下，利用Ｋａｌｍａｎ滤波进行多种导航传感器 信息融合可实现飞行器的精确导航／制导。但在实 际应用中很难得到系统精确的数学模型和噪声的统 计特性，使滤波精度降低甚至产生滤波发散现象。 为了解决此问题，通常采用自适应滤波技术，在滤波 的同时，利用观测数据的信息不断地在线估计和修 正模型参数、噪声统计特性以提高滤波的精度。
Ｓａｇｅ－Ｈｕｓａ自适应滤波算法，在已知Ｑ的情况下，对 Ｒ进行估计，该算法仍具有良好的自适应性。但由 于每次滤波都估计Ｒ，滤波的复杂度增加，实际应用 时，将无法保证系统的实时性。本文利用文献［５］中 判断滤波异常的判据，提出了一种新的自适应Ｋａｌ— ｍａｎ滤波算法，并将其应用在惯导／双星组合导航 系统中。
１
ｔ
ｅ＝击∑口ｉ口ｊ
（１１）
ｌｏｋ－－Ｍ十１
由式（９）～（１１）可得滤波严格收敛判据为
万方数据
口（志）ＶＴ（志）一Ｈ（ｋ）Ｐ（ｋＩ五一１）Ｈ７（志）＋袁（惫～１）
（１２）
式（１２）进一步整理得
％ｒ（愚）＝ｔｒ（Ｃ。）／ｔｒ（Ｐ。）＝１
（１３）
式中Ｖａｒ（正）为判断滤波状态的因子（简称滤波状态
因子），Ｖａｒ（愚）＝１为滤波严格收敛，Ｖａｒ（愚）≠１为 滤波异常。
小；当Ｖａｒ（愚）一１时，Ｃ（点）＝１，此时变为常规Ｋａｌ－
ｍａｎ滤波。 １．４ 自适应滤波与常规Ｋａｌｍａｎ滤波的比较
在噪声统计特性未知时，与常规Ｋａｌｍａｎ滤波
相比，简化的Ｓａｇｅ．Ｈｕｓａ自适应滤波和本文的自适
应Ｋａｌｍａｎ滤波的精度和可靠性更高；两者的计算 复杂度也比常规Ｋａｌｍａｎ滤波的高。这是因为两种
航系统阶数较高时，计算量增加更多。这在系统采
样周期较短的情况下，无法满足系统对实时性的要
求。
１．３自适应Ｋａｌｍａｎ滤波算法
文献［５］中的基本思想是：实际的余项应与其理
论特性“相匹配”，即在滤波的同时检验实际的余项，
并判断滤波是否出现异常。当实际的余项在原假设
Ｑ（足一１）、Ｒ（ｋ一１）下不相容时，则对Ｑ（ｋ）、Ｒ（走）进
（１８） （１９）
式中ｃ（忌）＞ｏ为在线调整的实数。 当Ｖａｒ（矗）＞１或Ｖａｒ（志）＜ｌ时，为使Ｖａｔ（愚）
值很快回到１附近，本文选用指数函数构造ｃ（ｋ）的
值：
Ｃ（最）一ｅｗ７（‘’～１
（２０）
式中 当Ｖａｒ（最）＞１时，且Ｖａｒ（志）越大时，ｃ（ｋ）也 越大；当Ｖａｒ（愚）＜１时，且Ｖａｒ（点）越小时，ｃ（ｋ）也越
２ ｍ２＋２ｍ
减法／次
２ｍ２＋３
１
Biblioteka Baidu
指数函数／次０
１
从表ｌ可见，在每一滤波周期内，本文提出的 自适应Ｋａｌｍａｎ滤波比常规Ｋａｌｍａｎ滤波增加了
ｍ２＋优次乘法运算、２次除法运算、ｍ２＋２ｍ次加法 运算、１次减法运算和１次指数函数运算。而该算 法和简化Ｓａｇｅ—Ｈｕｓａ自适应滤波相比，减少了ｍ３＋ ３ｍ２卵＋３ｍ２＋７２２ｍ—ｍ次乘法运算、ｍ３＋３ｍ２咒＋
Ｋ（忌一１）］７十日（志）Ｐ（ｋ－－１）ＨＴ（忌）｝ （８）
式中ｄ。一（１—６）／（１一ｂｋ＋１），Ｏ＜６＜１为遗忘因
子，通常在０．９５～０．９９问选取；Ｒ（点）为系统量测噪
声的方差矩阵；Ｋ（是）为增益矩阵。
简化的Ｓａｇｅ＿Ｈｕｓａ自适应滤波在每次滤波过
程中都要计算Ｒ（惫），计算量较大，特别是当组合导
对于式（１３），当Ｗａｒ（ｋ）＞１或Ｖａｒ（志）＜１时，
表明系统的量测噪声增大或减小，通过调节食（志），
使其增大或减小，保持Ｖａｒ（志）在１附近变化，进而 实现滤波的适应性。因此，在常规Ｋａｌｍａｎ滤波算 法的基础上，可得量测噪声在线调整的自适应Ｋａｌ—
ｍａｎ滤波算法的公式为
Ｘ（ｋ）一西（ｋ，ｋ一１）Ｘ（忌一１）＋Ｋ（惫）ｖ（ｋ） （１４）
第３１卷第６期 ２００９年１２月
压电与声光 ＰＩＥＺＯＥＩ。ＥＣＴＲＩＣＳ ８Ｌ ＡＣＯＵＳＴ００ＰＴＩＣＳ
文章编号：１００４—２４７４（２００９）０６—０９０８—０４
Ｖ０１．３１ Ｎｏ．６ Ｄｅｃ．２００９
新型自适应Ｋａｌｍａｎ滤波算法及其应用
赵 龙，吴 康
（北京航空航天大学控制一体化技术国家级重点实验室。北京１００１９１）
自适应滤波需在线估计或调整量测噪声的方差阵， 即在每个滤波周期内都对量测噪声方差阵进行调
整，简化的滤波要计算式（８）和ｄ。，本文的滤波要计 算式（１０）、（１１）、（１３）和（２０）（其中式（１０）在常规
Ｋａｌｍａｎ和简化的Ｓａｇｅ＿Ｈｕｓａ自适应滤波中也进行 计算，因此在比较时将其忽略）。
针对式（１）、（２）的系统方程（系统状态ｘ（五）为
Ａｂｓｔｒａｃｔ：Ｉｎ ｏｒｄｅｒ ｔｏ ａｖｏｉｄ ｔｈｅ ｄｉｖｅｒｇｅｎｃｅ ａｎｄ ｉｍｐｒｏｖｅ ｔｈｅ ｒｅａｌ—ｔｉｍｅ ｐｒｏｐｅｒｔｙ ｏｆ ｔｈｅ ｆｉｌｔｅｒ，ａｎ ａｄａｐｔｉｖｅ Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎｇ（ＡＫＦ）ａｌｇｏｒｉｔｈｍ ｉｓ ｐｒｅｓｅｎｔｅｄ。ｗｈｉｃｈ ｏｂｔａｉｎｅｄ ａ ｆａｃｔｏｒ ｏｆ ｆｉｌｔｅｒｉｎｇ ｓｔａｔｅ ｂｙ ｕｓｉｎｇ ｔｈｅ ｃｒｉｔｅｒｉｏｎ ｏｆ ｆｉｌｔｅｒｉｎｇ ａ— ｎｏｍａｌｉｅｓ，ａｎｄ ｔｈｅ ｍｅａｓｕｒｅｍｅｎｔ ｎｏｉｓｅ ｃｏｖａｒｉａｎｃｅ ｍａｔｒｉｘ ｉｓ ｃｏｎｆｉｒｍｅｄ ｂｙ ｕｓｉｎｇ ｔｈｉｓ ｆａｃｔｏｒ．ＡＦＫ ｉｓ ｒｅａｌｉｚｅｄ ｂｙ ｃｈａｎ— ｇｉｎｇ ｔｈｅ ｍｅａｓｕｒｅｍｅｎｔ ｎｏｉｓｅ ｃｏｖａｒｉａｎｃｅ ａｃｃｏｒｄｉｎｇ ｔｏ ｔｈｅ ｓｔａｔｅ ｏｆ ｔｈｅ ｆｉｌｔｅｒｉｎｇ．ＡＫＦ ｉｓ ａｐｐｌｉｅｄ ｔｏ ｔｈｅ ＩＮＳ／ＤＳ（Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ／Ｄｏｕｂｌｅ－ｓｔａｒ Ｓｙｓｔｅｍ）ｉｎｔｅｇｒａｔｅｄ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ，ａｎｄ ｃｏｍｐａｒｅｄ ｉｔ ｗｉｔｈ ｔｈｅ ｃｏｎｖｅｎｔｉｏｎａｌ Ｋａｌ— ｍａｎ ｆｉｌｔｅｒｉｎｇ ａｎｄ ｓｉｍｐｌｉｆｉｅｄ Ｓａｇｅ－Ｈｕｓａ ｆｉｌｔｅｒｉｎｇ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗｅｄ ｔｈａｔ ｔｈｅ ＡＫＦ ｓｉｍｐｌｉｆｉｅｄ ｔｈｅ ｃｏｍｐｕ· ｔａｔｉｏｎ ａｎｄ ｉｍｐｒｏｖｅｄ ｔｈｅ ｒｅａｌ—ｔｉｍｅ ｐｒｏｐｅｒｔｙ ｗｉｔｈ ｔｈｅ ｓａｍｅ ａｃｃｕｒａｃｙ ｏｆ ｓｉｍｐｌｉｆｉｅｄ Ｓａｇｅ－Ｈｕｓａ ｆｉｌｔｅｒｉｎｇ．
以维向量，量测量为ｍ维向量），与常规Ｋａｌｍａｎ滤 波相比，两种自适应滤波算法在每一滤波周期内增
加的计算量如表１所示。
９１０
压电与声光
表１ 两种自适应滤波算法增加的计算量
法则 乘法／次
言裟象
∥＋３ｍ２九＋４ｍ２＋ｎ２ｍ
本文的自适 应滤波算法
ｍ２＋ｍ
除法／次 加法／次
ｌ ｍ３＋３ｍ２，ｚ＋竹２仇一ｍ行一ｍ
关键词：自适应滤波ＩＫａｌｍａｎ滤波；组合导航系统
中图分类号ｉＶ２４９．３２＋８
文献标识码：Ａ
Ａ Ｎｅｗ Ｔｙｐｅ ｏｆ Ａｄａｐｔｉｖｅ Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎｇ Ａｌｇｏｒｉｔｈｍ ａｎｄ Ｉｔｓ Ａｐｐｌｉｃａｔｉｏｎ
ＺＨＡＯ Ｌｏｎｇ，ＷＵ Ｋａｎｇ
（Ｎａｔｉｏｎａｌ Ｋｅｙ Ｌａｂ．ｏｆ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ ｏｎ Ｉｎｔｅｇｒａｔｅｄ Ｃｏｎｔｒｏｌ Ｔｅｃｈｎｏｌｏｇｙ。Ｂｅｉｊｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ａｅｒｏｎａｕｔｉｃｓ ａｎｄ Ａｓｔｒｏｎａｕｔｉｃｓ。Ｂｅｉｊｉｎｇ １００１９１，Ｃｈｉｎａ）
摘要：为防止滤波发散和提高系统的实时性，提出了一种新的自适应Ｋａｌｍａｎ滤波算法。该算法利用滤波异 常判据获得一个滤波状态因子，通过滤波状态因子确定量测噪声协方差阵的值，在线调整噪声的统计特性实现自 适应滤波。将该算法应用到惯导／ｇｇ星组合导航系统中，并和常规Ｋａｌｍａｎ滤波和简化的Ｓａｇｅ－Ｈｕｓａ自适应滤波算 法进行仿真比较。仿真结果表明，在滤波精度与简化Ｓａｇｅ－Ｈｕｓａ自适应滤波相当的情况下，新算法简化了运算，提 高了实时性。
豫２ｍ一优２一ｍｎ一３ｍ次加法运算和２ｍ２＋２次减法 运算；增加了１次除法和１次指数函数的运算。因 此，文本的自适应Ｋａｌｍａｎ滤波算法再增加了一定
的计算量后即可实现滤波的自适应，比简化Ｓａｇｅ－ Ｈｕｓａ自适应滤波的计算量小，特别是系统维数越
高，节省的计算量也越多。在组合系统误差模型和 噪声统计特性未知时，利用本文提出的算法进行组
万方数据
第６期
赵龙等：新型自适应Ｋａｌｍａｎ滤波算法及其应用
９０９
值。
当系统是时变的且噪声的统计信息未知时，此
时常规Ｋａｌｍａｎ滤波的滤波精度会降低，甚至可能
发散。为克服此问题，在对Ｓａｇｅ－Ｈｕｓａ自适应滤波
研究的基础上，利用文献ｒ－５］中的滤波异常判据给出
一种量测噪声在线调整的自适应Ｋａｌｍａｎ滤波。
行估计来取代原来假设的Ｑ（ｋ一１）、Ｒ（忌一１）。其
常用判据为
ｕ１（愚）口（愚）＞ｙ￡ｒ（［－Ｅ（ｖ（ｋ）ｖＴ（愚）］）
（９）
式中ｙ＞１为储备系数；ｔｒ为矩阵的迹；臼（愚）为信
息序列。
口（忌）方差的理论值为
Ｐ。＝Ｅ［口（志）可Ｔ（点）］＝日（忌）Ｐ（忌Ｉ南一１）ｘ
Ｈ１（惫）＋袁（五一１）
（１０）
。（忌）的实测方差为
Ｈ１（足）＋Ｒ（忌）］一１
（５）
Ｐ（五Ｉｋ一１）一面（矗，点一１）Ｐ（愚一１）咖Ｔ（忌，ｋ一１）＋
Ｆ（ｋ，ｋ一１）Ｑ（忌）ｒＴ（忌，ｋ一１） （６） Ｐ（愚）＝［１－－Ｋ（ｋ）Ｈ（ｋ）］Ｐ（ｋｌｋ一１）×
［Ｊ—Ｋ（志）Ｈ（志）］Ｔ
（７）
Ｒ（愚）一（１一ｄ＾）Ｒ（是一１）＋巩｛［Ｊ—Ｈ（五）×
Ｋ（忌一１）］ｕ（愚）ＶＴ（足）［Ｊ—Ｈ（是）×
口（忌）一Ｚ（忌）一Ｈ（愚）叉（足Ｉ愚一１）
（１５）
Ｋ（愚）＝Ｐ（ｋＩ ｋ一１）ＨＴ（志）［Ｈ（矗）Ｐ（志Ｉｋ一１）×
ＨＴ（忌）＋袁（忌一１）］一１
（１６）
Ｐ（ｋＩ愚一１）一西（五，ｋ一１）Ｐ（ｋ一１）０７（最，忌一１）＋
ｒ（是，忌一１）Ｑ（七），（志。ｋ一１）（１７）
ｐ（志）＝（Ｊ—Ｋ（志）日（最））Ｐ（志Ｉ七一１） 袁（忌）一ｃ（愚）袁（忌——１）