變增益聯邦KF組合導航定位算法研究

張怡，趙凱華，姚斌

西北工業大學電子信息學院，西安 710072

變增益聯邦KF組合導航定位算法研究

張怡，趙凱華，姚斌

西北工業大學電子信息學院，西安 710072

1 引言

GPS導航系統雖然具有定位精度高，誤差不隨時間累積，良好的通用性等優點。但是易受到外界的干擾以及多徑效應的影響，使得GPS系統在某些信號強度低、干擾大的場合導航效果表現的并不理想。INS導航系統可以很好地解決這個問題，但INS導航系統的誤差會隨時間積累。因此，用INS輔助GPS進行導航定位越來越受到青睞，INS/GPS組合后可以大大提高系統的綜合性能，適應各種復雜環境下的定位導航[1-3]，并且緊耦合組合方式在整體性能上較松耦合組合方式有較大提高[4]。

濾波算法在衛星導航定位中發揮著非常大的作用，針對GPS/INS組合導航系統導航定位算法，已有大量研究工作[5-7]。在傳統的GPS/INS緊耦合組合導航系統中，由于偽距和多普勒頻移誤差的存在，系統存在一定的誤差偏移。針對這種誤差偏移，為進一步提高GPS/INS緊耦合組合導航算法的定位精度，以聯邦卡爾曼濾波為基礎的GPS/INS緊耦合組合導航定位算法，針對偽距測量誤差對GPS/INS組合導航系統定位精度的影響，對偽距進行預濾波，提高了定位精度[8]。

傳統的卡爾曼濾波是無限增長記憶的濾波，某時刻的最優估計要用到該時刻以前的觀測數據。因此，隨著步數的增大，濾波值中的老數據比重太大，而新數據比重太小，當系統存在模型誤差和計算誤差時，系統預測誤差方差陣逐漸趨于零，新的觀測數據對修正狀態估計的作用太小，不能有效地抑制誤差對狀態估計值的影響，從而導致濾波發散。為解決這個問題，Fagin與Sorenson等人提出了衰減記憶濾波算法，該算法通過引入衰減因子限制卡爾曼濾波器的記憶長度，提高新量測值的利用權重從而抑制濾波發散[9]。

聯邦卡爾曼濾波算法雖然提高了定位精度，隨著濾波步數的增加，同樣存在發散問題。本文研究了一種改進的聯邦卡爾曼濾波算法，通過深入研究衰減記憶濾波算法，引入一種可變加權系數來持續地增加誤差狀態估計中新量測值的修正作用。仿真表明，該算法能夠進一步提高組合導航系統的定位精度，有效抑制了濾波的發散作用，對工程應用有一定參考價值。

2 變增益聯邦卡爾曼濾波器設計

2.1 濾波器結構設計

組合導航系統的主要誤差來源就是GPS接收模塊解算出的偽距誤差，為削弱該誤差帶來的影響，運用一種二級卡爾曼濾波器進行導航定位解。濾波器結構如圖1所示，在數據送到主卡爾曼濾波器進行融合前先對偽距進行預濾波，削弱偽距誤差對整個系統定位精度的影響[8]。由于INS系統短時間定位精度較高，為降低整個系統的復雜程度，不對慣導信息進行預濾波。在完成預濾波之后，將經過預濾波的偽距信息和由INS得到的慣導信息導入到主濾波器中，兩路數據在主濾波器中進行信息數據融合。

圖1 聯邦卡爾曼濾波器結構圖

2.2 狀態方程的建立

為了全面反映載體運動狀態的變化，并充分考慮載體運動中的位置、速度、加速度等，狀態向量設為：

式中，δr為位置誤差矢量，δν為速度誤差矢量，δε為姿態角誤差，δω為陀螺儀零偏，δa為加速度計零偏；假設δω、δa為常值偏差。

系統狀態誤差方程為[10]：

式中，Fi是比力向量fi的反對稱矩陣，代表由載體坐標系到慣性坐標系的方向余弦矩陣，Ni代表引力系數矩陣。

將該連續的狀態方程離散化，即可得到離散系統的卡爾曼濾波狀態方程：

2.3 量測方程的建立

在系統量測方程中，若直接采用偽距與多普勒頻移測量殘余作為觀測量，則觀測量中存在著本地時鐘與衛星時鐘的鐘差。因此，將相對于不同衛星得到的偽距與多普勒頻移測量殘余相減[11]，得到偽距與多普勒頻移單差：

2.4 主卡爾曼濾波器濾波方程的建立

在主濾波器中，采用應用卡爾曼濾波算法作為數據融合算法。卡爾曼濾波首先利用上一個狀態的狀態量，通過狀態方程估計出這一個狀態下量測量的值，完成預測過程，然后利用預測過程得到的信息量以及系統的觀測量即可完成卡爾曼濾波的校正過程[11]，濾波方程為：

理論上，聯邦卡爾曼濾波隨著時間的推移，觀測數據數量不斷增多，濾波估計的精度應當越來越高。但是，在實際工程應用中，系統模型描述和噪聲統計特性的誤差會使得系統預測誤差方差陣隨著濾波時間的推移逐漸趨于零，從而狀態估計會過分依賴舊量測值，新量測值對預測值的修正作用下降，導致濾波發散，影響系統定位精度。因此，可以通過逐漸降低舊量測值的權值，相應增大新量測值的權值，從而抑制濾波的發散。

式（10）中，增益矩陣取時刻k=N，則：

經分析，逐漸減小N時刻以前的KN值，相對地突出KN，便能在一定程度上抑制濾泡發散，考慮增大Pk及P0的值來減小Zi(i＜N)及對的影響。在Pk/k-1方程中引入可變權值系數：(1-sk)/(1-s)，(0＜s＜1)，可以看出隨k值增大，權值系數會不斷增大并大于1，這樣便可保證對新量測值的利用，于是Pk/k-1方程可以改寫為[12]：

本文采用偽距與多普勒頻移測量殘余為觀測量，量測方程為非線性方程，需要采用擴展卡爾曼濾波的方法來進行濾波。改進后的變增益聯邦卡爾曼主濾波器解算過程如下所示[13-15]。

3 仿真結果與分析

為驗證變增益聯邦卡爾曼濾波算法相對于原始聯邦卡爾曼濾波算法對導航系統定位精度的改善效果，本文采用Matlab進行一組仿真測試，初始位置設為：（1 545 203.817 775 705，4 245 412.597 793 822，4 487 631.251 578 394），初始速度為（0，0，0），初始方向余弦矩陣為{1 0 0；0 1 0；0 0 1}。

載體軌跡仿真如圖2所示。由圖2可知，模擬載體經歷了加速、爬升、滾動、轉彎等多個過程，涵蓋了現有大部分的運動狀態。

圖2 載體仿真軌跡

原始組合導航算法的位置誤差和速度誤差仿真如圖3所示；原始聯邦卡爾曼濾波算法與改進后的聯邦卡爾曼濾波算法分別應用在GPS/INS緊耦合組合導航系統后，對于位置誤差和速度誤差的仿真如圖4和圖5所示。

圖3 原始組合導航位置和速度誤差

圖4 位置誤差對比

圖5 速度誤差對比

從圖4可以明顯看出，在X、Y、Z軸方向上位置偏差均有一定的減小，Z軸方向上的位置偏差減小量最為明顯，這說明聯邦卡爾曼濾波算法利用緊耦合方法計算偽距與多普勒頻移單差可以有效提高組合導航系統的整體性能，其中Z軸上的改善更加明顯。并且還可以看出，改進后的變增益聯邦卡爾曼濾波算法位置誤差減小量更大，說明改進的變增益卡爾曼濾波算法在加大對新量測值的利用權值后，能有效抑制系統的發散，可以進一步提高系統的定位精度。

從圖5可以明顯看出，兩種聯邦卡爾曼濾波算法均能有效減小速度誤差，并且本文的變增益聯邦卡爾曼濾波算法在速度定位上的誤差相對于傳原始的聯邦卡爾曼濾波算法有了一定的提高。另外，從圖中速度誤差的偏移還可以看出，當載體位置出現較大變化時，系統可以很快速地修正這個速度偏差。這主要是由于本文方法在主濾波器之前，已經完成了修正量的計算，大大減小了運算量，因此可以快速地修正速度誤差偏移。

4 結束語

提出了一種變增益聯邦卡爾曼濾波算法，以二級聯邦卡爾曼濾波器為基礎，通過研究衰減卡爾曼濾波算法，引入變增益加權系數。針對偽距測量誤差對于GPS/INS組合導航系統定位精度的影響，利用子濾波器對GPS接收模塊解算出的偽距數據先進行預濾波，然后在主濾波器數據融合過程中引進變增益加權系數，加大新量測值對預測值的修正作用。本文算法吸收了聯邦卡爾曼濾波算法的優點，考慮了最新量測值。仿真結果表明，該算法能有效改善濾波性能，提高導航系統的定位精度，抑制濾波發散。

[1]Li Yong，Wang Jinling.Low-cost tightly coupled GPS/INS integration based on a nonlinear Kalman filtering design[C]// Proceedings of IONNTM Conference，Monterey，CA，2006.

[2]Kim K，Park C G.INS/GPS tightly coupled approach using an INS error predictor[C]//Proc of GNSS 18th International Technical Meeting of the Satellite Division，Long Beach，CA，2005.

[3]Dikshit VG，MahapatraP R.Medium-coupledbusbased INS/GPS sensor fusion for accurate and reliable positioning[C]// Proceedings of ESAV Conference，Capri，Italy，2008.

[4]趙思浩，陸明泉，馮振明.EKF與UKF在緊耦合組合導航系統中的應用[J].系統工程與電子技術，2009，31（10）：2450-2454.

[5]向禮.非線性濾波方法及其在導航中的應用研究[D].哈爾濱：哈爾濱工業大學，2009.

[6]楊柳慶，肖前貴，牛妍.基于漸消卡爾曼濾波器的定位系統設計[J].南京航空航天大學學報，2012，44（1）：134-138.

[7]宋清昆，劉小磊.神經網絡修正動態GPS卡爾曼濾波算法研究[J].計算機工程與應用，2011，47（15）：152-155.

[8]何偉，廉保旺，馮曉明.基于聯邦卡爾曼濾波的GPS/INS緊耦合組合導航定位算法[J].火力與指揮控制，2012，36（8）：149-152.

[9]李楊，胡柏青.基于改進衰減記憶濾波的組合導航算法研究[J].計算機仿真，2012，29（3）：117-119.

[10]董緒榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科學技術大學出版社，1998：245-248.

[11]謝剛.GPS原理與接收機設計[M].北京：電子工業出版社，2009.

[12]王虎，王解先，白貴霞.改進的漸消卡爾曼濾波在GPS動態定位中的應用[J].同濟大學學報，2011，39（1）：124-127.

[13]吳志華，丁楊斌，申功勛.改進的非線性魯棒EKF算法及其應用[J].計算機工程與應用，2011，47（3）：207-209.

[14]Guo L，Ljung L.Exponential stability of general tracking algorithms[J].IEEE Tran on Autom Contr，1995，40：1376-1387.

[15]Lippiello V，Siciliano B，Villani L.Adaptive extended Kalman filtering for visual motion estimation of 3D objects[J].Control Engineering Practice，2007，15：123-134.

ZHANG Yi,ZHAO Kaihua,YAO Bin

School of Electronics and Information,Northwestern Polychechnical University,Xi’an 710072,China

The pseudorange error worked out by the GPS receiver module is the major errors of GPS/INS navigation system, which can be weakened by using one federal Kalman filter algorithm of integrated navigation.This algorithm has two Kalman filters.The first Kalman filter filters the pseudo range and Doppler shift got from the GPS module.Then,the results got from the first Kalman filter is filtered by the second Kalman filter with the data got from the INS module.And it gets the correction and the optimum estimate.With the filter step increasing,the system prediction error variance tends to zero gradually,then the state estimation will excessively dependence on the old measurements,which will cause filter divergence and affect the position accuracy. To effectively increase the correction amount of the new measurements,this paper introduces a variable weighting factor in federal Kalman filter integrated navigation algorithm.The simulation results show that the improved variable gain federal Kalman filtering algorithm has the advantages of federal Kalman filter,and the filtering effect of this algorithm is improved obviously,which can effectively restrain the filter divergence and improve the positioning precision of the system.

federation Kalman filter;variable gain;location algorithm;accuracy;divergence

GPS接收模塊解算出的偽距誤差是GPS/INS組合導航系統的主要誤差，采用一種二級聯邦卡爾曼濾波組合導航算法加以削弱，將衛星接收模塊解算出的偽距信息和多普勒頻移信息在第一級卡爾曼濾波后，再通過主濾波器與INS模塊解算出的信息進行修正處理，得到校正量和定位位置最優估計。隨著濾波步數增加，系統預測誤差方差陣逐漸趨于零，狀態估計會過分依賴舊量測值，從而導致濾波發散，影響系統定位精度。為有效提高新量測值的修正作用，在聯邦卡爾曼濾波組合導航算法中引入一種可變加權系數。仿真結果表明，改進后的變增益聯邦卡爾曼濾波算法具備聯邦卡爾曼濾波的優點，并且該算法濾波效果有較明顯的改善，能有效抑制濾波發散，提高系統的定位精度。

聯邦卡爾曼濾波；變增益；定位算法；精度；發散

A

TP301.6

10.3778/j.issn.1002-8331.1304-0099

ZHANG Yi,ZHAO Kaihua,YAO Bin.Research on variable gain federation Kalman filter location algorithm of coupled system.Computer Engineering and Applications,2013,49（19）：196-199.

國家自然科學基金（No.61202394）；西北工業大學研究生創業種子基金（No.Z2013095）。

張怡（1958—），女，教授，主要研究方向為通信、導航、遙測、遙控系統的信息傳輸與處理，多媒體通信與計算機通信；趙凱華（1988—），男，碩士，主要研究方向為GPS的導航與定位。E-mail：huakai051838@163.com

2013-04-08

2013-06-01

1002-8331（2013）19-0196-04

CNKI出版日期：2013-06-08http://www.cnki.net/kcms/detail/11.2127.TP.20130608.1001.028.html