GPS_INS组合导航中的自适应滤波算法

GPS ΠINS 组合导航中的自适应滤波算法Ξ宋迎春1,2, 陈正阳1(1.中南大学信息物理工程学院,湖南长沙410083;2.湖南人文科技学院数学系,湖南娄底417000)[摘要]　研究了G PS ΠINS 导航系统中的不确定噪声问题,给出了一种基于协方差匹配技术的自适应卡尔曼滤波算法,该法当测量噪声的协方差已知时可以自适应估计出系统噪声的协方差.它是在滤波过程中通过判定系统噪声的协方差是否发生了变化,若它发生了变化再估计出新的协方差,然后再按得到的噪声统计估计值计算新息序列的协方差阵,因此有效地提高了滤波结果的精度并消除了滤波发散现象.数字仿真表明效果良好.关　键　词:G PS ΠSINS 导航;卡尔曼滤波;自适应滤波;统计检测;协方差匹配中图分类号:T N965 文献标识码:A 文章编号:10005900(2006)02009504Adptive Filtering Technology in GPS ΠINS I ntegrated N avigationSONG Ying chun 1,2, CHEN Zheng -yang 1(1.School of In fo Physics Engineering ,Central S outh University ,Changsha China ;2.Department of M ath.,Hunan University of Humanities and Science and T echnology ,Loudi 417000China )【Abstract 】　The problem of G PS ΠI NS navigation systems with uncertain noise is considered.An adaptive com putationmethod of K alman filter based on covariance matching technique is introduced.The covariance of the system noise can beestimated accurately by using this method when the covariance of the measurement noise is known.In filtering ,it can seeif there is any change occurred to the covariance of the system noise ,and if there is any it estimates the new covariance ,and uses the obtained statistical estimation of the noise to com pute the covariance matrix of the posterior sequence ,thuscan m odify predicted states and eliminates the filter divergence problem.And finally the numerical simulation shows thatit gives g ood effect.K ey w ords :　G PS ΠI NS navigation ;K alman filtering ;adaptive filter ;statistical testing ;covariance matching标准的卡尔曼滤波(SK F )已经被广泛地使用在G PS ΠI NS 组合导航系统中.我们知道标准的卡尔曼滤波(SK F )要求系统模型和相关噪声的统计性质是预先准确知道的,然而在许多实际情形中这种假设并不成立[1].在G PS ΠI NS 组合导航系统中,温度的变化会导致惯性测量产生误差[2],G PS 测量误差也会随环境而变化.在这些情形下,动态模型和相关误差的统计性质不可能是不变的.对于测量噪声和动态噪声的统计性质不够了解,一方面会导致滤波结果不精确或者把偏差加到状态估计里面去,另一方面将导致滤波发散[3].如,对于测量噪声协方差R (k )和动态噪声协方差Q (k ),如果给定它们太小,会使模型的补偿不足,增大了又会使模型的滤波精度降低甚至于发散.另外,不充分的噪声统计将影响滤波中的某些观测量(加速度计,陀螺漂移)的估计.由于一些复杂的关系,这些估计又影响主要的状态变量(位置,速度,姿态)的估计.不充分的噪声统计和变化的估计环境严重影响着G PS ΠI NS 组合导航系统的准确性.针对噪声统计特性的不确定,学者们提出了一些自适应卡尔曼滤波方法[4].Jazwinski (1969)提出了模型方差自适应补偿法[5],即在K alman 滤波过程中,利用观测信息,自适应地估计模型误差的协方差矩阵.Mehra (1970)提出了利用新息序列的自适应估计开窗逼近法[6],即要求观测方程协方差阵及状态误差协方差阵随时自适应于观测信息[7,8].这种自适应滤波一般称为Sage -Husa 自适应滤波[9].它的基本原理是利用前m 步残差序列估计观测噪声协方差阵和状态噪声协方差阵,然而,Sage -Husa 的自适应算法有一些缺陷和局限性,如果使用不当,有可能导致不理想的结果.表面上看起来,当系统噪声方差阵Q 和测量噪声方差阵R 未知时,Sage -Husa 的自适应算法可以把Q 和R 同时估计出来,而且相当多的人员在工作中也确实是这么做的.然而事实上Sage -Husa 法不能在Q 和R 均未知时把它们估计出第28卷第2期2006年6月 湘　潭　大　学　自　然　科　学　学　报Natural Science Journal of X iangtan University V ol.28N o.2Jun.2006Ξ收稿日期:20051204 基金项目:国家自然科学基金资助项目(40574003);教育部博士点基金资助项目(20050533057b ) 作者简介:宋迎春(1964),男,湖南双峰人,博士,副教授来[10].杨元喜(2004)[11]提出了利用经系统误差修正后的观测向量和状态预测向量及相应的协方差矩阵,再进行动态导航滤波计算,能有效提高导航解的精度.本文基于协方差匹配技术的自适应卡尔曼滤波算法,提出了一种系统噪声方差变化的检测与校正方法,该法当R已知时可以自适应地估计出Q.它的独特之处在于原理上易于理解,在实际中也很容易实现.它是通过判定系统噪声的协方差是否发生了变化,若它发生了变化再估计出新的协方差,然后再按得到的噪声统计估计值计算新息序列的协方差阵,因此提高了滤波的精度并消除了滤波发散现象.1　常规卡尔曼滤波方程G PSΠI NS组合导航中,卡尔曼滤波常用离散化模型来描述系统.设系统状态方程和观测方程分别为:X(k+1)=Φ(k+1,k)X(k)+Γ(k)W(k)(1)Z(k)=H(k)X(k)+V(k)(2)其中,X(k)表示系统n维状态向量,Z(k)是m维观察向量,{W(k)},{V(k)}是均值为零,分别为{Q(k)},{R(k)}的相互独立的正态白噪声序列.对于这样一组模型方程,状态向量X(k)的最佳估计可以由下面的卡尔曼滤波方程组给出:^X(k|k+1)=<(k,k-1)^X(k-1|k-1)(3) ^X(k|k)=^X(k|k-1)+K(k)v(k)(4) v(k)=Z(k)-H(k)^X(k|k-1)(5) K(k)=P(k|k-1)H T(k)[H(k)P(k|k-1)H T(k)+R(k)]-1(6) P(k|k-1)=Φ(k,k-1)P(K-1|k-1)ΦT(k,k-1)+Γ(k-1)Q(k-1)ΓT(k-1)(7) P(k|k)=P(K-1|k-1)-K(k)H(k)P(k|k-1)(8)方程(3)至(8)是一组递推式,当已知初始无偏估计^X(0|0)及其误差协方差矩阵P(0|0)后,就可以通过这组递推方程组获得每个历程的最佳线性无偏估计^X(k|k),^X(k+1|k)及其误差协方差P(k|k -1)、P(k|k),这组递推式称为卡尔曼滤波器.2　动态噪声方差变化的检测在G PSΠI NS导航解算中,我们可以认为动态模型及测量的统计特性是可信的(否则,可运用自校正方法对模型进行补偿和对R(k)进行估计).这样当滤波发生异常时,主要原因来自动力学模型不正确和动态噪声Q(k)的统计不清楚.下面,假定测量模型及测量的统计特性是可信的,来讨论滤波异常的检测.记tk 时刻,实际的动态噪声方差阵为Qr(k),滤波器装定的动态噪声方差阵为Qm(k).作假设H0∶Q r=Q m(k), H1∶Q r<Q m(k), H2∶Q r>Q m(k). 我们知道,在卡尔曼滤波的假设条件下,^X(k|k)为X(k)的无偏最小方差估计,即^X(k|k)= E{X(k)|ζ(k)},这里ζ(k)={Z(1),Z(2),…,Z(k)}.当滤波的条件不完全满足时,滤波不具有最小方差这种优良性质,且有滤波的发散现象出现.故当H成立时,从v(k)=Z(k)-H(k)^X(k|k-1),易知v(k)～N(O,H(k)P(k|k-1)H T(k)+R(k))且{v(k)}为白噪声序列.又E{v(k)v T(k)}=H(k)P(k|k-1)H T(k)+R(k)(9)于是,χm (k)=v T(k)[H(k)P(k|k-1)H T(k)+R(k)]-1v(k)～χ2m,即χ2m服从具有m个自由度的χ2分布,m是观测方程的维数.设H0对H1的虚警概率为PF1,H0对H1的虚警概率为PF2.即PF1=P{χ2m(k)<TH1|H0}=1-P{χ2m(k)≥TH1|H0}(10)PF2=P{χ2m(k)>TH2|H0}(11) 又设ξk,N={v(k),…,v(k+N-1)}容易验证:P{ξk,N|Q r(k)}关于统计量χ2m(k)具有单调似然比.引进假设H′0和H″:69 湘　潭　大　学　自　然　科　学　学　报 2006年对于H ′0∶Q r ≥Q m (k ),H 1∶Q r <Q m (k )存在一致最优势(UM P )检验方案:若χm (k )<TH 1,接受假设H 1;若χ2m (k )≥TH 1,则接受假设H ′0=H 0∪H 2.对于H ″0∶Q r ≥Q m (k ),H 2∶Q r <Q m (k ),存在一致最优势(UM P )检验方案:若χm (k )>TH 1,接受假设H 2;若χ2m (k )≤TH 1,则接受假设H ″0=H 0∪H 2.综合上述两组假设检验方案,得到若TH 1≤χ2m (k )≤TH 2,接受假设H 0.若χ2m (k )<TH 1,则接受假设H 1.若χ2m (k )>TH 2,则接受假设H 2.上述检测方案易于实现,不过运用一个观测资料v (k )构造检验统计量缺乏统计意义,我们可以运用观测资料{v (k ),…,v (k +N -1)}.它们是独立的,于是χ2(N )=6N -1i =0v T k +1(k )[H (k )P (k |k -1)H T (k )+R (k )]-1v (k +i )(k )～χ2mN 此处,χ2mN 服从具有mN 个自由度的χ2分布,同样可以应用χ2-检验假设.综上所述,本检测方案如下:1)给定N 和PF 1和PF 2,由χ2分布表,查得TH 1,TH 22)在卡尔曼滤波过程中,每隔l (l ≥1)步,求得统计量χ2的观察值.3)如果接受了H 0,则称滤波是自适应的并且保持动态噪声方差阵为Q m (k )不变.若拒绝H 0则对Q m (k )进行校正.3　系统噪声的协方差校正当动力学模型有误差时,人们常常以增大或减小系统噪声的协方差Q (k -1)的取值以补偿模型的误差,使达到自适应的目的.我们可以利用上面所述的滤波适应性检测方法,易于构成Q (k -1)的自适应估计.当我们检测结果接受H 1时应使Q (k -1)减小,设协方差Q (k -1)变为 Q (k -1)-Q ′(k -1),其中 Q (k -1)为滤波检测前的系统噪声协方差,Q ′(k -1)为检测后需要减小的量.由(7)有P (k |k -1)= P (k |k -1)-Γ(k -1)Q ′(k -1)ΓT (k -1)(12)其中P (k |k -1)=Φ(k ,k -1)P (k -1|k -1)ΦT (k ,k -1)+Γ(k -1) Q (k -1)ΓT (k -1)(13)又由(9)和(12)有E {v (k )v T (k )}=H (k ) P (k |k -1)H T (k )+R (k )-H (k )Γ(k )Q ′(k -1)ΓT (k )H (k )(14)现用C =1N 6N -1i =0v (k -i )v T (k -i )作为E {v (k )v T (k )}的估计,有H (k )Γ(k )Q ′(k -1)ΓT (k )=[H (k ) P (k |k -1)H T (k )+R (k )]-C (15)由上式,我们可以得到Γ(k )^Q ′(k -1)ΓT (k )=[H T (k )H (k )]-1H T (k ){[H (k )P ′(k |k -1)H T (k )+R (k )]-C }H (k )[H T (k )H (k )]-1(16)当我们检测结果接受H 2时,应使Q (k -1)增加.即Q (k -1)= Q (k -1)+Q ′(k -1),因此P (k |k -1)= P (k |k -1)+Γ(k -1)Q ′(k -1)ΓT (k -1)(17)同样有Γ(k )Q ′(k -1)ΓT (k )=[H T (k )H (k )]-1H T (k ){C -[H (k )Q ′(k |k -1)H T (k )+R (k )]}H (k )[H T (k )H (k )]-1(18)这样我们可以利用(12)和(16)或(17)和(18)来计算出P (k |k -1),从而求出自适应滤波解.4　数字仿真飞行轨迹按照实际复杂的高动态飞行而设计.飞机的初始位置为东经132°,北纬28°,高度1000m.79第2期 宋迎春 G PS ΠI NS 组合导航中的自适应滤波算法 从0s 开始,飞机以初始速度50m Πs ,匀速平直向前飞行10s ,加速向前飞行50s ,匀速平直前飞行10s ,拉起2s ,爬行50s ,改平2s ,匀速平直前飞行10s ,飞机进入右拐弯1s ,右拐弯50s ,改平1s ,匀速平直前飞行10s ,进入俯冲2s ,持续俯冲50s ,改平2s ,飞机匀速平直前飞行30s.整个飞行过程280s ,滤波周期设置为6s ,PF 1=PF 2=0.1.滤波器的初始状态方差设置如表1所示,系统噪声和观测噪声如表2所示.表2　系统及量测噪声方差T ab.2　System and measurement noises 噪声方差 设定值位置噪声方差东,北向1000m 2,天2000m 2速度噪声方差东,北向1(m Πs )2,天1.44(m Πs )2姿态噪声方差100(secm in )2陀螺随机漂移1°Πh 加速度计零偏4310-4g 伪距噪声方差400m 2伪距率噪声方差0.01(m Πs )2表1　滤波器的初始状态值设置T ab.1　Setup of the in itial state of filte 初始状态 设定值位置误差1000m 速度误差2m Πs 姿态误差100’陀螺零漂1°Πh 加速度计零偏10-4g 接收机时钟偏差1000m 接收机时钟漂移3m Πs 图1,2,3表示惯导北向的位置,速度,加速度偏差的滤波结果.其中虚线表示标准卡尔曼滤波(SK F )结果,实线为经过改进了滤波(MK F)结果.图1　北向位置误差 图2　北向位置速度误差Fig.1　P osition error of an aircraft in N orth Fig.2　Velocity error of an aircraft in N orth图3　北向位置加速度误差Fig.3　Accelerometer error of an aircraft in N orth(下转第106面)参　考　文　献[1]　吉学东,张树军,张勇,等.小型交流电力测功机系统的研制[J].摩托车技术,2004(12):10-13.J I Xue-dong,ZHANG Shu-jun,ZHANG Y ong,et al.R&D of Small AC E lectrodynam ometer System[J].M otorcycle T echnology.2004(12):10 -13.[2]　张桂香,罗乾超.交流电力测功机的方案研究[J].铁道学报,1999(2):102-104.ZHANG G ui-xiang,LUO Qian-chao.Research on Design of Alternating Current E lectric Dynam ometers[J].Journal of the China Railway S oci2 ety.1999(2):102-104.[3]　K eig o W atanabe.A fuzzy-gaussian neural netw ork and its application to m obile robot control[J].IEEE T rans.C ontrol Systems T echnology,1996,4(2):193-199.[4]　M auer G F.A fuzzy logic controller for an ABS Braking System[J].IEEE T rans Fuzzy Systems,1995,3(4):381-388.[5]　Sudkam p R J.H Hammel Interpolation,com pletion,and learning fuzzy rules[J].IEEE T rans Syst M an Cybern,1994,24(2):332-342[6]　刘向杰,柴天佑,张焕水.三维模糊控制器的研究[J].自动化学报,1998,24(2):230-235.LIU X ing-jie,CHAI T ian-y ou,ZHANG Huan-shui.S tructure Analysis of Three Dimensional Fuzzy C ontroller[J].Acta Automatica S inica, 1998,24(2):230-235.(上接第98面)参　考　文　献[1]　Schaffrin B.G enerating robustified K alman Filter for the integration of G PS and INS[R].T ech Rep15,Institute of G eodesy,University of S tutt2gart,1991.[2]　T itterton D H,W eston J L.S trapdown inertial navigation technology[R].Peter Peregrinus Ltd,The Institution of E lectrical Engineers,London,1997.[3]　T oda N F,Schlee F H,Obsharsky P.Regions of K alman Filter convergence for several autonom ous navigation m odes[R].AIAA G uidance,C ontrol,and Flight Dynam ics C on f,Huntsville,A L14-16August,1967.[4]　M ohamed A H,Schwarz K P.Adaptive K alman Filtering for INSΠG PS[J].Journal of G eodesy,1999,73:193-203.[5]　JazW inski A H.M athematics in Science and Engineering[C]∥S tochastic Processes and Filtering Theory.New Y ork and London:Academ ic Press.1969,64.[6]　K och K R and Y ang Y.R obust K alman filter for rank deficient observation m odel[J].Journal of G eodesy,1998,72:436-441.[7]　M ohamed A H and Schwarz K P.Adaptive K alman filtering for INSΠG PS[J].Journal of G eodesy,1999,73:193-203.[8]　W ang J,S tewart M P and Tsakiri M.Adaptive K alman filtering for integration of G PS with G LONASS and INS[A].IUGGΠIAG Brim ingham[C].August18-31,1999.[9]　Sage A P,Husa G W.Adaptive Filtering W ith unknown Prior S tatistic[J].Joint Automatic C ontrol C on ference,1969(3):760-769.[10]　Zhang Changyun.Approach Adaptive Filtering Alg orithm[J].Acta Aeronautica et Astronautica S inica,1998,19(7s):40-44.[11]　Y ang Y uan-xi,Zhang Shuang-cheng.Fittings of Systematic Error and C ovariance M atrices in Navigation[J].Acta G eodaetica et CartographicaS inica.2004,33(3):189-194.[12]　Huang Fengzhao,Peng Y unxiang.Investigation on Integrated G PSΠSINS Navigation System W ith Pseudorange(deltarange)coupled[J].Journal ofChinese Inertial T echnology.1998,6(2):1-9.。