文章编号:1008-8652(2005)01-001-004联合卡尔曼滤波器在数据融合中的应用胡宏灿1,2 郭 立1 朱俊株1(1.中国科学技术大学 合肥 230026; 2.海军大连舰艇学院 大连 116018) 【摘要】 介绍多传感器数据融合中联合卡尔曼滤波器的设计步骤,并将此方法用于舰船组合导航系统,计算机仿真和理论分析表明,该滤波器可以做到全局最优,其结构遵循信息分配原则,提高了系统的数值稳定性和容错性,减小了数据传输的工作量与计算量,便于计算机实现,能够满足组合导航系统需要。
关键词:组合导航系统;数据融合;联合卡尔曼滤波中图分类号:T P391.7 文献标识码:AThe Application of Federal Kalman Filter in Data Fusion SystemHu Ho ng can1,2 Guo Li1 Zhu Junzhu1(1.University of Science and T echnology of China H ef ei230026;2.Dalian N av al Vessels A cademy Dalian116018)Abstract:A new design o f Kalman filter based on data fusion is presented in the paper.Fistly,the fr ame Kalman filter is intr oduced.T hen,the algo rithm is given.T he simulatio n results show that the metho d is useful in integr ated navigation sy stem because it can impr ove accur acy and r eliability,and it has hig h fault-tolerant ability.Keywords:integ rated nav igatio n sy stem;data fusio n;feder al Kalman filter1 引言数据融合技术是近年来新兴的一门实践性较强的技术,它是对系统多个传感器的数据进行处理的过程。
众所周知,由于任何传感器都有自身的不足之处,所以单一传感器具有误报风险大,可靠性和容错能力低等缺点。
为了对测量环境或对象的特征有个全面、正确的认识,克服单一传感器的上述缺点,多传感器数据融合技术应运而生。
简单的说,多传感器融合技术就是融合多个传感器的信息,以产生比单个传感器更可靠、更准确的信息。
常用方法有贝叶斯估计法和DS证据理论法及经典推算法等,神经网络、小波分析等智能方法近年来也是研究数据融合的重要方法和手段。
卡尔曼滤波器自上世纪六十年代被提出以后,作为一种新型的滤波手段在控制、跟踪、测量领域得到广泛应用。
由于卡尔曼滤波器对数据的估计是无偏最优估计,滤波器结构简单等特点,使得卡尔曼滤波器在多传感器数据融合中应用极为广泛。
过去使用的集中式卡尔曼滤波器要集中处理所有传感器的数据,计算量大,实时性差,并且不具备容错性。
本文基于Car lson提出的联合卡尔曼滤波算法,介绍了利用信息分配原则实现多传感器信息最优融合的滤波器的设计,不仅使系统具备了一定的容错能力,实时性也有较大幅度的提高。
最后给出了联合卡尔曼滤波器在舰船组合导航中的应用实例。
2 联合卡尔曼滤波器的设计步骤联合卡尔曼滤波器的设计主要围绕两个方面,第一是对数据进行分散处理,第二是分散处理过的数据X收稿日期:2004-11-26图1 联合卡尔曼滤波器结构简图再进行全局融合。
具体做法是在众多传感器中选择一个信息全面,可靠性有保证的传感器作为参考传感器,使之与其他传感器两两联合,形成若干对数据进行分散处理的子滤波器,各局部滤波器根据状态方程和测量方程进行滤波,输出建立在局部测量基础上的最优估计结果X i ,P i ,(i =1,2,……n );这些局部最优估计再在全局滤波器内按照融合算法合成,从而获得全局估计X m ,P m ,每个滤波阶段完成后,全局滤波器将全局结果X m 和按信息分配原则形成的协方差矩阵P m向各局部滤波器进行信息反馈,来保证系统有较好的容错性。
基于这一思想,可以得出联合卡尔曼滤波器的结构,见图1。
2.1 局部和主滤波器算法局部状态方程:X i (k +1)=5(k )X i (k )+#(k )W (k ) i =1,2,3,…n(1) 局部测量方程:Z i (k )=H i (k )X i (k )+V i (k ) i =1,2,3,…n (2) 式中,5(k )为时变状态转移矩阵(但是对定常线性系统,如在A -B 滤波中它是常数阵);X (k )是k 时刻的状态向量;#(k )是系统干扰矩阵;w (k )是模型噪声向量,可看作是零均值的高斯白噪声,且有方差阵Ew (k )w (k )′=Q K (Q K >0)。
式中Z (k )为k 时刻的观察向量;H (k )为观察矩阵;V (k )是测量噪声向量。
假定为零均值高斯白噪声,有方差矩阵Ev k v ′k =R K (R K >0)。
局部滤波器算法:X d i (k +1/k )=5(k )X d i (k /k )(3)P i (k +1/k )=5(k )P i (k /k )5(k )T +#(k )Q (k )#(k )T(4)K i (k +1)=P i (k +1/k )H i (k +1)(H i (k +1)P i (k +1/k )H i (k +1)T +R )-1(5)X d i (k +1/k +1)=X d i (k +1/k )+K i (k +1)(Z i (k +1)-H i (k +1)X d i (k +1/k ))(6)P i (k +1/k +1)=(I -K i (k +1)H i (k +1))P i (k +1/k )(7)(3)-(7)式为局部滤波器的递推算法,式中K (k +1)为k +1时刻的增益阵,P i (k +1/k +1)是k +1时刻状态向量的协方差阵的滤波值,Xd i (k +1/k +1)是k +1时刻的状态滤波值。
主滤波器算法:X m =P m (P -11X 1+P -12X 2+…+P -1n X n )(8)P m =(P -11+P -12+…+P -1n )-1(9)式中的X m ,P m 是全局最终的滤波值。
B i 是信息分配系数。
根据以上公式,可以得到软件算法流程如图2所示。
2.2 算法中各数据阵的作用和确定状态转移矩阵和系统误差矩阵由系统模型确定;测量矩阵由状态向量和观测向量确定。
以上算法中关键环节是Q 、R 、B i 的确定。
在卡尔曼滤波器的应用中除了尽可能精确的描述动态方程和测量方程外,一个重要问题就是对上述三者的选取。
Q 对滤器精度有直接的影响,动态模型越是不精确这种影响就越大,如果Q 值选取过大,则滤波在过去观测量上的加权衰减就过快,导致的后果是滤波不能很好的利用已有的测量信息,从而降低了滤波器的精度;反之,如果Q 值选取过小,使滤波在过去观测量上的衰减过慢,随着滤波的递推,将会引进越来越大的模型噪声,从而使滤波误差越来越大,这就是通常说的滤波发散现象。
所以Q 的选取至关重要,基本原则是使Q 的值与系统精度相匹配。
对于满足完全可控和完全可测的定常系统,当滤波器递推充分多的步数后,增益矩阵K k +1将趋于常数阵K ,增益矩阵达到稳态的快慢取决于Q /R 的比值(这里可理解为系统的信噪比),此值越大,增益阵达到稳态就越快,当采样间隔一定时,增益阵的大小也唯一依赖于Q /R 的比值。
所以系统的测量方程所描述的测量环节的精度,也一定要和系统匹配得当。
B i 的选取是整个滤波图2 卡尔曼算法流程器的又一重大环节,通常的做法是取0<B i <1的实数,具体的方法有平均分配法即:B i =1/n (i =1,2,3,…n ),和对角矩阵法即:B i (k )=diag (a 1,a 2,a 3,…,a n )a j =P i (k /k )j j P 1(k /k )jj +P 2(k /k )jj +…+P n (k /k )jj可以证明:∑n i =1B i =I ,满足信息守恒原则。
3 联合卡尔曼滤波器在IN S /GPS /LO RAN -C 组合导航中的应用舰船上的惯性导航陀螺仪大多采用平台式结构,当陀螺以水平阻尼方式工作时,其误差方程可表示为:D a U IN S D a K IN S D a K INS =00-8cos U 8tg U 008sec U 00D U IN S D K IN S D K IN S+1000-sec U 00tg U 1E e E n E z (10) 上式中U 为舰船所处的纬度,8是地球自转角速度,D K INS 为舰船航向误差角,D UINS ,DK INS 分别是舰船所在位置的纬经度误差,E n ,E e ,E z 分别为北向东向和方位陀螺的漂移率。
对于长期航行的舰船来说,惯性导航的主要误差源就是陀螺的北向东向和方位陀螺的漂移。
通过对陀螺特性的研究可得到陀螺漂移模型可表示为阶段常值加一阶高斯-马尔可夫过程[3],即:E i =E c i +E s i 。
根据系统误差方程可构造状态变量为:X =[D U INS ,D K INS ,D K INS ,E s e ,E s n ,E s z ,E c e ,E c n ,E c z ](11)状态方程为:Xa (t )=F (t )X (t )+GW (t )(12)式中F (t )是9×9的时变系数矩阵,它由惯性误差方程即方程(10)和陀螺漂移模型确定;W (t )是由w E c ,w E n ,w E z 组成的白噪声矢量,方差强度为常值Q ,G 是系统误差矩阵。
INS 和GPS 构成子滤波器LF 1,取系统状态矢量等同全局状态矢量即式(11),状态方程等同全局状态方程,测量周期为300s ,观测量为INS 和GPS 位置之差,即:Z 1(t k )=U INS (t k )-U GPS (t k )K INS (t k )-K GPS (t k )(13) 观测方程可表示为:Z 1(t k )=H 1X 1(t k )+V 1(t k )(14)式中H 1=100000000010000000(15) V 1(t k )为GPS 输出信息的噪声序列,是方差为R 1的高斯白噪声矢量。
第1期胡宏灿等 联合卡尔曼滤波器在数据融合中的应用图3 组合导航系统中联合 卡尔曼滤波器同理可得INS 和Loran C 组成的子滤波器LF 2,状态矢量和状态方程等同全局状态矢量和全局状态方程;测量方程为:Z 2(t k )=H 2X 2(t k )+V 2(t k )(16)Z 2(t k )取INS 和Lo ran C 的航向之差,所以式中:H 2=001000000(17) V 2(t k )为Lo ranC 输出噪声误差序列,是方差阵为R 2的噪声矢量。