基于自适应Kalman滤波算法的联邦滤波器(1)

图ｌ 自适应Ｋａｌｍａｎ滤波算法的联邦滤波器计算框图 图中带黑圈的序号代表了其执行顺序，小括号
（）代表其需要计算的式子，线上的向量和矩阵表示 之间流动的信号。其中序号①步骤也可以放在最后 做，即序号⑦步骤之后。
３ 仿真算例
以某再入飞行器仿真弹道参数为真实数据，其 表示为地心坐标系下的位置和速度向量［Ｘ，ｙ，Ｚ， ｙ：，ｙ，，亿］。系统状态方程为：
此时式（１０）的ｚ。、Ｈ应修改为各传感器相应的Ｚｉ、 Ｈ，，状态的一步预测式（１７）修正为：
Ｘ＾。Ｉ～１一ＡＸ＾一ｌ＋ＢＵ＾一ｌ＋吼一１
（２６）
量测更新式（２２）修正为：
Ｘ缸一Ｐ｛ｌ＾Ｐ缸一ｌＸ缸一ｌ＋Ｐ缸Ｈ’Ｒ｛一１ ７（Ｚ｛一
“一１），歹＝１，２，…，Ⅳ
（２７）
可以看出，基于自适应Ｋａｌｍａｎ滤波算法的联
（１０）
【尺；＝生≯尺。一，＋｝（２。一以）（２。一“）ｒ
ｆ仉＝Ｘ¨一Ｘ¨一１
ｆ． 正一１．
．１
＿｛吼一］厂吼一ｌ十百仇
（１１）
【Ｑ。一生尹Ｑ。一。＋｝（仇一乱）（仇一磊）丁
２ 使用自适应Ｋａｌｍａｎ滤波算法的联 邦滤波器
联邦滤波器ｍ２１对子滤波器的估计信息合成，子 滤波器是平行结构形式，均使用Ｋａｌｍａｎ滤波算法。 同时采用方差上界技术和信息分配原则来消除各传 感器子滤波器估计结果的相关性，把全局状态估计 信息和系统噪声信息分配给各子滤波器，但不改变 子滤波器的算法形式。
摘 要：为弥补传统联邦滤波器实用性不好，缺乏对对象模型和传感器噪声的自适应估计能力的缺陷，利用自适应 Ｋａｌｍａｎ滤波算法的思想，结合联邦滤波器本身的算法结构，对联邦滤波器进行改进，使之具有很强的自适应性，能够自适应 地计算出模型噪声和传感器噪声的协方差阵。给出了基于自适应Ｋａｌｍａｎ滤波算法的联邦滤波器的计算架构，其他优秀的自
控制矩阵，仉（户×１）为已知的控制输入向量，玑（，ｚ× １）为系统噪声向量，根据式（２）可知，其为器对式（１）进行独立测量，相应的测
量方程为：
乙＝ＨＸ·＋ｎ
（３）
其中，乙为（ｇ×１）的传感器测量值，Ｈ（ｇ×，２） 为传感器测量矩阵。ｎ（口×１）为传感器测量噪声，其
统计特性为：
Ｘ蚪１—４＾Ｘ＾＋ＢｌＵ＾＋仇 其中：Ｘ＾一（ｚＩ，挑，ｚ＾，ｙ１，ｙ，。，ｙ～）丁，
Ａ＝［：？］，Ｂ一宦］，ｕ。＝匿三 ，玑一
ｌ矿又是）一一象ＩＤｙｙｚ—ｇｚ＋Ⅳｚ
．｛矿，＠）一一易ＩＤｙｙ，一ｇ，＋眠
厅
【矿ｚ（正）一一器ｌＤ矿ｕ一＆＋Ⅳｚ
式中，ｐ为弹道系数，舒为地表重力加速度，＆， ｇ，，承为重力分量，ＪＤ为大气密度，Ⅳ，，Ⅳ，，Ⅳ：为加 速度随机扰动，即模型噪声‘副。
Ｋ＾一尸¨一１日：１［Ｈ＾Ｐ¨一ｌＨ｝＋Ｒｌ—１］＿１ （７）
预测误差协方差阵：
Ｐ＾，＾一１＝ＡＩ—ｌＰ＾一１，＾一ｌＡＩ一１ｒ＋Ｑ＾一１
（８）
估计误差协方差阵：ＰＭ一［ｊ—ＫｔＨｔ］Ｒ卜。（９）
系统噪声方差和传感器噪声方差的自适应估
计：
ｆ２＾＝日Ｘ蹦一ｌ—Ｚ＾
Ｊ．“｛“一一字］ｒ秕＾一。ｌ＋十丢ｉ么乞‘ｔ
基于白适应Ｋａｌｍａｎ滤波的联邦滤波器算法调 整如下：式（２５）之后使用式（１１）进行系统噪声估计。 请注意此处寇Ｉ。为融合后的状态估计值。同理在各
万方数据
·９０·（总第３３—１７８４）
火力与指挥控制
２００８年第１２期
子滤波器的测量更新前使用式（１０）计算传感器噪声
方差，即在式（１９）之后使用式（１０）计算。需要注意，
为节省篇幅，此处给出Ｘ方向噪声曲线，如图２ 所示。其他方向情况大致一样。
Ｂ陉
仉为加速度分量，分别为：
田２ ｘ方向噪声曲线
其中，‘＊’数据为原始噪声曲线，‘一’为本文提 出的基于自适应Ｋａｌｍａｎ滤波算法的联邦滤波器滤
Ｋｅｙ ｗｏｒｄｓ：ｆｅｄｅｒａｌ ｆｉｌｔｅｒ，Ｋａｌｍａｎ ｆｉｌｔｅｒ，ａｄａｐｔｉｖｅ
引言
在现代导航、数据融合等系统中，Ｋａｌｍａｎ滤波 技术是一种非常有效的手段。但是，在应用Ｋａｌｍａｎ 滤波技术时，需要知道被研究对象的数学模型和传 感器噪声统计的先验知识；采用不精确的数学模型 和传感器噪声统计特性设计Ｋａｌｍａｎ滤波器可能导 致较大的状态估计误差，甚至造成滤波发散。而采用 自适应Ｋａｌｍａｎ滤波技术就可以解决以上问题，现 在已经有许多学者在这方面做了很多工作，而且也 已提出很多实用的自适应Ｋａｌｍａｎ滤波方法‘引。
（１５） （１６）
戈＾ｌＩ—ｌ—Ａ又＾一ｌＩＩ—ｌＡ了’ＢＵｌ—ｌ
（１７）
Ｐ｛压Ｌ１一［ＡＰ｛－。肛一ｌＡ丁＋Ｑ｛一１］～，＿『一１，２，…，Ｎ，，，ｌ
（１８）
Ｘ｛Ｉ＾一ｌ—Ｘ＾Ｉ＾一ｌ，歹＝１，２，…，Ⅳ，，，ｚ
（１９）
量测更新：
鼎百１一Ｐ｛Ｉｊ—ｌ＿１＋目口Ｒ｛一１＿１Ｈ’，＿『＝１，２，…，Ⅳ
１ 自适应Ｋａｌｍａｎ滤波算法
考虑线性系统模型，假设其状态方程为：
ＸＨｌ—ＡＸＩ＋ＢＵ＾＋仉
（１）
Ｅ［仉］一ｏ，Ｅ［仉玑＋』ｒ］一ＱＩ以
（２）
肖龙远，等：基于自适应Ｋａｌｍａｎ滤波算法的联邦滤波器
（总第３３—１７８３） ·８９·
其中，瓦为咒×１维系统状态向量，Ａ（咒×，１）为
已知的系统状态转移矩阵，Ｂ（ｎ×户）为已知的输入
Ｅ［ｙ｛］＝ｏ，Ｅ［ｙｉ，ｙ｛＋ｚ］＝Ｒｉ艿。，Ｅ［Ｖ｛‰］＝ｏ
（１３）
按照联邦滤波器的信息分配原理和状态重置
有：
Ｐ｛一ｌＩＩ—ｌ－１一岛Ｐ暑１ｌ＾一１，Ｑｉ一１－１一房ＱＩ—ｌ－１（１４）
Ⅳ
风一１一∑佛，又｛一。Ｉ。一。：爻。一。ｌ。一，，．『一１，２，…，
Ⅳ，所 时间更新： Ｐ订｛一１一［ＡＰＩ—ｌｌ＾一１Ａ丁＋ＱＩ—１］＿１
ＸＩＡＯ Ｌｏｎｇ—ｙｕａｎ，ＺＥＮＧ Ｃｈａｏ （Ｊ，ｌｓ￡ｉｆ“抬ｏ，Ｅ如ｃｆ，口以站Ｅ理ｇｉ捍Ｐｇｒｉ行ｇ，（Ｍｉ，ｚ以ＡｃｎｄＰ搠岁。，Ｅ咒ｇｉ，２Ｐｅ一，ｚｇ Ｐ缈ｓｉｃｓ，＾矗口，拶口雄ｇ ６２１９００，Ｃ＾ｉ咒口）
Ａｂｓｔｒａｃｔ：Ｔｏ ｓ０１ｖｅ ｔｈｅ ｔｒａｄｉｔｉｏｎａｌ ｆｅｄｅｒａｔｅｄ ｆｉｌｔｅｒ’ｓ ｓｈｏｒｔ ｏｆ ｐｒａｃｔｉｃａｂｉｌｉｔｙ，ｌａｃｋ ｏｆ ｔｈｅ ａｂｉｌｉｔｙ ｔｏ ａｄａｐｔ ｔｏ ｏｂｊｅｃｔ ｍｏｄｅｌ ａｎｄ ｎｏｉｓｅ ｏｆ ｓｅｎｓｏｒｓ，ｂａｓｅｄ ｏｎ ｔｈｅ ｉｄｅａ ｏｆ ａｄａｐｔｉＶｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ａｌｇｏｒｉｔｈｍ．Ｃｏｍｂｉｎｅ ｔｈｅ ａｌｇｏｒｉｔｈｍ ｏｆ ｔｈｅ ｆｅｄｅｒａｔｅｄ ｆｉｌｔｅｒ，ｔｈｅ ｆｅｄｅｒａｔｅｄ ｆｉｌｔｅｒ ｉｓ ｉｍｐｒｏｖｅｄ ｔｏ ｂｅｃｏｍｅ ｍｏｒｅ ａｄａｐｔｉｖｅ，ｃａｎ ａｄａｐｔｉｖｅｌｙ ｃｏｍｐｕｔｅ ｔｈｅ ｃｏＶａｒｉａｎｃｅ ｍａｔｒｉｘ ｏｆ ｔｈｅ ｍｏｄｅｌ ａｎｄ ｓｅｎｓｏｒｓ ｎｏｉｓｅ． Ａｎｄ ｔｈｅ ａｌｇｏ“ｔｈｍ ｆｒａｍｅｗｏｒｋ ｏｆ ｔｈｅ ｆｅｄｅｒａｔｅｄ ｆｉｌｔｅｒ ｂａｓｅｄ ｏｎ ｔｈｅ ａｄａｐｔｉｖｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ａｌｇｏｒｉｔｈｍ ｉｓ ｐｒｅｓｅｎｔｅｄ， ｏｔｈｅｒ ｐｅｒｆｅｃｔ ａｄａｐｔｉｖｅ ａｌｇｏｒｉｔｈｍ ｃａｎ ｂｅ ａｄｄｅｄ ｔｏ ｔｈｉｓ ｆｒａｍｅｗｏｒｋ． Ｔｈｅ ｅｆｆｉｃｉｅｎｃｙ ｏｆ ｔｈｉｓ ａｌｇｏｒｉｔｈｍ ｉｓ ｖａｌｉｄａｔｅｄ ｂｙ ｔｈｅ ｓｉｍｕｌａｔｉｏｎ．
Ｖ０１．３３。Ｎｏ．１２ Ｄｅｃｅｍｂｅｒ，２００８
文章编号：１００２一０６４０（２００８）１２—００８８一０３
火力与指挥控制 Ｆｉｒｅ Ｃｏｎｔｒｏｌ ａｎｄ Ｃｏｍｍａｎｄ ＣｏｎｔｒｏＩ
第３３卷第１２期 ２００８年１２月
基于自适应Ｋａｌｍａｎ滤波算法的联邦滤波器
肖龙远，曾超 （中国工程物理研究院电子工程研究所，四川 绵阳６２１９００）
当传感器数量增加时，通常采用Ｎ．Ａ．Ｃａｌｓｏｎ
收稿日期：２００７—１１—１０
修回日期：２００７—１２—２５
作者简介：肖龙远（１９８２一 ），男，四川绵阳人，硕士研究
生，主要研究方向为：数据融合、信号处理，嵌
入式系统等。
万方数据
等［１］（１９８８年）提出的联邦滤波器来解决标准 Ｋａｌｍａｎ滤波器计算负担重、通信负担重、容错性差 的困难。但是联邦滤波器仍然会面临数学模型和传 感器噪声统计特性不精确的困难，具体表现就是过 程噪声协方差阵Ｑ和传感器噪声协方差阵Ｒ需要 采用自适应手段进行计算。将自适应Ｋａｌｍａｎ滤波 技术的处理方法扩展到联邦滤波器中就非常必要。 本文以Ｓａｇｅ提出的自适应Ｋａｌｍａｎ滤波算法为例 讨论其在联邦滤波器中的使用方法。同理，其他自适 应Ｋａｌｍａｎ滤波算法也可以通过相同的方式在联邦 滤波器中使用。
邦滤波器实际上是在各个子滤波器中增加了对各传 感器噪声的估计和在主滤波器中增加了对系统噪声
的估计两个部分，而对联邦滤波器的滤波结构没有
任何改变，所以其最优性仍可以保证。 基于自适应Ｋａｌｍａｎ滤波算法的联邦滤波器结
构图（此处以２个子滤波器为例）如下：
硝一ｌＩ·－Ｉ’Ｑｌ—Ｉ，癍一ＩＩ＾一ｌ（１‘，１５）
Ｅ［ｙ。］一ｏ，Ｅ［ｙ＾ｙＨｌ］一Ｒ。ｄ，，Ｅ［ｙ。７。］一ｏ（４） 即ｎ也是不相关的白噪声，并且与系统噪声不相
关。
对Ｓａｇｅ—Ｈｕｓａ的自适应滤波算法‘４３进行简化如
下：
状态一步预测：
Ｘｔ．１一ｌ—Ａ＾一ｌＸＩ一１＋及一Ｉ矾一Ｉ
（５）
状态估计：
又¨＝戈Ｍ—ｌ＋ＫＩ［Ｚｔ—日Ｉ戈¨一１］
（６）
滤波增益矩阵：
表ｌ各分量噪声方差
分量
Ｘ
ｙ
Ｚ ｙｚ ｂ ｙｚ
传感器各分量 原——始．噪～一 声．方．差．
８．５８ ２．７７ ７．３８ ２．２８ １．３２ １．４８ 。。‘