基于采樣點正交變換的改進CKF算法

趙 麗，薛建平

1.陜西國際商貿學院 商學院，陜西 咸陽 712046

2.空軍工程大學 航空航天工程學院，西安 710038

1 引言

從統計學意義上講，隨機量的概率密度函數可以表征所有的統計信息。遞歸貝葉斯估計（Recursive Bayesian Estimation，RBE）綜合利用了概率論中的貝葉斯準則以及隨機量的Markov特性，為狀態概率密度推演提供了一個最優的、精確的框架模型。但是，在對概率密度進行遞歸求解時RBE涉及多個多維積分，一般情況下無法求得其解析解。因此，雖然RBE在理論和形式上是“完美的”，但在實際應用中，需進行各種近似以求解出多維積分解析解。

遞歸貝葉斯估計中多維積分的近似方法一般可以分為兩類：全局方法和局部方法。全局方法中不需要對概率密度函數做任何近似，只是將多維積分用特定的數值仿真的方法進行近似求解，例如，網格濾波算法（Grid-based Filter，GBF）[1]和粒子濾波算法（Particle Filter，PF）[2]。局部方法是指對遞歸貝葉斯估計框架中概率密度函數進行特定假設近似的方法，其中最常用的假設分布是高斯分布，如高斯埃爾米特卡爾曼濾波（Gussian Hermitian Kalman Filter，GHKF）[3]、無跡卡爾曼濾波（Unscented Kalman Filter，UKF）[4]、差分濾波（Divided Difference Filter，DDF）[5]、容積卡爾曼濾波（Cubature Kalman Filter，CKF）[6]，高階容積卡爾曼濾波（Highdegree CKF，HCKF）[7]和稀疏網格濾波（Sparse-Grid Quadrature Filter，SGQF）[8]等在內的多種次優非線性濾波方法，在各領域都取得了較好的應用效果[9-11]。

傳統UKF算法基于對稱采樣原則，可調參數選為κ=3-n，可以從無跡變換（Unscened Transform，UT）傳遞的均值和方差中獲得更豐富的高階矩信息。但若n＞3，κ為負值，UT傳遞方差時易出現非正定問題。為此，文獻[6]在高斯濾波框架下提出了一種基于容積變換（Cubature Transform，CT）的CKF算法。實際上，從數值積分角度分析，將UKF的可調參數κ置零，便可得到CKF算法，由此證明CKF是一種特殊的基于對稱采樣的UKF算法。

事實上，CKF雖然解決了傳統UKF的數值不穩定問題，但同時又引入了非局部采樣問題。原因在于CKF采樣時方差矩陣需乘系數，維數較高時，采樣點距中心點的距離變大，影響了濾波精度。因此，如何同時解決UKF算法的數值不穩定和CKF算法的非局部采樣是一個值得研究的問題。

為此，在分析CKF采樣原理和CKF局限性的基礎上，設計出一種正交變換矩陣，通過對CKF的采樣點進行變換，進而導出一組新的采樣點，并基于此提出了改進的CKF算法，以解決高斯濾波框架下算法存在數值不穩定和非局部采樣的問題，最后通過非局部采樣問題影響明顯的系統模型驗證了算法的有效性。

2 UKF與CKF的數值積分

考慮單位高斯分布權函數的數值積分[12-13]

用以下2n個加權采樣點構造I[g(x)]的三階精度的數值積分近似：

其中，ei表示除第i個分量為1外其他分量全為零的n維向量。式（2）滿足以下關系：

其中，標量xi表示向量x的第i個分量。

文獻[4]導出的UKF算法用于近似單位高斯分布權函數的三階精度數值積分的加權采樣點為：

文獻[6]導出的CKF算法用于近似單位高斯分布權函數的三階精度數值積分的加權采樣點為：

3 CKF算法的局限性分析

UKF算法中可調參數選為κ=3-n，對于維數n＞3的非線性濾波問題，中心點處sigma點的權重為負值，容易導致傳遞的方差非正定，進而造成濾波算法的數值不穩定。Arasaratnam提出的CKF算法在高維問題中較UKF具有更高的精度和穩定性。

雖然CKF解決了UKF算法數值不穩定性的問題，但是自身又存在非局部采樣問題。非局部采樣問題是指基于采樣點的非線性濾波算法，其使用的采樣點偏離中心點（均值處）很大，造成濾波精度的下降。由式（5）看出，UKF中每個采樣點與中心點的距離與成比例，κ一般取3-n，因此維數增加不會引發非局部采樣問題。由于CKF沒有可調參數對狀態的擴張進行有效的抵消，每個采樣點與中心點的距離與n成比例，隨著維數的增加會產生非局部采樣問題。

4 改進CKF的導出

為更好地說明非局部采樣問題產生的原因，分析一個典型的UT過程。

考察如下非線性傳遞函數：

將每個sigma點經非線性變換進行傳遞：

傳遞后y的均值和方差分別為：

采用多元Taylor級數展開的方法對UT變換的精度進行分析。定義如下變量：

其中，σxi,j是σxi的第 j個分量。對式（9）中任意傳遞sigma點在處進行Taylor級數展開：

其中，?/?xj表示對應于x（jx的第j個分量）的偏導數。當 l=1時，式（14）可以表示為由于sigma點關于均值處是對稱的，所以式（13）中所有奇次冪項都是零。根據式（10）和式（13），UT傳遞后的均值可表示為：

根據文獻[14-15]，傳遞后的均值可精確到三階，而誤差只在四階及其以上的信息中引入。對UT傳遞均值的第 j個分量，其四階及以上信息可表示為：

其中，hom()

κ表示這些高階信息是可調參數κ的函數。對于CKF而言，κ=0，上式可進一步表示為：

從以上公式的推導過程可以看出，隨著維數的增加，CKF傳遞均值的高階信息顯著增加。在實際應用中，真實值的高階矩信息是未知的，因此使用UKF或CKF計算的均值的高階矩信息可能更接近真實信息，也可能偏離真實信息[16]。相對于某一算法最優表現，更關心的其最差表現，對于基于采樣點的非線性濾波算法而言，使其高階信息趨于0不失為一種好的選擇。基于以上討論和認識，下面導出本文所要提出的基于采樣點正交變換的改進CKF算法。

由式（2）可知：

ξ表示三階精度數值積分公式中所采用的一組采樣點集，滿足如下關系式：

其中，ξk表示ξ點集中的第k個向量，ξk,i表示向量ξk第i個元素。一般研究等權重對稱分布的采樣點，如果ξ中每個采樣點都是等權重對稱分布的話，式（19）和式（21）是自然滿足的。因此在構造三階精度數值積分公式中等權重對稱分布采樣點時只需考慮式（20）。

式（20）對應的矩陣形式為：

對一組采樣點進行正交變換不會改變其對性，因此Cξ自然滿足式（19）和（21）。將式（22）中ξ替換為Cξ可得

因此Cξ也是三階精度數值積分公式中所采用的一組采樣點，則有如下結論：

假設ξ是三階精度數值積分公式中所采用的一組采樣點集，C是與ξ行維數一致的正交矩陣，則Cξ也是三階精度數值積分公式中所采用的一組相同權重的采樣點集。

定理1n×n矩陣B其列集的形式為：

證明（1）首先假設n是偶數，則：

因此

綜合式（30）和式（35）可得 B?BT=In。綜上，定理得證。

可以利用式（24）中的正交矩陣B對CKF中的采樣點進行正交變換得到一組新的滿足三階精度數值積分公式的采樣點。為了使得到的新的采樣點不存在數值不穩定問題，利用B對式（19）中的cubature點進行正交變換。

首先將式（6）中的cubature點表示成矩陣形式：

則ξ經B正交變換后為：

利用正交變換矩陣B對CKF中的sigma進行變換，即可得到一種新的高斯濾波算法——ICKF（Improved CKF）。

5 仿真驗證

5.1 維數可變的強非線性系統

考慮如下維數可變的強非線性系統模型：

圖1 改進CKF算法流程示意圖

其中，xk是n維高斯隨機變量，n可變；系統噪聲wk-1～N(w;0,In)，觀測噪聲為 vk～N(v;0,1)。為了比較不同維數條件下CKF和ICKF算法的性能，維數n的變化范圍選取為1～50。仿真時間K=500，仿真時用于產生仿真數據的初始真值 x0=0.1×1n×1，其中 1n×1表示n×1維矩陣的每一個元素都為1。仿真的初始濾波條件設置為，各濾波算法的性能采用如下定義的評價指標進行比較。

其中，M=50表示Monte Carlo仿真次數，xk(1)表示xk的第一個元素。圖2給出了不同維數下EKF、CKF、ICKF三種濾波算法的MRMSE。

圖2 不同維數下三種濾波算法的MRMSE

從圖2中可以看出，CKF和ICKF的濾波精度始終優于EKF，這是因為CKF和TCKF所使用的CT方法可以精確到三階，而EKF中所使用的線性傳遞方法則只能精確到一階。CKF算法的性能隨著維數的增加而逐漸降階，這說明CKF中通過cubature點傳遞所捕捉的高階矩信息與真實值之間逐漸偏離。相反，ICKF算法中通過transformed sigma點傳遞所捕捉的高階矩信息基本為0，因此基本不受維數變化的影響。

為了更加充分地說明ICKF相對于CKF的精度優勢，圖3給出了n=30時三種濾波算法50次Monte Carlo仿真的RMSE，圖4給出了RMSE的均值和標準差。

圖3 n=30時50次Monte Carlo仿真的RMSEs

圖4 50次Monte Carlo仿真的RMSEs均值和標準差

從圖中可以明顯看出，在高維、強非線性濾波問題中，ICKF相對于CKF具有更高的精度，從而驗證了ICKF算法的有效性。

5.2 角測量跟蹤模型

角測量模型描述的是一個二維平面內的動態目標跟蹤問題，具體數學模型為：

為了公平地比較兩種非線性濾波算法，進行100次獨立Monte Carlo仿真實驗并取其平均，圖5給出了動態目標的跟蹤軌跡。

圖5 運動目標的跟蹤軌跡

由圖5可見，當初始協方差矩陣誤差較大時，CKF和ICKF都能很快收斂，但ICKF的收斂速度更快，初始抖動更小，其跟蹤軌跡也更為接近真實軌跡。

定義時刻的位置均方誤差為：

類似的，定義k時刻的速度均方誤差RMSEvel。位置和速度跟蹤均方誤差分別如圖6和圖7所示。

圖6 位置跟蹤均方誤差

圖7 速度跟蹤均方誤差

圖6 和圖7表明，ICKF對運動目標位置和速度的跟蹤效果均更優，其初始誤差曲線更為平滑、誤差更小。

5.3 空中目標跟蹤模型

將ICKF應用到空中目標跟蹤系統中，并與CKF進行對比。假設目標在以未知角速度Ω等高度機動飛行，飛行高度h=10 000 m，運動模型為：

其中，T為測量時間間隔；高斯白噪聲vk-1～N(0,Qk)，協方差陣Qk=diag(M1,M1,M2)，M1=q1T[T2/3,T/2;T/2,1]，M2=q2TI，q1和q2為過程噪聲強度參數。

坐標系原點的測量雷達獲取目標的斜距η和方位角θ信息。測量向量為yk=[ηkθk]T，滿足：

其中，wk～N(0,Rk)為量測噪聲，為協方差陣，ση和σθ為雷達斜距測量噪聲和方位角測量噪聲的標準差。

定義均方根誤差和均方根誤差均值為：

表示位置、速度和轉速的均方根誤差及其均值，用于考察算法的跟蹤性能。其中和分別為第i次打靶中第k時刻的目標真實位置和估計值；N為蒙特卡洛仿真次數；?分別取為pos、vel和omg。每次仿真中所有濾波方法初始狀態相同，進行100次蒙特卡洛仿真實驗，仿真時間為100 s。

圖8 目標位置估計均方根誤差

圖9 目標速度估計均方根誤差

圖10 目標角速度估計均方根誤差

值得說明的是，所提改進算法是基于正交變化的改進CKF算法，所針對的問題是CKF算法應用于高維問題時出現的非局部采樣問題而導致估計精度下降的問題。改進之處體現在對CKF算法的采樣點進行了一次正交變換，沒有改變采樣點的個數和權值，在以上的仿真實驗應用中，改進CKF算法所采用的采樣點只需要一次定義即可，沒有增加計算的復雜度，計算量與CKF相當。

6 結束語

本文提出了一種新的基于采樣點正交變換的ICKF算法，由于只是對采樣點進行變換，采樣點的權值與傳統CKF相同，保證了濾波算法的穩定性，經過正交變換后的采樣點傳遞真實均值和方差的高階矩信息基本為0，從而進一步提高了CKF算法在高維估計問題中的濾波性能。將此方法與現有的各種改進的CKF算法結合，可以取得更好的估計效果。

下一步的工作目標是將所提算法與已有的各種CKF改進算法相結合，更進一步地提高ICKF的濾波性能，并拓展其在諸如動基座的初始對準，慣性導航等領域中的實際工程應用。