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

劉向陽

（海軍工程大學 武漢 430000）

1 引言

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

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

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

2 非線性濾波方法

2.1 擴展卡爾曼濾波（EKF）

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

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

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

1）狀態的一步預測：

2）協方差的一步預測：

3）量測的一步預測：

4）新息協方差：

5）濾波器增益：

6）狀態更新：

7）協方差更新：

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

2.2 粒子濾波（PF）

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

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

權值遞推公式如下：

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

2.3 無跡卡爾曼濾波（UKF）

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

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

1）計算2nx+1個Sigma采樣點ξi和對應的權值Wi：

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

2）Sigma采樣點ξi經非線性函數傳播得到一組新的樣本點 yi：

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

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

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

2.4 容積卡爾曼濾波（CKF）

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

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

利用一步預測Cubature點及其對應的權值，便得到狀態和協方差的一步預測：

已知 x^k+1|k和Pk+1|k，構造量測Cubature點：

根據量測方程，可以得到預測量測Cubature點：

則量測的一步預測及協方差為

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

3 仿真分析

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

系統模型為［12］

式中：

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

參數設置為 α=1，β=0，k=2。

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

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

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

圖1 四種典型濾波算法的狀態估計曲線

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

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

表1 四種算法的綜合比較

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

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

4 結語

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