有色噪聲條件下的高斯和卡爾曼濾波算法

馬麗麗，張 曼，陳金廣

（西安工程大學 計算機科學學院，陜西 西安710048）

0 引 言

標準的卡爾曼濾波要求系統的過程噪聲和量測噪聲必須為零均值的高斯白噪聲。白噪聲只是一種理想狀態，只有在噪聲的相關性比較弱，可以忽略不計的條件下，才可以將其近似地表示成白噪聲。在一個實際的系統中，噪聲往往是相關的 （即噪聲為有色噪聲），或者滿足非高斯分布。此時，標準卡爾曼濾波算法的應用受到了限制。

針對噪聲為非高斯分布的情況，Sorensor等提出一種利用高斯和理論進行遞推貝葉斯估計的算法，該算法只能處理線性系統的狀態估計問題；Alspach等又將高斯和濾波理論和擴展卡爾曼濾波算法進行結合，提出一種相應的非線性貝葉斯估計算法，但擴展卡爾曼濾波只適用于弱非線性系統，當遇到強非線性系統時，該算法濾波精度較低；針對該問題，Kotecha等推導出高斯和粒子濾波算法，該算法能夠有效提高濾波精度，但時間復雜度較高；文獻 ［1］提出一種高斯和容積卡爾曼濾波算法，該算法在保證濾波精度的同時時間復雜度增加不大；以往的算法中，高斯和權值的計算都是從一個時刻傳播到下一時刻，文獻 ［2］提出一種自適應高斯和濾波算法，與以往算法不同之處在于，高斯和的權值在每一時刻進行更新，該算法能夠提供一個更準確的概率密度函數，以更好地進行狀態估計；文獻［3］在回顧非線性濾波算法的同時，也介紹了高斯和濾波算法的研究進展。

文獻 ［4］將有色噪聲條件下的卡爾曼濾波算法用在電池SOC的估計問題中，實驗結果表明，該濾波算法可以提高有色噪聲影響下電池SOC的估計精度；當系統的量測噪聲為有色噪聲時，文獻 ［5，6］提出一種基于高斯馬爾科夫過程的相關觀測噪聲函數模型估計方法，將其分別運用到GPS高頻動態變形監測和橋梁自振頻率提取模型的研究中，與標準卡爾曼濾波的處理方法相比，該算法在GPS高頻動態變形監測中有更好的濾波效果，在橋梁自振頻率提取模型中能夠得到更真實的橋梁振動頻率；文獻 ［7－9］對過程噪聲或者量測噪聲為有色噪聲的處理方法做了介紹，總結出相應的濾波方程；此外，文獻 ［8］驗證了標準卡爾曼濾波算法是有色噪聲卡爾曼濾波算法的特例；文獻 ［9］針對非線性系統，推導了量測噪聲為有色噪聲時的濾波算法；文獻 ［10］提出一種有色噪聲條件下的自適應濾波算法，實驗結果表明，該算法能夠取得較好的濾波效果。

上述算法討論了過程噪聲和量測噪聲為非高斯分布，以及過程噪聲和量測噪聲為有色噪聲的濾波方法。本文在此基礎上進行研究，推導出過程噪聲和量測噪聲同時為有色噪聲且滿足非高斯分布的濾波算法，即：有色噪聲條件下的高斯和卡爾曼濾波算法。

1 問題描述

卡爾曼濾波的系統模型為

式中：Xk——系統 的 狀 態 向 量，Φk，k－1——狀 態 轉 移 矩 陣，Γk，k－1——噪聲分布矩陣，Wk－1——過程噪聲，Zk——系統的量測向量，Hk——量測矩陣，Vk——量測噪聲。過程噪聲Wk和量測噪聲Vk是非高斯分布的有色噪聲，且滿足方程

2 有色噪聲條件下的高斯和卡爾曼濾波

當噪聲為非高斯分布或者系統模型的后驗概率密度函數不能用單個高斯分布來近似時，可以使用高斯和濾波算法進行近似處理。高斯和濾波算法的思想是，任意的概率密度函數p（X）都可以使用N 個高斯分布項的疊加進行近似，即采用如下高斯和的形式表示

根據式 （5），式 （3）和式 （4）中的ξk 和ζk 可以采用如下高斯和的形式近似

則式 （3）和式 （4）可進一步表示為

系統初始狀態分布的高斯和形式為

如式 （1）～式 （4）所示，系統的過程噪聲和量測噪聲為非高斯分布的有色噪聲。首先將有色噪聲白化，即將噪聲的形式轉化為非高斯分布的白噪聲序列，在此基礎上結合高斯和濾波理論進行卡爾曼濾波。

采用式 （11）表示的狀態向量，狀態方程和量測方程可以重新描述為

式 （12）和式 （13）可以簡寫為如下形式

根據式 （9）、式 （15）和式 （16）可得

即

可以得到量測方程

其中

因為

所以

由式（31）～式（33）可知，預測方程式（28）～式（30）實際上是濾波方程，因此k＋1時刻的狀態估計方程為

協方差估計方程為

增益矩陣為

每個高斯項的權值為

其中

則k＋1時刻的狀態估計值為

k＋1時刻的協方差估計值為

高斯和濾波算法能夠處理非高斯噪聲的濾波問題，然而隨著時間的不斷推移，高斯項的數目會不斷增加。若初始狀態有K 個高斯項，經過第一步濾波之后有KLM 個高斯項，第k步濾波之后有KLkMk個高斯項。高斯項數目呈指數增加，這就意味著計算量不斷增加，使其應用受到限制。因此需要通過修剪與合并來降低高斯項的數目。

根據具體情況設置最大高斯項數目G、最小權值閾值ath及兩個高斯項之間的最大距離閾值D 。每一步濾波結束后判斷高斯項數目，如果高斯項數目大于G，則保留其中權值較大的G 個高斯項；否則在剩余的高斯項中，將權值小于ath的高斯項與鄰近的權值大于ath的高斯項進行合并。

采用馬氏距離，即式 （43）計算兩個高斯項之間的距離，進而判斷兩個高斯項是否鄰近

式中：αm，μm，Σm——第m 個高斯項的權值、均值和協方差，αn，μn，Σn——第n個高斯項的權值、均值和協方差。當兩個高斯項的距離小于D 時，說明兩個高斯項鄰近，將這兩個高斯項合并，合并后的高斯項的權值、均值和協方差的計算公式如下

如果權值小于ath的高斯項未找到與其鄰近且權值大于ath的高斯項，則將其丟棄。

可以證明，當式 （3）中的Πk，k－1＝0時，上述濾波過程即為有色量測噪聲條件下的高斯和卡爾曼濾波算法；當式 （4）中的Ψk.k－1＝0，上述濾波過程即為有色過程噪聲條件下的高斯和卡爾曼濾波算法；當Πk，k－1＝0且Ψk.k－1＝0時，本文算法退化為標準的高斯和卡爾曼濾波算法。

3 仿真實驗及結果分析

仿真實驗中，將有色噪聲條件下的高斯和卡爾曼濾波算法與采用白噪聲處理的高斯和卡爾曼濾波算法進行對比，從濾波效果可以看出本文算法的可行性。

設一維線性系統的模型為

其中，系統過程噪聲Wk－1和系統量測噪聲Vk為有色噪聲，且分別滿足如下方程式

其中，Πk，k－1＝1，Ψk.k－1＝0.5，ξk－1 和ζk－1 是滿足非高斯分布的白噪聲。采用白噪聲處理的高斯和卡爾曼濾波算法時，系統的過程噪 聲Wk＝ξk－1 ，量 測 噪 聲Vk＝ζk－1 ，ξk－1 和ζk－1同樣是滿足非高斯分布的白噪聲。這里假設系統的初始狀態、過程噪聲和量測噪聲分別為兩個高斯分布的疊加

式中：α1＝0.5，α2＝0.5，X01＝10，X02＝10，P01＝0.05，P02＝0.15，β1 ＝0.8，β2 ＝0.2，mQ1＝0，mQ2＝0，Q1＝0.05，Q2＝0.3，γ1＝0.8，γ2＝0.2，mR1＝0，mR2＝0，R1＝0.5，R2＝3。

分別采用有色噪聲條件下的高斯和卡爾曼濾波算法（colored GS－KF）和按照白噪聲處理的高斯和卡爾曼濾波算法 （GS－KF）進行狀態估計，整個Monte Carlo仿真次數為20，每一次Monte Carlo仿真的時間步數為100。圖1和圖2分別是進行一次Monte Carlo仿真得到的狀態估計和狀態誤差，表1是進行20次Monte Carlo仿真得到的兩種算法的均方根誤差和耗費時間。

表1 兩種算法的均方根誤差和時間

圖1 兩種算法的狀態估計

圖2 兩種算法的誤差

由圖1可見，有色噪聲條件下的高斯和卡爾曼濾波算法的狀態估計結果接近真實狀態，兩者基本吻合；而按照白噪聲處理的高斯和卡爾曼濾波算法，在第10時刻之前基本接近真實狀態，然而在第10時刻之后，其濾波結果與真實值偏差較大。由圖2可見，有色噪聲條件下的高斯和卡爾曼濾波算法誤差較小，按照白噪聲處理的高斯和卡爾曼濾波算法的誤差較大。由表1給出的具體數值可以進一步看出，雖然有色噪聲條件下的高斯和卡爾曼濾波算法的時間復雜度相對較高，但其均方根誤差小，濾波精度高。綜上所述，當系統的過程噪聲和量測噪聲為非高斯分布的有色噪聲時，本文算法能夠更好地跟蹤目標狀態的變化，有效提高濾波精度。

4 結束語

針對噪聲既為有色噪聲又滿足非高斯分布這一情況，本文以卡爾曼濾波為濾波基礎，采用狀態擴增法和量測擴增法，將有色噪聲轉化為白噪聲序列，在此基礎上依據高斯和濾波理論處理非高斯噪聲問題，推導出過程噪聲和量測噪聲同時為有色噪聲時的高斯和卡爾曼濾波算法。該算法能有效提高濾波精度，拓展卡爾曼濾波算法在工程實踐中的應用范圍。本文只在卡爾曼濾波基礎上做了相應的推導和具體實現工作，對于其它濾波算法，例如擴展卡爾曼濾波、不敏卡爾曼濾波等，也可以進行類似的推導。

［1］Leong PH，Arulampalam S，Lamahewa TA，et al.A Gaussiansum based cubature Kalman filter for bearings－only tracking ［J］.IEEE Trans on Aerospace and Electronic Systems，2013，49 （2）：1161－1176.

［2］Tereganu G，Singla P，Singh T，et al.Adaptive Gaussian sum filter for nonlinear Bayesian estimation ［J］.IEEE Trans on Automatic Control，2011，56 （9）：2151－2156.

［3］Pawe S，Zsofia L，Jelmer B，et al.Parametric Bayesian filters for nonlinear stochastic dynamical systems：A survey ［J］.IEEE Trans on Cybernetics，2013，43 （6）：1607－1624.

［4］Du Lianbo，Cheng Ximing.The research on battery SOC estimation within colored noise［C］／／IEEE Conference and Expo，Transportation Electrification Asia－Pacific （ITEC Asia－Pacific），2014：1－5.

［5］ZHANG Haonan，KUANG Cuilin，DAI Wujiao.Kalman filtering considering colored noise in the application of monitoring GPS high frequency dynamic deformation ［J］.Engineering Survey，2013，41 （4）：50－54 （in Chinese）. ［張昊楠，匡翠林，戴吾蛟.顧及有色噪聲的Kalman濾波在GPS高頻動態變形監測中的應用 ［J］.工程勘察，2013，41 （4）：50－54.］

［6］HAN Houzeng，WANG Jian，MA Changzhong.Research on the model of self－vibration frequency of bridge extraction of colored noise based on Kalman filter［J］.Geodesy and geodynamics，2012，32（1）：123－126（in Chinese）.［韓厚增，王堅，馬昌中.基于有色噪聲的Kalman濾波的橋梁自振頻率提取模型研究 ［J］.大地測量與地球動力學，2012，32 （1）：123－126.］

［7］FU Mengyin，DENG Zhihong，YAN Liping.Kalman filtering theory and its application in navigation system ［M］.2nd ed.Beijing：Science Press，2010 （in Chinese）. ［付夢印，鄧志紅，閆莉萍.Kalman 濾波理論及其在導航系統中的應用［M］.2版.北京：科學出版社，2010.］

［8］ZHAO Changsheng，TAO Benzao.Kalman filter under colored noise［J］.Journal of Wuhan University （Information Science Edition），2008，33 （2）：180－182 （in Chinese）.［趙長勝，陶本藻.有色噪聲作用下的卡爾曼濾波 ［J］.武漢大學學報 （信息科學版），2008，33 （2）：180－182.］

［9］ZHAO Changsheng.Colored noise filter theory and algorithm［M］.Beijing：The Publishing House of Surveying and Mapping，2011 （in Chinese）.［趙長勝.有色噪聲濾波理論與算法［M］.北京：測繪出版社，2011.］

［10］CUI Yuanyuan， WANG Boxiong，LIU Jiannan，et al.Adaptive filtering in digital ultrasonic signal with colored noise ［J］.Optics and Precision Engineering，2014，22（12）：3377－3383 （in Chinese）. ［崔 園園，王 伯雄，柳 建楠，等.數字超聲波信號中有色噪聲的自適應濾波 ［J］.光學精密工程，2014，22 （12）：3377－3383.］