基于R陣動態(tài)估計的自適應(yīng)濾波算法

萇永娜， 張 海， 李玉潔， 楊 靜

(北京航空航天大學(xué)控制一體化技術(shù)國家級重點實驗室，北京 100191)

0 引言

在觀測噪聲動態(tài)估計研究中，文獻［1］提出了自適應(yīng)更新狀態(tài)噪聲、觀測噪聲誤差統(tǒng)計特性的方法;文獻［2］提出了基于新息序列的自適應(yīng)開窗逼近法，依據(jù)觀測信息自適應(yīng)調(diào)整狀態(tài)協(xié)方差陣和觀測協(xié)方差陣;文獻［3］提出了一種動態(tài)自適應(yīng)抗差濾波算法，能夠自適應(yīng)地分配觀測信息和模型信息對狀態(tài)估計的權(quán)重。

對于線性系統(tǒng)而言，上述觀測噪聲特性統(tǒng)計方法均依賴于新息量，如果一步狀態(tài)預(yù)測值估計不準(zhǔn)確，觀測噪聲特性的估計會出現(xiàn)偏差，從而影響濾波精度。

本文從組合導(dǎo)航系統(tǒng)中不同測量系統(tǒng)的測量互補特性出發(fā)，在一定情況下，通過相對測量關(guān)系的構(gòu)建，實現(xiàn)觀測噪聲的動態(tài)估計，在此基礎(chǔ)上，以預(yù)設(shè)濾波精度為指標(biāo)，通過構(gòu)造自適應(yīng)因子對估計觀測噪聲進行適當(dāng)?shù)恼{(diào)節(jié)，最終實現(xiàn)高精度的自適應(yīng)濾波。

1 基于R陣動態(tài)估計的自適應(yīng)濾波算法理論分析

1.1 現(xiàn)有自適應(yīng)濾波算法分析

標(biāo)準(zhǔn)卡爾曼濾波(SKF)中，狀態(tài)估計和濾波增益為

自適應(yīng)濾波是通過估計或修正系統(tǒng)模型參數(shù)、噪聲特性或濾波增益等方法來提高濾波精度的，例如漸消濾波［4］和抗差自適應(yīng)濾波［5-7］在SKF基礎(chǔ)上的改進如下所述。

漸消濾波增益陣為

其中:Φk，k-1為系統(tǒng)一步轉(zhuǎn)移矩陣;Qk為系統(tǒng)噪聲方差陣;Pk-1為狀態(tài)估計方差陣。

抗差自適應(yīng)濾波增益陣為

漸消因子λ與抗差自適應(yīng)因子γ分別為

其中:tr［·］為矩陣求跡符號;c0為常值。

漸消濾波將驗前狀態(tài)協(xié)方差矩陣膨脹λ(λ≥1)倍，抗差自適應(yīng)濾波通過構(gòu)造自適應(yīng)因子γ(0＜γ≤1)來調(diào)節(jié)狀態(tài)噪聲的協(xié)方差陣，由式(3)、式(4)看出，兩者的最終結(jié)果均是將濾波增益陣K增大，從而提高觀測信息的利用權(quán)重，但當(dāng)觀測誤差變大時，其濾波效果將明顯變差。由式(5)、式(6)知，λ與γ的構(gòu)造均是新息量的函數(shù)，一旦狀態(tài)估計值有較大誤差，對觀測噪聲的估計會出現(xiàn)偏差，從而影響自適應(yīng)濾波的估計精度和穩(wěn)定性。

1.2 觀測噪聲R自適應(yīng)估計方法分析

文獻［8］針對GPS/DR系統(tǒng)提出了一種改進的航向估計算法，利用GPS和陀螺儀信息自適應(yīng)估計航向測量噪聲方差陣R。文獻［9］提出了一種基于不同測量系統(tǒng)測量特性的觀測噪聲方差陣R自適應(yīng)估計定理，并從理論上證明了其正確性。設(shè)對于同一物理量Z(k)同時存在兩種不同的測量系統(tǒng)M1、M2，并且兩系統(tǒng)的測量特性互補。其中，測量系統(tǒng)M1的測量結(jié)果在真值附近抖動，在某些情況下其抖動幅度較大，而M2具有短時高精度，且其測量值的變化趨勢與真實值的變化趨勢基本一致。設(shè)兩系統(tǒng)測量值分別為Z1(k)、Z2(k)，單測量系統(tǒng)自差分序列分別為ΔZ1(k)、Z2(k)，且

則M1的測量噪聲方差陣R可進行如下近似估計:

對于測量噪聲方差陣R的估計，文獻［9］中提出了一種連續(xù)滑動窗口的統(tǒng)計方法，在GPS/INS機載導(dǎo)航系統(tǒng)中進行了仿真驗證，當(dāng)測量噪聲方差陣R滿足在一定區(qū)間內(nèi)近似為同分布的條件時，此方法能夠獲得良好的估計效果。但是，滑動窗口的寬度在實際應(yīng)用中難以確定，并且若窗口內(nèi)只有當(dāng)前歷元的觀測誤差很大，由于使用了大量歷史數(shù)據(jù)的平滑作用，會使當(dāng)前歷元觀測噪聲的估計值降低，從而無法獲得當(dāng)前歷元狀態(tài)量的最優(yōu)估計。此外，此方法也在一定程度上增加了計算量和存貯量。

1.3 基于R陣動態(tài)估計的自適應(yīng)濾波算法

在實際系統(tǒng)中，觀測噪聲的動態(tài)特性通常是復(fù)雜多變的，為如實反映各個歷元瞬時噪聲特性，本算法基于R自適應(yīng)估計定理的思想，針對單次歷元的觀測噪聲特性進行動態(tài)估計。事實上，基于不同測量特性的R估值定理可以獲得接近實際系統(tǒng)的測量噪聲特性(文獻［9］已進行了仿真驗證)，但若直接作用于濾波解算中，濾波效果并不十分理想。

在卡爾曼濾波中，式(3)第二項體現(xiàn)了修正信息對狀態(tài)估計的貢獻，其權(quán)重由Kk確定。對于觀測信息Zk，基于雙測量系統(tǒng)的R估計定理主要針對觀測噪聲的隨機誤差進行估計，并未考慮系統(tǒng)誤差;對于Hk，由于設(shè)計卡爾曼濾波器時選取的系統(tǒng)模型往往包含一定的不確定性，因此也包含了一定的誤差，從而造成修正信息可信度的下降。Kk是建立在P和R均可精確獲得的理想情況下的最優(yōu)增益，而這在實際系統(tǒng)中難以實現(xiàn)。因此為減小狀態(tài)估計誤差，本文從預(yù)設(shè)的定位精度ε出發(fā)，在當(dāng)前濾波增益陣Kk的基礎(chǔ)上乘一個自適應(yīng)因子α(0＜α≤1)，等價于將估計R進行一定程度的膨脹作用(設(shè)為β倍，β≥1)，將系統(tǒng)誤差、模型誤差的影響包含進來，從而得到精度更高的濾波估計。

由于狀態(tài)向量通常含有不同類型的參數(shù)，且各觀測分量對同一狀態(tài)量的作用也不盡相同，因此本文采用序貫處理方法對各觀測分量進行處理。設(shè)Rk為對角陣(在實際系統(tǒng)中具有一定的普遍性)，主對角線元素，…，k均為一維變量。依據(jù)序貫處理思想，設(shè)k時刻，對于第i個觀測分量，有

定義自適應(yīng)因子為α=diag［α1α2… αm］，相應(yīng)地 β=diag［β1β2… βm］。

因此新的濾波增益陣也可以表示為

由式(12)及式(14)聯(lián)立可得

因αi不為零，式(15)兩邊分別求逆，得

由于0＜αi≤1，最終新的噪聲陣為

2 基于R陣動態(tài)估計的自適應(yīng)濾波算法實現(xiàn)

1)單測量系統(tǒng)自差分。

設(shè)k時刻，如第1.2節(jié)所述，組合導(dǎo)航系統(tǒng)中兩測量系統(tǒng)的測量值分別為Z1(k)、Z2(k)，首先對兩測量系統(tǒng)分別求單系統(tǒng)自差分序列ΔZ1(k)、ΔZ2(k)為

2)雙測量系統(tǒng)互差分。

3)雙測量系統(tǒng)互差分序列單歷元估計。

4)自適應(yīng)因子的構(gòu)造。

設(shè)組合系統(tǒng)需要達到的定位精度為ε，測量噪聲估計分量值為R^ik，基于測量噪聲估計方差陣映射的觀測誤差水平，自適應(yīng)因子αi可構(gòu)造為

其中，c為常數(shù)，通常取為0.2 ～0.8。

5)本自適應(yīng)濾波的序貫處理核心算法。

同SKF相比，本自適應(yīng)算法僅增加了各歷元雙系統(tǒng)互差分及自適應(yīng)因子的簡單計算，并且采用序貫處理方法，對Zk各分量進行順序處理，將對高階矩陣的求逆轉(zhuǎn)變?yōu)榈碗A矩陣的求逆，有效降低了計算量。

3 算法驗證

3.1 GPS/DR組合導(dǎo)航濾波模型

為驗證本算法的有效性，本算例在GPS/DR實際系統(tǒng)中，在多遮擋、多障礙物的路段，對機車進行高精度定位。本文采取直接法建模，選取的狀態(tài)向量為，各分量依次為北、東向位置，航向角，速度，航向角速度，陀螺儀零偏B，里程儀刻度因子S;選取的觀測向量為Z=，各分量依次為 GPS測量的北、東向位置、航向，里程儀測量的速度，陀螺儀測量的航向角速度。

本文采用EKF方法進行線性化，得出的狀態(tài)轉(zhuǎn)移矩陣Φk+1，k及觀測矩陣Hk如下:

其中:T為濾波周期;Pod為里程儀測量的脈沖數(shù)。

3.2 算例分析

3.2.1 實驗條件

機車運行初始條件:位置為北緯36.598090°、東經(jīng)114.451120°，航向角為 322.0755°，速度為 2.7344 m/s。GPS采用差分定位，為實現(xiàn)高精度定位，本實驗中將經(jīng)緯度坐標(biāo)轉(zhuǎn)換為WGS-84直角坐標(biāo)系。本實驗中，機車在鋼廠的鐵軌上運行，相鄰鐵軌大約為5 m，并且由于工況要求，機車運行速度較低，主要對東、北向位置進行估計，定位精度要求為5 m。

3.2.2 對比算法

除標(biāo)準(zhǔn)卡爾曼濾波以外，本算法與改進的sage-husa自適應(yīng)算法［9］及 MAKF［10］算法進行了同條件對比實驗。改進的sage-husa自適應(yīng)算法只針對觀測噪聲R的統(tǒng)計特性進行自適應(yīng)估計。MAKF算法中，GPS測量噪聲的估計方法為

其中:i為當(dāng)前濾波時刻;Ck為雙系統(tǒng)互差分序列;M為滑動窗口寬度，此處取為30。

3.2.3 仿真結(jié)果分析

在實驗條件下，GPS測量結(jié)果如圖1所示。150～174 s、230～245 s區(qū)段內(nèi)GPS信號良好，定位結(jié)果正常;175～229 s區(qū)段內(nèi)GPS由于受到周圍高層建筑物的影響，輸出結(jié)果明顯偏離真實軌道。采用SKF、改進的sage-husa算法、MAKF算法及本自適應(yīng)算法進行GPS/DR組合導(dǎo)航濾波解算，實驗結(jié)果如圖2及圖3所示。其中本算法構(gòu)造自適應(yīng)因子時，經(jīng)過反復(fù)試驗，式(21)中常數(shù)c最終取為0.2。與其他各算法相比，本算法能夠自適應(yīng)地跟蹤周圍環(huán)境的變化，輸出結(jié)果光滑連續(xù)，最大定位誤差為4 m，滿足預(yù)設(shè)精度要求。

圖1 GPS測量結(jié)果Fig.1 The output of GPS

從圖2a、圖2c中可以看出，SKF中觀測噪聲的統(tǒng)計特性由驗前統(tǒng)計結(jié)果獲得，其結(jié)果恒定不變(10 m2)，其他算法可對觀測噪聲進行自適應(yīng)估計。175～198 s時間段內(nèi)，GPS的噪聲特性實際上已經(jīng)發(fā)生了較大變化，但是改進的sage-husa算法、MAKF算法均未做出響應(yīng);200～230 s時間段內(nèi)，在GPS的定位誤差較大的情況下，雖然改進的sage-husa算法、MAKF算法對GPS的噪聲特性進行了自適應(yīng)估計，但估計動態(tài)性能較差;231～245 s時間段內(nèi)，GPS已恢復(fù)正常定位，而改進的sage-husa算法、MAKF算法對GPS觀測噪聲的估計仍嚴(yán)重偏高;整個時間段內(nèi)，本自適應(yīng)濾波算法快速、及時地跟蹤了GPS噪聲特性的變化。

由于改進的sage-husa算法在對R更新時耦合了狀態(tài)估計誤差的影響，因而估計結(jié)果不可靠。MAKF算法對R的估計建立在窗口內(nèi)各歷元的統(tǒng)計結(jié)果的基礎(chǔ)上，因而會受到歷史數(shù)據(jù)平滑作用的影響，削弱了對觀測噪聲估計的動態(tài)性能，從而使其在高動態(tài)的實際系統(tǒng)中的應(yīng)用受到限制。本自適應(yīng)算法針對單次歷元的觀測噪聲特性進行估計，避免受到大量過去數(shù)據(jù)的影響，且在對R的估計過程中不存在與狀態(tài)估計誤差協(xié)方差陣P的耦合關(guān)系;從預(yù)設(shè)濾波精度出發(fā)，基于估計的觀測噪聲水平確定自適應(yīng)調(diào)節(jié)因子的比例，對當(dāng)前估計觀測噪聲信息進行適當(dāng)?shù)呐蛎涀饔?，相?dāng)于將觀測估計誤差、狀態(tài)估計誤差等包含進來，從而實現(xiàn)了準(zhǔn)確、可靠的濾波估計效果。

圖2 4種算法的R估計結(jié)果Fig.2 Comparison of four Kalman filtering algorithms in estimating R

圖3 4種算法實驗結(jié)果對比Fig.3 Comparison of four Kalman filtering algorithms

4 結(jié)論

本算法基于組合導(dǎo)航系統(tǒng)中不同測量系統(tǒng)的測量特性，構(gòu)造相對測量關(guān)系估計單次歷元的噪聲特性，避免了現(xiàn)有估計方法中狀態(tài)誤差和歷史數(shù)據(jù)的影響;在滿足預(yù)設(shè)精度的前提下，通過構(gòu)造自適應(yīng)因子對估計R陣適當(dāng)調(diào)節(jié)，收斂于濾波增益K陣對觀測信息有效利用的準(zhǔn)則，有效地提高了濾波精度。同改進的sage-husa算法、MAKF等現(xiàn)有自適應(yīng)濾波方法相比，本自適應(yīng)算法在實際系統(tǒng)中得到了良好的應(yīng)用，在濾波精度和抗干擾性能上，具有明顯的優(yōu)越性，具有深入的理論推廣意義和工程實用價值。此外，由于組合導(dǎo)航系統(tǒng)中通常存在某些兩個測量系統(tǒng)及以上的物理量，因此本文主要針對雙測量系統(tǒng)的測量特性進行自適應(yīng)估計，對于只有單個測量系統(tǒng)的物理量，可以考慮構(gòu)造等效觀測量的方法進行估計，值得進一步研究。

［1］SAGE A P,HUSA G W.Adaptive filtering with unknown prior statistic［C］//Proceedings of Joint Automatic Control Conference，Washington DC:American Automatic Control Council,1969:760-769.

［2］MEHRA R K.On the identification of variances and Adaptive Kalman filtering［J］.IEEE Transactionson Automatic Control,1970,15(2):175-184.

［3］CUI X，YANG Y.Adaptively robust filtering with classified adaptive factors［J］.Progress in Natural Science,2006,16(8):846-851.

［4］王虎，王解先，白貴霞，等.改進的漸消卡爾曼在GPS動態(tài)定位中的應(yīng)用［J］.同濟大學(xué)學(xué)報，2011，39(1):124-128.

［5］YANG Yuanxi,GAO Weiguang.Comparison of two fading filters and adaptively robust filter［J］.Geo-spatial Information Science,2007,10(3):200-203.

［6］YANG Y,GAO W.An optimal adaptive Kalman filter［J］.Journal of Geodesy,2006,80(4):177-183.

［7］YANG Yuanxi,GAO Weiguang.A new learning statistic for adaptive filter based on predicted residual［J］.Progress in Natural Science,2006,16(8):833-837.

［8］劉倩，張海，周艷麗，等.改進的GPS/DR組合導(dǎo)航自適應(yīng)航向估計算法［J］.電光與控制，2010，17(6):71-77.

［9］張海，常艷紅，車歡.基于GPS/INS不同測量特性的自適應(yīng)卡爾曼濾波算法［J］.中國慣性技術(shù)學(xué)報，2010，18(6):331-336.

［10］常艷紅.GPS/INS組合導(dǎo)航自適應(yīng)濾波技術(shù)研究［D］.北京:北京航空航天大學(xué)，2010.

［11］WANG Jian,LIU Jiang,CAI Bogen.Study on information fusion algorithm in embedded integrated navigation system［C］//2008 International Conference on Intelligent Computation Technology and Automation,2008:1007-1010.