自適應(yīng)擴(kuò)維UKF算法在SINS/GPS組合導(dǎo)航系統(tǒng)中的應(yīng)用

孫堯，馬濤，高延濱，王璐

(1.哈爾濱工程大學(xué)自動化學(xué)院，黑龍江哈爾濱 150001;2.上海交通大學(xué)航空航天學(xué)院，上海 200240)

卡爾曼濾波是一種遞推的線性無偏最小方差估計，當(dāng)理論模型與實(shí)際系統(tǒng)完全匹配時，卡爾曼濾波器輸出的殘差序列是互不相關(guān)的高斯白噪聲序列［1］.然而，其前提是必須精確已知系統(tǒng)模型和噪聲的統(tǒng)計特性，否則將會出現(xiàn)濾波器精度下降甚至發(fā)散等問題［2］.這是由于當(dāng)系統(tǒng)噪聲統(tǒng)計特性未知時，估計誤差的協(xié)方差矩陣隨時間逐漸收斂于零，狀態(tài)估計過分依賴于陳舊數(shù)據(jù)，導(dǎo)致濾波發(fā)散［3］.

衰減記憶濾波方法能夠通過自適應(yīng)漸消因子限制濾波器的記憶長度，從而抑制了濾波發(fā)散［4］.但是，對高階系統(tǒng)進(jìn)行濾波時，由于系統(tǒng)各狀態(tài)量估計精度的差異，通過簡單漸消因子的計算，很難達(dá)到理想的效果.針對這一問題，文獻(xiàn)［3，5］通過自適應(yīng)漸消矩陣限制濾波器的記憶長度，但其前提條件為系統(tǒng)狀態(tài)量是直接可觀測的，即系統(tǒng)量測矩陣中對應(yīng)部分為對角陣，這極大地限制了自適應(yīng)漸消矩陣在具有復(fù)雜量測方程系統(tǒng)中的應(yīng)用.

傳統(tǒng)漸消因子的計算方法以卡爾曼濾波為基礎(chǔ)，僅適用于線性系統(tǒng)狀態(tài)估計，而組合導(dǎo)航系統(tǒng)是典型的非線性系統(tǒng)，由于忽略了高階誤差項(xiàng)，基于線性誤差模型的組合導(dǎo)航系統(tǒng)濾波精度將會下降.Julier提出了基于Unscented變換的UKF(unscented Kalman filter)算法［6］，不論系統(tǒng)非線性程度如何，Unscented變換都能以三階泰勒精度逼近非線性系統(tǒng)狀態(tài)的后驗(yàn)分布.然而，傳統(tǒng)UKF算法假定系統(tǒng)噪聲為加性噪聲，不參與系統(tǒng)非線性傳遞，Merwe等針對含有非加性噪聲的非線性系統(tǒng)，提出了擴(kuò)維UKF(augmented UKF，AUKF)算法［7］，文獻(xiàn)［8］從理論上給出了AUKF算法可以提高濾波精度的證明.但UKF和AUKF算法與經(jīng)典卡爾曼濾波器一樣，以最小方差估計原理為理論基礎(chǔ)，都需要精確已知系統(tǒng)的模型和噪聲統(tǒng)計特性.

針對以上分析，提出了帶自適應(yīng)漸消矩陣的AUKF(adaptive fading matrix augmented UKF，AFMAUKF)算法.為了使?jié)u消矩陣適用于具有復(fù)雜量測方程的系統(tǒng)，提出一種新的自適應(yīng)漸消矩陣的計算方法，并以Unscented變換對系統(tǒng)的后驗(yàn)均值和協(xié)方差進(jìn)行近似，從而將自適應(yīng)漸消矩陣的使用擴(kuò)展到含有非加性白噪聲的非線性系統(tǒng)狀態(tài)估計領(lǐng)域.

1 漸消矩陣的分析與計算

考慮如下線性離散系統(tǒng):

文獻(xiàn)［3］提出了一種自適應(yīng)漸消因子卡爾曼濾波(adaptive fading factor Kalman filter，AFF-KF)算法，記為

式中:Q和R分別為系統(tǒng)噪聲和量測噪聲的協(xié)方差矩陣，且Q和R正定.

自適應(yīng)漸消因子為

式中:

從而得到帶自適應(yīng)漸消因子的誤差協(xié)方差矩陣一步預(yù)測過程為

這種計算漸消因子的方法簡單，適用于低階系統(tǒng)的濾波問題.在高階系統(tǒng)中，由于各狀態(tài)量估計精度的差異，使用自適應(yīng)漸消因子的濾波效果并不理想.

文獻(xiàn)［5］通過自適應(yīng)漸消矩陣對卡爾曼濾波的記憶長度進(jìn)行限制，記:

自適應(yīng)漸消矩陣為

該方法的前提條件為系統(tǒng)量測矩陣中對應(yīng)部分為對角陣，當(dāng)系統(tǒng)狀態(tài)量存在耦合關(guān)系，或系統(tǒng)狀態(tài)量不是直接可觀測的時候，該計算方法無法使用.且隨著時間的推移，Ck+1的計算量會逐漸增大，保存從初始時刻開始的新息矢量對于長時間工作的濾波器來說也是很難實(shí)現(xiàn)的.

針對以上問題，本文根據(jù)濾波最優(yōu)的條件，提出一種自適應(yīng)漸消矩陣的計算方法.

若k時刻濾波最優(yōu)，根據(jù)卡爾曼濾波的無偏性和正交性原理，殘差應(yīng)服從均值為0的高斯分布，即

由式(3)構(gòu)造殘差的加權(quán)平方和:

根據(jù)χ2分布的定義，γk+1服從m自由度的中心χ2分布.通過自適應(yīng)漸消矩陣對一步預(yù)測誤差協(xié)方差矩陣的修正過程同式(1)，根據(jù)式(1)可以看出，Sk+1無法通過直接計算獲得.假設(shè)Ωk+1Hk+1=Hk+1Sk+1，則有

根據(jù)χ2檢驗(yàn)準(zhǔn)則，得到Ωk+1的計算方法:

式中:Ak+1和Bk+1的計算方法同式(1).

假設(shè)Hk+1是行滿秩的.當(dāng)Hk+1不存在全零列時，自適應(yīng)漸消矩陣為

從而通過式(7)得到經(jīng)自適應(yīng)漸消矩陣修正后的一步預(yù)測誤差協(xié)方差矩陣.

在組合導(dǎo)航系統(tǒng)中Hk+1存在全零列的情況十分常見，此時假設(shè)系統(tǒng)狀態(tài)維數(shù)為n，系統(tǒng)觀測量維數(shù)為m，Hk+1中全零列的維數(shù)為p，將Hk+1表達(dá)為如式(8):

漸消因子矩陣的計算過程為:

2 AFM在UKF算法中的等價描述

通過以上分析可以看出，計算自適應(yīng)漸消因子和漸消矩陣的方法都是基于卡爾曼濾波框架的，需要獲得系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和量測矩陣等信息.然而，在AUKF算法中，狀態(tài)轉(zhuǎn)移矩陣和量測矩陣等系統(tǒng)信息無法直接獲得;因此，無法通過卡爾曼濾波框架下的計算方法得到AUKF濾波器的自適應(yīng)漸消矩陣.由于AUKF與UKF算法在原理上是相同的，因此，通過UKF的等價描述，可以得到AUKF濾波器自適應(yīng)漸消矩陣的計算方法.

根據(jù)卡爾曼濾波和UKF算法的等價描述［9］:

可以得到計算自適應(yīng)漸消矩陣過程中各變量的表達(dá)式為

式中:P'k+1|k為系統(tǒng)誤差的協(xié)方差矩陣沿非線性函數(shù)的傳播結(jié)果，計算P'k+1|k的步驟與UKF算法中計算Pk+1|k的步驟相似，只是計算Sigma點(diǎn)沿非線性函數(shù)的傳播過程的時候，不考慮系統(tǒng)噪聲的作用即可.顯然，對于系統(tǒng)噪聲為加性白噪聲的非線性系統(tǒng)，有Qk=Pk+1|k－P'k+1|k.

針對自適應(yīng)漸消矩陣的計算，當(dāng)Hk+1不存在全零列時，可由式(5)～(7)獲得;否則，采用式(5)、(6)、(8)～(10).在此基礎(chǔ)上，可以得到經(jīng)自適應(yīng)漸消矩陣修正后的一步預(yù)測協(xié)方差矩陣為

3 AFM-AUKF算法

考慮如下非加性噪聲的非線性離散系統(tǒng):

式中:k≥0為離散時間變量;x∈Rn為狀態(tài)向量;u∈Rn為輸入向量;z∈Rm為輸出向量;非線性函數(shù)f∈Rq× Rn→Rn，h∈Rn→Rm.系統(tǒng)噪聲 vk、量測噪聲wk分別為q維和m維的高斯白噪聲，并具有以下統(tǒng)計特性:

式中:Q 和 R 為正定對稱陣，δk，j為 kronecker-δ函數(shù).

初始化系統(tǒng)狀態(tài)和誤差的協(xié)方差矩陣為

根據(jù)自適應(yīng)漸消矩陣在UKF算法中的等價計算方法，得到AFM-AUKF算法的步驟:

1)計算Sigma點(diǎn).

式中:L=n+q+m為擴(kuò)維后的系統(tǒng)狀態(tài)維數(shù).

2)權(quán)值計算.

式中:λ=α2(L+k)－L.

3)時間更新過程.

4)量測更新過程.

5)重新計算Sigma點(diǎn).

6)新的Sigma點(diǎn)權(quán)值計算.

式中:λ'=α2(L'+k)－L'.

7)經(jīng)漸消矩陣修正后的量測更新.

8)最小方差估計結(jié)果.

4 實(shí)驗(yàn)與分析

為了驗(yàn)證AFM-UKF算法的性能，本文以SINS/GPS組合導(dǎo)航系統(tǒng)作為平臺，對提出的方法進(jìn)行驗(yàn)證.選取北、東、地坐標(biāo)系作為導(dǎo)航坐標(biāo)系，取姿態(tài)四元數(shù)、速度、位置和陀螺零偏作為系統(tǒng)狀態(tài)向量，建立濾波器狀態(tài)方程，選取位置、速度為系統(tǒng)觀測向量，建立濾波器量測方程，具體模型參考文獻(xiàn)［11］.

分別采用UKF、自適應(yīng)漸消因子的擴(kuò)維UKF(adaptive fading factor augmented UKF，AFF-AUKF)和AFM-AUKF作為濾波器對組合導(dǎo)航系統(tǒng)的姿態(tài)、速度和位置信息進(jìn)行估計.通過系統(tǒng)狀態(tài)突變、系統(tǒng)噪聲統(tǒng)計特性不準(zhǔn)確和加速度計噪聲統(tǒng)計特性不準(zhǔn)確3個試驗(yàn)驗(yàn)證3種濾波方法對系統(tǒng)模型不確定的魯棒性和容錯能力.

4.1 系統(tǒng)狀態(tài)突變

采用靜態(tài)數(shù)據(jù)進(jìn)行試驗(yàn)，并在t=30 s時，設(shè)定系統(tǒng)的俯仰角和橫滾角誤差突變?yōu)椋?0°，檢驗(yàn)濾波器的容錯能力(如圖1～3).通過試驗(yàn)結(jié)果可以看出，AFF-AUKF和AFM-UKF算法與傳統(tǒng)UKF算法相比，姿態(tài)誤差的均值分別降低了44.6%和83.2%，姿態(tài)誤差的標(biāo)準(zhǔn)差分別降低了32.4%和54.9%.

圖1 系統(tǒng)狀態(tài)突變時的姿態(tài)誤差Fig.1 Attitude error of state mutation

圖2 系統(tǒng)狀態(tài)突變時的速度誤差Fig.2 Velocity error of state mutation

圖3 系統(tǒng)狀態(tài)突變時的位置誤差Fig.3 Position error of state mutation

4.2 系統(tǒng)噪聲統(tǒng)計特性不準(zhǔn)確

使用搖擺數(shù)據(jù)進(jìn)行試驗(yàn)，假設(shè)在t=30 s時，系統(tǒng)噪聲的統(tǒng)計特性發(fā)生突變，陀螺儀和加速度計噪聲的方差均變?yōu)樵瓉淼?00倍(如圖4).此時，AFFUKF和AFM-UKF算法與傳統(tǒng)UKF算法相比，姿態(tài)誤差的均值分別降低了54.4%和71.6%，姿態(tài)誤差的標(biāo)準(zhǔn)差分別降低了59.6%和90.6%.

圖4 系統(tǒng)噪聲不準(zhǔn)確時的位置誤差Fig.4 Attitude error while the statistics of system noise is inaccurate

4.3 加速度計噪聲統(tǒng)計特性不準(zhǔn)確

使用搖擺數(shù)據(jù)進(jìn)行試驗(yàn)，假設(shè)在t=30 s時，系統(tǒng)噪聲的統(tǒng)計特性發(fā)生突變，但僅有加速度計噪聲的方差變?yōu)樵瓉淼?00倍，而陀螺的噪聲統(tǒng)計特性不發(fā)生改變(如圖5所示).此時，AFF-UKF和AFMUKF算法與傳統(tǒng)UKF算法相比，姿態(tài)誤差的均值分別降低了7.0%和78.3%，姿態(tài)誤差的標(biāo)準(zhǔn)差分別降低了2.6%和88.1%，

圖5 加速度計噪聲統(tǒng)計特性不準(zhǔn)確時的位置誤差Fig.5 Attitude error while the noise’s statistics of accelerometer is inaccurate

4.4 結(jié)果分析

從以上實(shí)驗(yàn)結(jié)果可以看出，由于系統(tǒng)的速度和位置狀態(tài)量是直接可觀測的，所以3種方法在位置和速度估計效果上的差異并不明顯，只是AFM-UKF算法的位置估計精度略高于AFF-UKF算法和傳統(tǒng)UKF算法.但是通過表1中數(shù)據(jù)的比較能夠看出:在系統(tǒng)狀態(tài)突變時，2種衰減記憶方法對突變狀態(tài)的跟蹤能力均強(qiáng)于傳統(tǒng)UKF算法，AFM-UKF算法的跟蹤速度又略高于AFF-UKF算法;在系統(tǒng)噪聲全部發(fā)生改變且變化幅度一致時，AFF-UKF和AFM-UKF算法均能夠提高姿態(tài)估計的精度，并且AFMUKF算法精度高于AFF-UKF算法;但當(dāng)系統(tǒng)噪聲中各部分變化不一致時，由于噪聲統(tǒng)計特性精度的差異，AFF-UKF算法的濾波精度受到極大的影響，系統(tǒng)精度的改善不明顯，而AFM-UKF算法的精度遠(yuǎn)遠(yuǎn)高于UKF算法和AFF-UKF算法的精度，可見，AFM-UKF算法對于系統(tǒng)模型不確定性具有更強(qiáng)的魯棒性.

表1 UKF、AFF-UKF和AFM-UKF算法實(shí)驗(yàn)結(jié)果精度比較Table 1 Filtering precision of UKF，AFF-UKF and AFM-UKF of experimental results (°)

5 結(jié)束語

本文提出的帶自適應(yīng)漸消矩陣的擴(kuò)維UKF算法，有效地解決了含有非加性白噪聲的非線性系統(tǒng)在系統(tǒng)狀態(tài)存在突變和系統(tǒng)噪聲統(tǒng)計特性不準(zhǔn)確時的濾波問題.實(shí)驗(yàn)結(jié)果表明，AFM-AUKF算法不僅與傳統(tǒng)UKF算法相比，具有更強(qiáng)的魯棒性和更高的濾波精度;而且與AFF-AUKF算法相比，AFM-AUKF算法對于噪聲統(tǒng)計特性的變化魯棒性更強(qiáng)，尤其對于部分噪聲統(tǒng)計特性發(fā)生變化時，由于漸消矩陣的靈活性，使算法具有更強(qiáng)的容錯能力.

［1］CHUI C K，CHEN G.Kalman filtering with real-time applications［M］.4th ed.New York:Springer-Verlag，2009:20-28.

［2］HAJIYEV C.Adaptive filtration algorithm with the filter gain correction applied to integrated INS/Radar altimeter［J］.Journal of Aerospace Engineering，2007，221(5):847-855.

［3］耿延睿，崔中興，張洪鉞，等.衰減因子自適應(yīng)濾波及在組合導(dǎo)航中的應(yīng)用［J］.北京航空航天大學(xué)學(xué)報，2004，30(5):434-437.

GENG Yanrui，CUI Zhongxin，ZHANG Hongyue，et al.A-daptive fading Kalman filter with applications in integrated navigation system［J］.Journal of Beijing University of Aeronautics and Astronautics，2004，30(5):434-437.

［4］夏啟軍，孫優(yōu)賢，周春暉.漸消卡爾曼濾波器的最佳自適應(yīng)算法及其應(yīng)用［J］.自動化學(xué)報，1990，16(3):210-216.XIA Qijun，SUN Youxian，ZHOU Chunhui.An optimal adaptive algorithm for fading Kalman filter and its application［J］.Acta Automatica Sinica，1990，16(3):210-216.

［5］高青偉，趙國榮，吳芳，等.衰減記憶自適應(yīng)濾波在慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)中的應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，2010，32(12):2648-2651.

GAO Qingwei，ZHAO Guorong，WU Fang，et al.Application of adaptive fading filter technique in transfer alignment of inertial navigation systems［J］.Systems Engineering and Electronics，2010，32(12):2648-2651.

［6］JULIER S，UHLMANN J K，DURRANT-WHYTE H F.A new approach for filtering nonlinear system［C］//Proceedings of the American Control Conference.Seattle，USA，1995:1628-1632.

［7］MERWE R.Sigma-point Kalman filters for probabilistic inference in dynamic state-space models［D］.Portland，USA:Oregon Health and Science University，2004:108-125.

［8］BRIERS M，MASKELL S R，WRIGHT R.A Rao-Blackwellised unscented Kalman filter［C］//Proceedings of the 6th International Conference of Information Fusion.Cairns，Australia，2003:55-61.

［9］王小旭，趙琳，夏全喜，等.基于Unscented變換的強(qiáng)跟蹤濾波器［J］.控制與決策，2010，24(7):1063-1068.

WANG Xiaoxu，ZHAO Lin，XIA Quanxi，et al.Strong tracking filter based on unscented transformation［J］.Control and Decision，2010，24(7):1063-1068.

［10］霍成立，謝凡，秦世引.面向室內(nèi)移動機(jī)器人的無跡濾波實(shí)時導(dǎo)航方法［J］.智能系統(tǒng)學(xué)報，2009，4(4):295-302.

HUO Chengli，XIE Fan，QIN Shiyin.A case study in real-time UKF-based navigation for indoor autonomous travelof mobile robots［J］.CAAI Transactions on Intelligent Systems，2009，4(4):295-302.

［11］Van der MERWE R，WAN E A.Sigma-point Kalman filters for integrated navigation［C］//Proceedings of the 60th Annual Meeting of The Institute of Navigation.Dayton，USA，2004:641-654.