GPS/INS組合導航的變分貝葉斯自適應卡爾曼濾波

沈 忱，徐定杰，沈 鋒，蔡佳楠

(哈爾濱工程大學自動化學院，150001哈爾濱)

經典卡爾曼濾波器的設計是針對系統參數與噪聲確定且在工作過程中保持不變的狀態空間模型，但在組合導航等實際工程領域的建模過程中，由于系統本身模型可能存在攝動，或受到統計特性未知的噪聲干擾及其他控制輸入等諸多不確定因素，經典濾波器中假設不變的參數在系統運行過程中都有可能會發生變化，因此，上述假設通常不能得到很好的滿足，從而使濾波器產生更大的估計誤差，極端情況下甚至不能工作［1］.近年來，為了增強系統的自適應性，許多自適應算法都相繼被提出，用以確定因不同的擾動而發生變化的參數［2-3］.

文獻［4］概括了目前幾種主要的自適應卡爾曼濾波算法，包括貝葉斯法、最大似然法、相關法以及協方差匹配法.其中，出于計算方面便利性的考慮，相關法相比于其他方法受到了更多關注.事實上，所有這些算法都可以被視為貝葉斯方法在不同情形下的特例.但在大多數情形下，積分項過于復雜，或者變量維數過高，導致積分運算求解困難［5］，使得貝葉斯方法在應用上有很大局限性.于是，為了避開求解后驗分布所涉及的積分運算，采樣方法受到了關注.其中以馬爾可夫鏈蒙特卡洛方法最為典型，它屬于隨機采樣逼近，能夠無限逼近得到精確的后驗分布，但是這種精確性是以犧牲運算量為代價的.所以，在很多實時性要求較高的場合，譬如本文關注的組合導航，采樣方法是不提倡的.

另一種可行的、更快速的逼近方法是確定型逼近.本文所采用的變分貝葉斯學習就屬于這類逼近.變分貝葉斯學習的叫法來源于文獻［6］.它最早源于計算機科學領域，文獻［7］對該理論體系進行了詳細的闡述.雖然變分貝葉斯方法最初的提出大都是為了解決機器學習以及模式識別領域當中的參數估計與模型選擇等問題，但是，近年來許多學者也陸續將其與狀態估計問題相結合，應用到濾波器相關的設計中來，使變分貝葉斯的應用領域得到了極大擴展.其中，文獻［8］最早提出將變分貝葉斯用于卡爾曼濾波的動態噪聲參數估計;文獻［9］將其用于有未知輸入干擾的狀態空間模型進行噪聲、狀態估計;文獻［10］用變分貝葉斯對整個隨機系統的參數估計;而文獻［11］將變分貝葉斯與卡爾曼濾波結合，用于高維數圖像的分析.

考慮到實際GPS/INS組合導航系統中，由于運載體自身形變產生的桿臂效應，或導航設備受到外界環境干擾引入的誤差，通常以GPS和INS所測導航參數的差為量測的噪聲統計信息將會變得不確定，且容易發生變化.本文在文獻［8-9］基礎上，進一步詳細闡述了變分貝葉斯參數估計的機理并利用它，從貝葉斯濾波的角度，設計了一種針對GPS/INS組合導航的高斯線性模型的噪聲自適應卡爾曼濾波器.每次時間更新后，用變分貝葉斯學習估計得到方差陣的后驗分布，再根據該噪聲統計矩對運載體的速度與位置狀態進行更新，從而達到自適應濾波的目的.實驗部分將驗證該算法在組合導航系統中在噪聲可變環境下對于運載體運動狀態估計的有效性.

1 準備工作

1.1 變分貝葉斯學習

在模型選擇與參數估計的問題當中，核心任務是在得到觀測數據后，根據已有先驗知識估計一組參數θ.根據貝葉斯準則，θ的后驗分布表示如下:

其中一個關鍵的步驟是計算上式的分母，也叫證據或者邊緣似然函數.當觀測量的維數或者參數的個數增加，上述積分將十分困難，甚至無法求解.考慮到諸如MCMC的數值計算方法的效率問題，采用變分貝葉斯學習來逼近求解參數的后驗分布.該方法提議用一個新的、形式較為簡單的分布(簡單是因為它可求)來逼近真實的后驗分布［6］.對式(1)分母，即邊緣似然函數取對數，運用一次Jensen不等式，如下所示:

其中F(q(θ))源于統計物理學，是自由能量的負值.根據文獻［5］中的建議再對邊緣似然函數進行數學處理，可以得到

式(2)中最后一個等號右邊的第二項被定義為Kullback-Leibler(KL)散度，具有如下性質:當新的分布q(θ)等于真實后驗p(θ｜z)時，DKL散度等于零為最小值.此時F(q(θ))達到最大值.于是，我們可以把F(q(θ))當作ln p(z)的下界，通過不斷最大化F(q(θ))(下界)來使q(θ)逼近p(θ｜z).將F(q(θ))關于q(θ)求導并置零，可以得到q(θ)的通解表達式如下［6］:

其中，Eq(xk≠i)［］代表關于q(θk≠i)的期望，可以認為參數集合近似分布的聯合概率密度函數可分解為各個參數獨立分布的乘積.這個假設是變分貝葉斯學習核心所在，在求解后驗分布時具有很大的計算優勢，而該假設的理論基礎最早出現在均值域理論領域(Mean Field Theory)，多篇關于變分貝葉斯方法的論文也證明了該理論及其在實際運用中的可行性與便利性［6，12］.這樣，多變量的聯合概率分布p(θ)就可以近似為各變量邊緣分布q(θi)的乘積，使得對多變量的聯合估計可以方便地轉化為對這些變量邊緣分布的迭代估計，從而顯著降低計算的復雜程度.

根據式(3)，變分貝葉斯學習可以歸納如下:對于每個待估計參數，通過計算聯合概率密度函數關于其他參數分布的期望，可以得到該參數新的分布.如此迭代計算，直至負自由能量(下界)達到最大值.這里涉及到變分貝葉斯算法的收斂性問題，即判斷何時下界達到最大值，而下界可以表示為

其中，Eq(θi-)［·］表示對［·］表達式求關于θ中除去θi外其他隨機變量的期望.由文獻［5］可知，該下界函數是關于所有q(θ)的凸函數，且隨著迭代循環估計的進行而遞增.根據文獻［13］中凸函數性質，下界F［q(θ)］一定收斂，且曲線梯度趨于平滑.本方法中，可以用相鄰兩次迭代的下界的差值作為判斷收斂的依據，當差值小于某一閾值后可以判定算法收斂，得到估計結果.所以，本質上講，變分貝葉斯參數迭代估計屬于梯度下降算法［13］.

需要指出的是，根據式(3)的形式，如果在共軛指數函數分布域(conjugate-exponential family，CE)中合理選取待估計參數的先驗分布，它的近似分布也將得到形式相同、參數不同的后驗分布.而CE域是一類應用很廣的分布域，大多數常用分布都屬于這個域.它的上述性質也在Beal的博士論文的2.4節作了詳細證明［6］，在之后的推導中我們也將看到合理選取CE中的分布帶來的計算上的便利性.

1.2 組合導航濾波模型

本文研究對象為零輸入、高斯線性離散時間的GPS/INS組合導航系統，其模型通常表述如下［14-15］:

在k時刻，時變觀測噪聲vk被認為是一個d×1的，方差為Rk(d×d)的零均值高斯噪聲序列，滿足vk～N(0，Rk).更特別的，選取的協方差矩陣是一個對角線元素依次為的對角陣. zk是一個d×1的觀測向量.系統狀態變量xk(包括位置誤差、速度誤差等)是一個n×1的，初始化均值和協方差分別為m0與P0的服從高斯分布的向量.即滿足xk～N(mk，Pk).wk、Gk、Φk與Hk分別是系統噪聲、系統噪聲輸入矩陣、狀態轉移矩陣與觀測矩陣，它們都被認為是已知的.

考慮到推導的簡潔性，用z1∶k表示從時刻1到k的觀測序列，假設θ代表被估計參數，則θ-k表征了在時刻k觀測前，根據k-1時刻估計的先驗參數值.θk表征在進行第k次觀測后得到的更新的后驗估計值，θ^代表θ的期望值.

顯然，當觀測噪聲服從方差可變的高斯分布時，經典的卡爾曼濾波器的估計性能就會因為現實噪聲統計特性與設計值不匹配而降低，當噪聲變化頻繁或與預期值相去甚遠時，濾波器甚至不能工作.一個可行的辦法就是在每次狀態更新前對觀測噪聲的分布進行實時估計，這也是自適應濾波的一個重要特點:實時更新系統或噪聲參數.本文采用的是變分貝葉斯學習.同時，因為該分布是高斯分布，所以可以用其二階統計矩(均值或方差)來刻畫其分布.在下一節中，文章將以貝葉斯準則為基礎，從概率論的角度對于改進的卡爾曼濾波算法進行推導.

2 自適應卡爾曼濾波

2.1 參數估計推導

在這個模型中，結合貝葉斯準則(1)，我們感興趣的參數系統狀態與觀測噪聲方差的后驗分布表述如下:

其中:

對于式(5)中倒數第二項中的p(zk｜z1∶k-1，xk，Rk)，假設第k次測量zk與之前的測量無關，固可化簡為p(zk｜xk，Rk).而對于一階馬爾科夫過程，又有

對xk-1與 Rk-1積分，可以得到如下的Chapman-Kolmogorov方程:

所以，在貝葉斯濾波理論當中，遞推的濾波算法每一步可以被看作是由預測(7)和更新(5)組成.在預測步驟中，參數的分布依賴于第k-1次觀測，時間更新后，得到參數的先驗分布(7);在第k次觀測更新后，根據貝葉斯準則對先驗進行校正，從而得到后驗分布(5)，為k+1時刻前的預測步驟提供輸入.另外，注意到這個預測-更新步驟中，除了一些極簡單特殊的情形，式(6)與式(7)中的積分基本無法得到其解析解，即使先前已經假設系統是高斯線性的.

現假設式(7)中各個參數是相互獨立的，根據先驗知識，系統狀態與觀測噪聲方差分別服從高斯分布和逆伽瑪分布:

現根據變分貝葉斯學習，進行如下設計:在第k次觀測后，建議用1個新的分布q(xk，Rk｜z1∶k)來代替p(xk，Rk｜z1∶k).同理，也認為該近似分布是因子可分解的，即:q(xk，Rk｜z1∶k)= q(xk)q(Rk).這里，考慮到表述的簡潔，略去了近似分布中參數xk，Rk對于z1∶k依賴的表述.

根據通解(3)的表達式，對每個參數分布取對數表達式計算依次如下:

其中C代表歸一化因子，它代表對式(6)中的證據所求期望的值，在變分貝葉斯學習中，它被認為是常數而略去.

對式(9)兩邊取指數表達式后發現，q(xk)是一個擁有新的均值與方差的高斯分布.這得益于之前所敘述的在共軛指數函數域中選取先驗分布的優越性.于是有q(xk)=N(xk｜mk，Pk)，其中:

同理，對于變量Rk有

所以，方差Rk仍然服從跟式(8)中形式一樣的逆伽瑪分布

新分布的參數分別為

上述兩式在變分貝葉斯學習中也被稱作參數更新方程.考慮到

展開式(16)第二項得

式中mk與Pk分別是新的高斯分布中的兩個參數.而又根據逆伽馬分布的性質，式(12)中的每個元素可以分別計算得到

最后考慮算法收斂的判斷.下界函數的計算可以表示為

其中Γ(·)與φ(·)分別為Gamma函數和digamma函數.當｜F(n)［xk，Rk］-F(n-1)［xk，Rk］｜＜Δ(Δ為一較小量)時，即相鄰兩次迭代的下界值變化可忽略不計時，可認定算法收斂，進入下一濾波時刻.

這樣，式(9)～(10)就形成了對于狀態變量和觀測噪聲方差實時進行聯合迭代估計的變分貝葉斯學習算法.2.2節將該算法與卡爾曼濾波算法結合，得到新的自適應濾波器.

2.2 算法總結

變分貝葉斯卡爾曼濾波與經典卡爾曼濾波一樣，可以劃分為預測(時間更新)和更新(數據更新)兩個步驟.

在零初時刻，初始化系統狀態與噪聲的超參數.

在某個k-1時刻觀測后，根據式(4)中系統狀態方程進行時間更新，對系統狀態變量均值與方差首先進行預測，如下:

對于觀測噪聲方差分布參數的時間遞推，本文根據文獻［16］最大熵值約束定理的論述，取逆伽瑪分布的超參數:

其中λ是一個在(0，1］內的變化因子，它的引入用以保證時變的方差σ2能以一定的概率分布(譬如本文的逆伽瑪)變化，即使后驗與先驗擁有相同的概率密度函數形式.正如1.1節最后一段所討論的，這將為變分貝葉斯學習提供算法上的保證與計算上的便利.

在k時刻觀測后，根據k時刻數據依次對噪聲方差和系統狀態進行估計.需要分別計算式(19)、(10)～(12)、(15)與(16).算法具體流程如圖1.

注意到上述更新步驟中，運用變分貝葉斯方法，不僅系統狀態變量統計矩(均值mk與協方差Pk)的估計需要確定的噪聲方差，噪聲方差的計算需要超參數α與β，而且參數更新方程(15)、(16)中也需要已知的mk與Pk，如此產生循環迭代計算，直至算法收斂，進而進入下一個時刻的預測-更新步驟.

圖1 變分貝葉斯卡爾曼濾波

3 仿真實驗

仿真實驗主要從兩方面來驗證算法的性能與效果:第一，對系統狀態量的估計;第二，對變化噪聲的跟蹤.

［15］建模方法，取東北天坐標系為導航坐標系，以GPS和INS測得位置的差值為觀測量，選取INS的誤差為15維狀態變量，即

式中下標E、N、U代表導航坐標系(東北天坐標系)的 3個軸為平臺失準角表示速度誤差代表位置誤差沿載體坐標系3個軸向陀螺漂移;為沿載體坐標系3個軸向加速度計漂移.本實驗中只關注輸出的位置誤差與速度誤差的估計值.于是，1.2節中d=3，n=15.

在Matlab環境下設計軌跡發生器模擬飛行器的理想運動.通過設定初始的位置、速度、加速度、姿態、姿態角變化率等參數，描述理想運動軌跡.設定各初始值分別為:經度120°，緯度45°，高度300 m，飛行器加速度、速度、姿態角、姿態角變化率均為0，且初始化自適應算法的變分超參數αi=1，βi=0.02，其中i=1，2，3(因為位置觀測量為三維矢量).

圖2顯示了該飛行器在1 000 s仿真時間段內的飛行軌跡，采樣間隔為1 s.

為驗證該濾波器的變分貝葉斯自適應濾波效果，將觀測噪聲方差陣在時間［1，250］內設為

其中diag為對角陣函數.然后在251～700 s，701～1 000 s內分別設為

圖2 飛行器運動軌跡

圖3與圖4給出了上述假設條件下，基于變分貝葉斯的自適應卡爾曼濾波器的濾波誤差圖，并與單獨使用INS時的情形做比較.圖3實線為濾波后飛行器位置誤差的估計值，圖4實線為濾波后飛行器速度誤差的估計值.發現無論是位置誤差估計，還是速度誤差估計，由于慣導誤差的積累，未經濾波的狀態估計(虛線表示)在一定時刻后均出現發散.考察本文設計的組合模型的濾波器，其觀測量的噪聲統計特性未知，且隨時間發生變化，但卻有著較好的濾波精度，誤差值較小，依然能對位置誤差與速度誤差進行良好的估計.

更進一步，圖5與圖6比較了噪聲變化條件下卡爾曼濾波與自適應濾波的誤差.如圖所示，250 s時刻之前，由于噪聲沒有發生變化，卡爾曼濾波器與自適應濾波器有著相當的濾波誤差.而250 s時刻之后，噪聲開始發生變化，由于卡爾曼濾波器自身的局限性，并不能夠在線調整，以至于在之后750 s的時間里，總體估計誤差顯著增大，相比自適應濾波的曲線，濾波精度較差，尤以250～700 s間變化波動最為劇烈.

圖3 濾波前后位置誤差對比

圖4 濾波前后速度誤差對比

圖5 濾波算法位置誤差對比

圖6 濾波算法速度誤差對比

圖7給出了該自適應濾波器對觀測噪聲方差的跟蹤情況.從圖中可以看出，對于每一維噪聲參數不確定的量測，在給定初始值后，濾波器很快估計出各自的噪聲統計特性，而當后來真實噪聲方差發生變化時，還能對其進行動態跟蹤，并有著較為可觀的估計精度.而傳統卡爾曼濾波器因為假設模型的所有參數已知，并不會隨時間推移而發生變化，因此不具備自適應濾波功能，這也限制了其在工程領域內的廣泛應用.

值得注意的是，當噪聲方差發生突變后，跟蹤曲線會略微延遲，后經過快速調整跟蹤到真實值.這是由于利用變分貝葉斯方法對參數估計本質上是一個逐步逼近的過程，它不同于其他采樣的參數估計方法，需要通過反復迭代計算參數的近似分布，當下界的變化可以忽略時才能認為近似分布為參數的真實分布，即給出合理的參數估計值.所以，如果當噪聲保持不變，變分貝葉斯學習算法就會迅速收斂(通常一到兩次迭代循環)，更快地估計出方差值.

圖7 濾波器噪聲方差跟蹤

4 結語

針對實際噪聲統計特性未知或變化的GPS/ INS組合導航系統，本文提出的基于變分貝葉斯學習的自適應卡爾曼濾波算法，用變分貝葉斯學習將觀測噪聲的方差作為隨機變量進行估計，實時跟蹤噪聲，同時能對系統狀態進行良好的估計，具有令人滿意的濾波精度，在工程上較有應用價值.

參考文獻

［1］KALMAN R E.A new approach to linear filtering and prediction problems［J］.Transactions of the ASME，Journal of Basic Engineering，1960，82(D):35-45.

［2］KIM K H，LEE J G，CHAN G P.Adaptive two-stage Kalman filter in the presence of unknown random bias［J］.International Journal of Adaptive Control and Signal Processing，2006，20(7):305-319.

［3］HSIEH C S.On the optimality of two-stage Kalman filtering for systems with unknown inputs［J］.Asian Journal of Control，2010，12(4):510-523.

［4］MEHRA R.Approaches to adaptive filtering［J］.IEEE Trans.on Automatic Control，1972，17(5):693-698.

［5］BISHOP C M.Pattern recognition and machine learning［M］.New York:Springer，2007:462-469.

［6］BEAL M J.Variational algorithms for approximate Bayesian inference［D］.London:University College London，2003: 53-70.

［7］ATTIAS H.Inferring parameters and structure of latent variable models by variational Bayes［C］//Proceedings of the 15th Conference Annual Conference on Uncertainty in Artificial Intelligence.San Francisco，CA:Morgan Kaufmann，1999:21-30.

［8］SARKKA S，NUMMENMAA A.Recursive noise adaptive Kalman filtering by variational Bayesian approximations［J］.IEEE Transactions on Automatic Control，2009，54 (3):596-600.

［9］ SUN Junlong，ZHOU Jie，GU Xuekui.Variational Bayesian two-stage Kalman filterforsystemswith unknown inputs［J］.Procedia Engineering，2012，29 (2012):2265-2273.

［10］VRETTAS M D，CORNFORD D.Estimating parameters in stochastic systems:a variational Bayesian approach［J］.Physica D:Nonlinear Phenomena，2011，240 (23):1877-1900.

［11］BOUJEMMA A，RODET T.Variational Bayesian Kalman filtering in dynamicaltomography［C］// Proceedings of International Conference on Acoustics，Speech，and Signal Processing.Prague:［s.n.］，2011: 4004-4007.

［12］ARMAGEN A，ZARETZKI R L.A note on mean-field variational approximations in Bayesian probit models［J］.Computational Statistics＆Data Analysis，2011，55(1):641-643.

［13］BOYD S，VANDENBERGHE L.Convex optimization［M］.Cambridge:Cambridge University Press，2004: 67-78.

［14］付夢印，鄧志紅，閆麗萍.Kalman濾波理論及其在導航系統中的應用［M］.第2版.北京:科學出版社，2010:12-15.

［15］YUAN Gannan，YUAN Kefei，ZHANG Hongwei.A variable proportion adaptive federal Kalman filter for INS/ESGM/GPS/DVL integrated navigation system［C］// Proceedings of the 2011 Fourth International Joint Conference on Computational Sciences and Optimization. Washington，DC:［s.n.］，2011:978-981.

［16］KARNY M，DEDECIUS K.Approximate Bayesian recursive estimation:on approximation errors［R］. Prague:UTIA AV CR，2012:3-7.