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

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

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

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

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

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

1 漸消矩陣的分析與計(jì)算

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

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

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

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

式中:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

漸消因子矩陣的計(jì)算過(guò)程為:

2 AFM在UKF算法中的等價(jià)描述

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

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

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

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

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

3 AFM-AUKF算法

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

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

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

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

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

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

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

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

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

3)時(shí)間更新過(guò)程.

4)量測(cè)更新過(guò)程.

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

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

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

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

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

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

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

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

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

采用靜態(tài)數(shù)據(jù)進(jìn)行試驗(yàn)，并在t=30 s時(shí)，設(shè)定系統(tǒng)的俯仰角和橫滾角誤差突變?yōu)椋?0°，檢驗(yàn)濾波器的容錯(cuò)能力(如圖1～3).通過(guò)試驗(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)突變時(shí)的姿態(tài)誤差Fig.1 Attitude error of state mutation

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

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

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

使用搖擺數(shù)據(jù)進(jìn)行試驗(yàn)，假設(shè)在t=30 s時(shí)，系統(tǒng)噪聲的統(tǒng)計(jì)特性發(fā)生突變，陀螺儀和加速度計(jì)噪聲的方差均變?yōu)樵瓉?lái)的100倍(如圖4).此時(shí)，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)確時(shí)的位置誤差Fig.4 Attitude error while the statistics of system noise is inaccurate

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

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

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

4.4 結(jié)果分析

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

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

［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é)報(bào)，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］.自動(dòng)化學(xué)報(bào)，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］高青偉，趙國(guó)榮，吳芳，等.衰減記憶自適應(yīng)濾波在慣導(dǎo)系統(tǒng)傳遞對(duì)準(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)移動(dòng)機(jī)器人的無(wú)跡濾波實(shí)時(shí)導(dǎo)航方法［J］.智能系統(tǒng)學(xué)報(bào)，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.