幾種典型非線性濾波算法及性能分析

劉向陽

（海軍工程大學(xué) 武漢 430000）

1 引言

在應(yīng)用卡爾曼濾波［1］方法解決許多實際問題時，狀態(tài)方程或量測方程常常表現(xiàn)為非線性，即涉及到非線性濾波問題［2～4］。為此，學(xué)者們提出了大量次優(yōu)的近似方法，主要分為兩大途徑［5］：一是將系統(tǒng)方程中非線性部分線性化，即對泰勒級數(shù)展開，對高階項采用忽略或逼近的方法；二是利用采樣的方法，對近似非線性分布和非線性函數(shù)進行線性化近似。

在非線性濾波算法中，擴展卡爾曼濾波（ExtendedKalmanFilter，EKF）［6］算法最為常見，但由于EKF算法對非線性狀態(tài)方程或非線性量測方程進行了線性化處理，忽略了高階項，在強非線性情況下其跟蹤效果往往較差。為了克服這一問題，專家學(xué)者提出使用采樣方法逼近非線性分布來解決非線性問題，典型的算法是粒子濾波（ParticleFilter，PF）［7］算法。雖然PF取得了很好的濾波效果，但因其計算量大、實時性不高，并沒有在工程中廣泛應(yīng)用。近年來，無跡卡爾曼濾波（UnscentedKalmanFilter，UKF）［8］和容積卡爾曼濾波（Cubature KalmanFilter，CKF）［9］得到了廣泛關(guān)注。UKF 和CKF根據(jù)前一時刻的狀態(tài)估計值選取典型的采樣點，經(jīng)非線性傳遞后，對這些采樣點作加權(quán)處理，得到狀態(tài)估計均值和協(xié)方差的近似值，減小了線性化處理帶來的誤差，且不需要計算Jacobian矩陣。與UKF相比，CKF在高維濾波時估計精度與濾波穩(wěn)定性都優(yōu)于UKF，并且其容積點及其權(quán)值僅由狀態(tài)的維數(shù)唯一確定，可以提前計算與存儲［10］。

目前為止，在動態(tài)的非線性估計領(lǐng)域之中，并不存在一種“最好”的算法。任何濾波器的設(shè)計，都需要在估計精度、復(fù)雜度、數(shù)值穩(wěn)健性及計算量等各種指標之間綜合考慮。本文對幾種典型的非線性濾波方法的一般原理和濾波步驟進行了介紹，并引用一個經(jīng)典的非線性例子對各濾波器性能進行了詳細對比分析，為解決實際的非線性估計問題的處理提供了參考。

2 非線性濾波方法

2.1 擴展卡爾曼濾波（EKF）

EKF是利用Taylor展開式中的一次項對狀態(tài)方程和量測方程中非線性函數(shù)和進行線性化處理，EKF濾波步驟簡述如下［6］：

先計算Jacobian矩陣Fk和 Hk+1：

在標準卡爾曼濾波框架下進行遞歸濾波，具體的遞推步驟如下：

1）狀態(tài)的一步預(yù)測：

2）協(xié)方差的一步預(yù)測：

3）量測的一步預(yù)測：

4）新息協(xié)方差：

5）濾波器增益：

6）狀態(tài)更新：

7）協(xié)方差更新：

EKF算法簡單，易于實現(xiàn)，是比較常用的非線性濾波算法。利用EKF對目標進行跟蹤，只有當(dāng)系統(tǒng)的動態(tài)模型和觀測模型都接近線性時，也就是線性化模型誤差較小時，EKF才有可能就近于真實值，否則將會產(chǎn)生很大的誤差；此外，EKF對初始狀態(tài)的估計精度較高，如果估計的狀態(tài)初始值和初始協(xié)方差誤差較大時，可能導(dǎo)致濾波發(fā)散。

2.2 粒子濾波（PF）

PF是一種基于MonteCarlo仿真的最優(yōu)回歸貝葉斯濾波算法，它將狀態(tài)矢量表示為帶有權(quán)值的隨機樣本，并利用樣本和權(quán)值做狀態(tài)估計。與EKF相比，粒子濾波不受線性化誤差或噪聲模型的限制，適用于任何環(huán)境下的任何系統(tǒng)模型。PF模型簡述如下［7］：

k時刻的后驗概率密度可以表示為

權(quán)值遞推公式如下：

在粒子濾波運算中，經(jīng)過多次迭代，某些粒子權(quán)值會逐漸變小，樣本方差逐漸增大，即發(fā)生了粒子退化現(xiàn)象。通常采用兩種措施避免粒子退化，即重采樣和選取好的重要性概率密度函數(shù)，后者的衡量標準是使重要性概率密度函數(shù)容易采樣和使權(quán)值的方差最小［11］。

2.3 無跡卡爾曼濾波（UKF）

UKF算法的核心是UT變換，UT是一種計算非線性變換中的隨機變量的統(tǒng)計特征的新方法，是UKF的基礎(chǔ)。

假設(shè)x為一個nx維隨機向量，g:Rnx→Rny為一非線性函數(shù)且y=g()x，x的均值和協(xié)方差分別為 xˉ和 Px，計算UT變換的步驟可簡單敘述如下［8］：

1）計算2nx+1個Sigma采樣點ξi和對應(yīng)的權(quán)值Wi：

式中，κ是一個尺度參數(shù)，可以是滿足( )nx+κ ≠0的任何數(shù)值，是( )nx+κPx均方根矩陣的第i行或第i列。

2）Sigma采樣點ξi經(jīng)非線性函數(shù)傳播得到一組新的樣本點 yi：

3）用這組帶權(quán)值的新樣本點計算非線性變換后的均值 yˉ和協(xié)方差Py：

由于x的均值和方差都精確到二階，計算得到y(tǒng)的均值和方差也精確到二階，比線性化模型精度更高。在卡爾曼框架內(nèi)應(yīng)用UT技術(shù)就得到了UKF算法。

UKF是利用確定的采樣來近似狀態(tài)的后驗概率密度，不需要對模型進行線性化，可以有效地解決由系統(tǒng)的非線性加劇而引起的濾波發(fā)散問題，但是UKF仍是用高斯分布來逼近系統(tǒng)的后驗概率密度，所以在系統(tǒng)的后驗概率密度是非高斯情況下，濾波結(jié)果將會產(chǎn)生極大的誤差。

2.4 容積卡爾曼濾波（CKF）

CKF算法也是依據(jù)高斯濾波框架，其核心是采用三階球面—徑向容積規(guī)則近似非線性函數(shù)傳遞的后驗均值和協(xié)方差。與UT變換類似，三階球面—徑向容積規(guī)則依據(jù)狀態(tài)的先驗均值和協(xié)方差，通過容積規(guī)則選取容積點，再將這些容積點經(jīng)非線性函數(shù)傳遞，再用傳遞后的容積點加權(quán)處理來近似狀態(tài)后驗均值和協(xié)方差。計算步驟簡敘如下［9］：

然后 ηi，k通過非線性函數(shù) fk(·)傳播，得到Cubature點的一步預(yù)測：

利用一步預(yù)測Cubature點及其對應(yīng)的權(quán)值，便得到狀態(tài)和協(xié)方差的一步預(yù)測：

已知 x^k+1|k和Pk+1|k，構(gòu)造量測Cubature點：

根據(jù)量測方程，可以得到預(yù)測量測Cubature點：

則量測的一步預(yù)測及協(xié)方差為

如果k+1時刻傳感器提供的量測為zk+1，進行狀態(tài)濾波更新，便可得到k+1時刻的狀態(tài)估計和協(xié)方差。

3 仿真分析

為了比較EKF、UKF、CKF和PF這四種典型濾波算法的性能，這里引用一個經(jīng)典的非線性例子進行仿真。

系統(tǒng)模型為［12］

式中：

初始參數(shù)設(shè)定如下：系統(tǒng)噪聲vk～N( )0，0.1 ，觀測噪聲wk～N( )0，1，初始狀態(tài)x0=0.1，初始方差p0=5，時間序列長度為T=50。

參數(shù)設(shè)置為 α=1，β=0，k=2。

每次獨立仿真實驗的均方根誤差定義為［13］

在一次獨立實驗中，目標狀態(tài)的真實值與不同的非線性濾波算法對系統(tǒng)狀態(tài)估計如圖1～2所示。

圖1是一階EKF、UKF、CKF和PF四種濾波算法在非線性系統(tǒng)、高斯白噪聲條件下的50次Monte Carlo仿真估計均值曲線。圖2是四種估計算法在高斯噪聲條件下做目標跟蹤狀態(tài)估計的均方誤差曲線。由圖可以看出，在某些時段一階EKF的狀態(tài)估計嚴重偏離目標的真實軌跡，均方誤差很大，估計效果很不穩(wěn)定；UKF的仿真效果比EKF好得多，基本能夠跟蹤估計出目標的運動軌跡，均方誤差比EKF小了很多，估計效果比較穩(wěn)定；CKF在大多數(shù)時刻能夠比較準確的估計出運動目標的狀態(tài)，估計誤差保持在較小的狀態(tài)；PF的估計效果也比較理想，大多數(shù)時刻能夠比較準確地估計出目標的真實狀態(tài)，只有少數(shù)時刻估計值與真實值有偏差，PF的均方誤差保持在較小的狀態(tài)。

圖1 四種典型濾波算法的狀態(tài)估計曲線

圖2 四種典型濾波算法估計值的誤差曲線

下面對以上四種非線性濾波算法做綜合比較。

表1 四種算法的綜合比較

由表1可得，一階EKF在四種濾波算法中具有最快的計算速度，這在目標跟蹤及實時狀態(tài)估計應(yīng)用領(lǐng)域具有非常重要的意義。但EKF的估計精度比較低，估計值和真實值間的均方誤差比較大，且多次計算的均方誤差間的方差比較大，說明多次計算的估計效果浮動很大，估計效果不穩(wěn)定。CKF的計算速度與EKF同屬一個數(shù)量級，UKF的計算速度和估計精度兩項指標都不如CKF，但穩(wěn)定性方面好于后者。PF的濾波精度很高，但濾波復(fù)雜性提高，濾波速度明顯降低。理論上說，隨著粒子數(shù)的不斷增多，PF的濾波精度會不斷提升，當(dāng)粒子數(shù)足夠高時，狀態(tài)估計趨近于真實運動狀態(tài)，但粒子數(shù)越大，計算越復(fù)雜，濾波速度越慢，這限制了PF的推廣和應(yīng)用。但PF的優(yōu)勢在于它對環(huán)境噪聲的普遍適應(yīng)性，EKF、UKF和CKF只適用于高斯噪聲環(huán)境，PF不僅適用于高斯環(huán)境，對于非高斯噪聲環(huán)境同樣適用，這是以上其他濾波方法沒有的功能。

綜上所述，EKF雖然計算量小，但在精度上不及UKF、CKF、PF；PF雖然精度高，但計算量大，實時性不好；而UKF和CKF具有較高的精度和較強的實時性。

4 結(jié)語

本文將四種典型的非線性濾波算法EKF、UKF、CKF和PF進行了對比，并簡單闡述各濾波器的濾波算法步驟以及優(yōu)缺點。最后，通過仿真，將這四種濾波器的性能進行了分析比較。研究結(jié)果表明：1）EKF計算量小，但在精度上不及UKF、CKF和PF；2）PF精度高，但計算量大、實時性不好；3）UKF和CKF具有較高的精度和較強的實時性，CKF計算速度和估計精度更高，而UKF穩(wěn)定性更好。