自適應擴維UKF算法在SINS/GPS組合導航系統中的應用

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

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

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

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

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

針對以上分析，提出了帶自適應漸消矩陣的AUKF(adaptive fading matrix augmented UKF，AFMAUKF)算法.為了使漸消矩陣適用于具有復雜量測方程的系統，提出一種新的自適應漸消矩陣的計算方法，并以Unscented變換對系統的后驗均值和協方差進行近似，從而將自適應漸消矩陣的使用擴展到含有非加性白噪聲的非線性系統狀態估計領域.

1 漸消矩陣的分析與計算

考慮如下線性離散系統:

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

式中:Q和R分別為系統噪聲和量測噪聲的協方差矩陣，且Q和R正定.

自適應漸消因子為

式中:

從而得到帶自適應漸消因子的誤差協方差矩陣一步預測過程為

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

文獻［5］通過自適應漸消矩陣對卡爾曼濾波的記憶長度進行限制，記:

自適應漸消矩陣為

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

針對以上問題，本文根據濾波最優的條件，提出一種自適應漸消矩陣的計算方法.

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

由式(3)構造殘差的加權平方和:

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

根據χ2檢驗準則，得到Ωk+1的計算方法:

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

假設Hk+1是行滿秩的.當Hk+1不存在全零列時，自適應漸消矩陣為

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

在組合導航系統中Hk+1存在全零列的情況十分常見，此時假設系統狀態維數為n，系統觀測量維數為m，Hk+1中全零列的維數為p，將Hk+1表達為如式(8):

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

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

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

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

可以得到計算自適應漸消矩陣過程中各變量的表達式為

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

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

3 AFM-AUKF算法

考慮如下非加性噪聲的非線性離散系統:

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

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

初始化系統狀態和誤差的協方差矩陣為

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

1)計算Sigma點.

式中:L=n+q+m為擴維后的系統狀態維數.

2)權值計算.

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

3)時間更新過程.

4)量測更新過程.

5)重新計算Sigma點.

6)新的Sigma點權值計算.

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

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

8)最小方差估計結果.

4 實驗與分析

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

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

4.1 系統狀態突變

采用靜態數據進行試驗，并在t=30 s時，設定系統的俯仰角和橫滾角誤差突變為－10°，檢驗濾波器的容錯能力(如圖1～3).通過試驗結果可以看出，AFF-AUKF和AFM-UKF算法與傳統UKF算法相比，姿態誤差的均值分別降低了44.6%和83.2%，姿態誤差的標準差分別降低了32.4%和54.9%.

圖1 系統狀態突變時的姿態誤差Fig.1 Attitude error of state mutation

圖2 系統狀態突變時的速度誤差Fig.2 Velocity error of state mutation

圖3 系統狀態突變時的位置誤差Fig.3 Position error of state mutation

4.2 系統噪聲統計特性不準確

使用搖擺數據進行試驗，假設在t=30 s時，系統噪聲的統計特性發生突變，陀螺儀和加速度計噪聲的方差均變為原來的100倍(如圖4).此時，AFFUKF和AFM-UKF算法與傳統UKF算法相比，姿態誤差的均值分別降低了54.4%和71.6%，姿態誤差的標準差分別降低了59.6%和90.6%.

圖4 系統噪聲不準確時的位置誤差Fig.4 Attitude error while the statistics of system noise is inaccurate

4.3 加速度計噪聲統計特性不準確

使用搖擺數據進行試驗，假設在t=30 s時，系統噪聲的統計特性發生突變，但僅有加速度計噪聲的方差變為原來的100倍，而陀螺的噪聲統計特性不發生改變(如圖5所示).此時，AFF-UKF和AFMUKF算法與傳統UKF算法相比，姿態誤差的均值分別降低了7.0%和78.3%，姿態誤差的標準差分別降低了2.6%和88.1%，

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

4.4 結果分析

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

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

5 結束語

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

［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］耿延睿，崔中興，張洪鉞，等.衰減因子自適應濾波及在組合導航中的應用［J］.北京航空航天大學學報，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］夏啟軍，孫優賢，周春暉.漸消卡爾曼濾波器的最佳自適應算法及其應用［J］.自動化學報，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］高青偉，趙國榮，吳芳，等.衰減記憶自適應濾波在慣導系統傳遞對準中的應用［J］.系統工程與電子技術，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變換的強跟蹤濾波器［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］霍成立，謝凡，秦世引.面向室內移動機器人的無跡濾波實時導航方法［J］.智能系統學報，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.