UKF算法與SVDKF算法性能的比較

周 磊，董乃銘，洪振杰

（溫州大學 數學與信息科學學院，浙江 溫州 325035）

0 引 言

卡爾曼濾波技術是信號處理的理論基礎之一，目前，廣泛應用的擴展卡爾曼濾波算法（Extended Kalman Filtering，EKF）和基于Unscented的卡爾曼濾波算法（Unscented Kalman Filtering，UKF）都是在經典卡爾曼濾波（Kalman Filtering，KF）的基礎上改進而來的。EKF算法是將非線性系統模型在當前的估計值處采用Taylor級數展開近似線性化處理，是一種很好的弱非線性濾波技術，但由于忽略Taylor級數展開式的高階項，所以系統模型非線性程度較高時，會帶來較大誤差，造成濾波精度嚴重降低[1-3]；此外，EKF算法在線性化處理時必須采用雅克比（Jacobian）矩陣，這對于一些系統并不適用，如階躍線性系統和非奇異系統等，而且Jacobian矩陣的計算復雜，會引入人為誤差，不利于系統狀態的誤差估計。Julier提出的UKF算法是根據Unscented變換（UT）和經典卡爾曼濾波相結合得到的一種濾波算法[4]。UKF算法采用確定性采樣近似系統狀態后驗概率密度（PDF)，可以避免線性化處理所帶來的誤差，對于任何非線性系統其估計精度可達到Taylor級數展開的三階項，而EKF算法最多只能達到二階項，且不需要計算非線性方程的Jacobian矩陣，比EKF算法具有更好的穩定性；但UKF算法計算sigma樣本點需要對樣本協方差矩陣進行Cholesky分解，由于協方差矩陣的隨機性，在計算過程中會經常遇到病態問題[5-7]，從而影響了濾波的性能，甚至導致濾波發散。基于奇異值分解（SVD）的卡爾曼濾波算法(SVDKF)[8-9]在充分吸收UKF算法優點的基礎上，利用SVD代替Cholesky分解協方差矩陣產生sigma樣本點，能有效抑制狀態協方差矩陣的負定性，提高濾波數值的穩定性。

1 UKF算法

1.1 問題描述

UKF算法是在經典卡爾曼濾波算法的基礎上改進得到的一種貝葉斯濾波算法。UKF算法以Unscented變換（一種非線性變換）為基礎，選擇一組sigma樣本點集，利用每個sigma樣本點，通過Unscented變換后得到樣本點的統計量，估計系統狀態向量在非線性變換后的均值和協方差，然后再在經典卡爾曼濾波算法的框架下進行遞推循環。

UKF算法考慮如下動態系統模型：

其中，xt為系統在t時刻的狀態，yt為系統狀態的觀測；vt-1，wt分別為系統噪聲和觀測噪聲；f(…)為有界的非線性狀態轉移函數，h(…)為有界觀測函數。

貝葉斯估計是在給定觀測數據y1:t的基礎上遞推估計系統的狀態向量x1:t，即系統模型后驗概率分布p(x1:t|y1:t)，尤其是濾波概率分布p(xt|y1:t)。其中，x1:t={x1,x2,…,xt}，y1:t={y1,y2,…,yt}。基于濾波概率分布，就可以對系統狀態進行優化估計得到系統的各個狀態參量，如均值、最大后驗概率、置信度區間等。

1.2 UKF算法過程

（1）Cholesky分解。在UKF算法中產生sigma樣本點需要運用矩陣Cholesky分解，定義如下：設A∈[aij]∈Rn×n為對稱正定矩陣，A=GGT為矩陣A的Cholesky分解。其中G=Rn×n是一個具有正的對角線元素的下三角矩陣，是A的Cholesky分解因子，即A的平方根，記為G=chol(A)。

其中，j=1,2,…,2Na；為方根矩陣的第j列，λ是調整尺度參數，na是狀態的維數；α是一個標量，用于控制每個點到均值的距離(尺度，Scaling)；β是關于系統先驗信息的參數，對于高斯分布β=2最佳；ωi為對應的權值，并且滿足。

（3）時間更新。

（4）觀測更新。

依此步驟依次遞推循環。由此可見，UKF算法的遞推過程便于應用，相較于EKF算法無需對非線性的濾波方程進行Jacobian變換，可以有效解決由系統非線性的加劇而引起的濾波發散問題，具有良好的魯棒性。但在產生sigma樣本點過程中需要對樣本協方差矩陣進行Cholesky分解求矩陣平方根，如果矩陣P奇異，則不能進行Cholesky分解，迭代計算無法繼續進行；此外，Cholesky分解對計算誤差很敏感，運算過程中的有限字長或舍入誤差會經常使協方差矩陣P失去對稱性或正定性，從而產生濾波數值不穩定問題，降低濾波算法的精度。

2 SVDKF算法

為提高計算效率，增強狀態協方差矩陣迭代更新等計算過程中的數值穩定性,利用矩陣奇異值分解對擾動不敏感，具有穩定性，sigma樣本點協方差矩陣所包含的信息可以被其特征值和特征向量充分描述，從而提出SVDKF算法。

2.1 矩陣奇異值分解

對于一個m×n階矩陣A，其秩為r，則存在一個m×m階正交矩陣U和一個n×n階正交矩陣V，使得

如果ATA正定，則（17）式可簡化為：

若A對稱正定，則A=USUT，此時左奇異向量與右奇異向量相等，從而可以減少計算量。矩陣奇異值分解是非常穩定的。不管矩陣A是否奇異，奇異值分解都可以進行，并且分解是惟一的，即對于一個給定的矩陣A，只要對角陣∑形式固定，那么正交矩陣U和V就是惟一值。

2.2 SVDKF算法過程

SVDKF算法與UKF算法相似，只是在sigma樣本點采樣過程中有區別。利用SVD產生sigma樣本點的具體過程如下：

（2）產生新的sigma樣本點。

其中，sj和Uj分別為第j個特征值和相對應的特征向量；ρ為尺度系數，較為恰當的取值范圍是。

3 仿真實驗

將SVDKF算法與EKF算法、UKF算法進行比較，采用以下系統模型對SVDKF算法進行仿真實驗：

其中，過程噪聲服從Gamma分布，vt～Gamma(3,2)；觀測噪聲服從正態分布，nt～N(0,0.00001)；（24）式是采用線性和非線性兩種不同模型觀測系統狀態，觀測總時間T=60，動態系統模型的狀態采用均值估計，即，濾波算法的均方根誤差定義為：

對EKF算法、UKF算法和SVDKF算法進行100次獨立實驗，3種算法均方誤差的均值和方差見表1；在一次獨立實驗中，3種算法的狀態估計如圖1所示。

表1 EKF算法、UKF算法和SVDKF算法均方誤差的均值和方差

圖1 EKF算法、UKF算法和SVDKF算法的狀態估計

由仿真實驗結果可知，在非線性、非高斯場合，SVDKF算法的估計精度高于EKF算法和UKF算法。當0＜T＜30時，UKF算法和SVDKF算法相較于EKF算法可以獲得更好的估計結果；當T≥30時，SVDKF算法和EKF算法都能較好地吻合真實狀態，而UKF算法的狀態估計結果存在著明顯的誤差。因此，SVDKF算法的估計效果最為理想。

4 結束語

SVDKF算法結合了矩陣奇異值分解和UKF算法的優點。該算法以SVD代替Cholesky分解協方差矩陣產生sigma樣本點，具有良好的數值穩定性和魯棒性。與UKF算法相比，可以有效避免協方差矩陣求解平方根出現的病態問題，計算相對簡單，適用于線性和非線性動態系統的濾波計算，具有更好的濾波性能。

[1]Uhlmann J K.Algorithm for multiple target tracking[J].American Science，1992，80(2):128-141.

[2]Chen Z，Haykin S.Bayesian filtering:From Kalman filters to particle filters，and beyond[R].Adaptive Systems Lab:McMaster University，2003.

[3]Youmin Zhang，Guanzhong Dai，Hongcai Zhang.A SVD-based extended Kalman filter and applications to aircraft flight state and parameter estimation[C]//Proceeding of the American Control Conference.Piscataway:IEEE Press，1994:1809-1813.

[4]Julier S J，Uhlmann J K，Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariances[J].IEEE Transactions on Automatic Control，2000，45(3):477-482.

[5]Liu Bin，Ma Xiao-chuan，Hou Chao-huan.A particle filter using SVD based sampling Kalman filter to obtain the proposal distribution[C]//International Conference on Cybernetics and Intelligent Systems，2008:581-584.

[6]John L Crassidis，F Landis Markley.Unscented Filtering for Spacecraft Attitude Estimation[J].Journal of Gu ldance and Dynamics，2003，26(4):536-542.

[7]Van der Merwe R，Wan E A.The square-root unscented Kalman filter for state and parameter estimation[C]//Proceedings of the International Conference on Acoustics，Speech，and Signal Processing(ICASSP)，2001:3461-3464.

[8]Xiaoqin Zhang，Weiming Hu.SVD based Kalman particle filter for robust visual tracking[C]//19th International Conference on Pattern Recognition(ICPR)，2008:1-4.

[9]XIE Qiang，TANG He-sheng，DI Yuan.SVD-unscented kalman filter for nonlinear structural system identification[J].Chinese Journal of Applied Mechanics，2008，25(1):57-61.