基于采樣點(diǎn)正交變換的改進(jìn)CKF算法

趙 麗，薛建平

1.陜西國際商貿(mào)學(xué)院 商學(xué)院，陜西 咸陽 712046

2.空軍工程大學(xué) 航空航天工程學(xué)院，西安 710038

1 引言

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

遞歸貝葉斯估計(jì)中多維積分的近似方法一般可以分為兩類：全局方法和局部方法。全局方法中不需要對概率密度函數(shù)做任何近似，只是將多維積分用特定的數(shù)值仿真的方法進(jìn)行近似求解，例如，網(wǎng)格濾波算法（Grid-based Filter，GBF）[1]和粒子濾波算法（Particle Filter，PF）[2]。局部方法是指對遞歸貝葉斯估計(jì)框架中概率密度函數(shù)進(jìn)行特定假設(shè)近似的方法，其中最常用的假設(shè)分布是高斯分布，如高斯埃爾米特卡爾曼濾波（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]和稀疏網(wǎng)格濾波（Sparse-Grid Quadrature Filter，SGQF）[8]等在內(nèi)的多種次優(yōu)非線性濾波方法，在各領(lǐng)域都取得了較好的應(yīng)用效果[9-11]。

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

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

為此，在分析CKF采樣原理和CKF局限性的基礎(chǔ)上，設(shè)計(jì)出一種正交變換矩陣，通過對CKF的采樣點(diǎn)進(jìn)行變換，進(jìn)而導(dǎo)出一組新的采樣點(diǎn)，并基于此提出了改進(jìn)的CKF算法，以解決高斯濾波框架下算法存在數(shù)值不穩(wěn)定和非局部采樣的問題，最后通過非局部采樣問題影響明顯的系統(tǒng)模型驗(yàn)證了算法的有效性。

2 UKF與CKF的數(shù)值積分

考慮單位高斯分布權(quán)函數(shù)的數(shù)值積分[12-13]

用以下2n個(gè)加權(quán)采樣點(diǎn)構(gòu)造I[g(x)]的三階精度的數(shù)值積分近似：

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

其中，標(biāo)量xi表示向量x的第i個(gè)分量。

文獻(xiàn)[4]導(dǎo)出的UKF算法用于近似單位高斯分布權(quán)函數(shù)的三階精度數(shù)值積分的加權(quán)采樣點(diǎn)為：

文獻(xiàn)[6]導(dǎo)出的CKF算法用于近似單位高斯分布權(quán)函數(shù)的三階精度數(shù)值積分的加權(quán)采樣點(diǎn)為：

3 CKF算法的局限性分析

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

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

4 改進(jìn)CKF的導(dǎo)出

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

考察如下非線性傳遞函數(shù)：

將每個(gè)sigma點(diǎn)經(jīng)非線性變換進(jìn)行傳遞：

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

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

其中，σxi,j是σxi的第 j個(gè)分量。對式（9）中任意傳遞sigma點(diǎn)在處進(jìn)行Taylor級數(shù)展開：

其中，?/?xj表示對應(yīng)于x（jx的第j個(gè)分量）的偏導(dǎo)數(shù)。當(dāng) l=1時(shí)，式（14）可以表示為由于sigma點(diǎn)關(guān)于均值處是對稱的，所以式（13）中所有奇次冪項(xiàng)都是零。根據(jù)式（10）和式（13），UT傳遞后的均值可表示為：

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

其中，hom()

κ表示這些高階信息是可調(diào)參數(shù)κ的函數(shù)。對于CKF而言，κ=0，上式可進(jìn)一步表示為：

從以上公式的推導(dǎo)過程可以看出，隨著維數(shù)的增加，CKF傳遞均值的高階信息顯著增加。在實(shí)際應(yīng)用中，真實(shí)值的高階矩信息是未知的，因此使用UKF或CKF計(jì)算的均值的高階矩信息可能更接近真實(shí)信息，也可能偏離真實(shí)信息[16]。相對于某一算法最優(yōu)表現(xiàn)，更關(guān)心的其最差表現(xiàn)，對于基于采樣點(diǎn)的非線性濾波算法而言，使其高階信息趨于0不失為一種好的選擇?；谝陨嫌懻摵驼J(rèn)識，下面導(dǎo)出本文所要提出的基于采樣點(diǎn)正交變換的改進(jìn)CKF算法。

由式（2）可知：

ξ表示三階精度數(shù)值積分公式中所采用的一組采樣點(diǎn)集，滿足如下關(guān)系式：

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

式（20）對應(yīng)的矩陣形式為：

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

因此Cξ也是三階精度數(shù)值積分公式中所采用的一組采樣點(diǎn)，則有如下結(jié)論：

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

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

證明（1）首先假設(shè)n是偶數(shù)，則：

因此

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

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

首先將式（6）中的cubature點(diǎn)表示成矩陣形式：

則ξ經(jīng)B正交變換后為：

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

5 仿真驗(yàn)證

5.1 維數(shù)可變的強(qiáng)非線性系統(tǒng)

考慮如下維數(shù)可變的強(qiáng)非線性系統(tǒng)模型：

圖1 改進(jìn)CKF算法流程示意圖

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

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

圖2 不同維數(shù)下三種濾波算法的MRMSE

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

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

圖3 n=30時(shí)50次Monte Carlo仿真的RMSEs

圖4 50次Monte Carlo仿真的RMSEs均值和標(biāo)準(zhǔn)差

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

5.2 角測量跟蹤模型

角測量模型描述的是一個(gè)二維平面內(nèi)的動態(tài)目標(biāo)跟蹤問題，具體數(shù)學(xué)模型為：

為了公平地比較兩種非線性濾波算法，進(jìn)行100次獨(dú)立Monte Carlo仿真實(shí)驗(yàn)并取其平均，圖5給出了動態(tài)目標(biāo)的跟蹤軌跡。

圖5 運(yùn)動目標(biāo)的跟蹤軌跡

由圖5可見，當(dāng)初始協(xié)方差矩陣誤差較大時(shí)，CKF和ICKF都能很快收斂，但I(xiàn)CKF的收斂速度更快，初始抖動更小，其跟蹤軌跡也更為接近真實(shí)軌跡。

定義時(shí)刻的位置均方誤差為：

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

圖6 位置跟蹤均方誤差

圖7 速度跟蹤均方誤差

圖6 和圖7表明，ICKF對運(yùn)動目標(biāo)位置和速度的跟蹤效果均更優(yōu)，其初始誤差曲線更為平滑、誤差更小。

5.3 空中目標(biāo)跟蹤模型

將ICKF應(yīng)用到空中目標(biāo)跟蹤系統(tǒng)中，并與CKF進(jìn)行對比。假設(shè)目標(biāo)在以未知角速度Ω等高度機(jī)動飛行，飛行高度h=10 000 m，運(yùn)動模型為：

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

坐標(biāo)系原點(diǎn)的測量雷達(dá)獲取目標(biāo)的斜距η和方位角θ信息。測量向量為yk=[ηkθk]T，滿足：

其中，wk～N(0,Rk)為量測噪聲，為協(xié)方差陣，ση和σθ為雷達(dá)斜距測量噪聲和方位角測量噪聲的標(biāo)準(zhǔn)差。

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

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

圖8 目標(biāo)位置估計(jì)均方根誤差

圖9 目標(biāo)速度估計(jì)均方根誤差

圖10 目標(biāo)角速度估計(jì)均方根誤差

值得說明的是，所提改進(jìn)算法是基于正交變化的改進(jìn)CKF算法，所針對的問題是CKF算法應(yīng)用于高維問題時(shí)出現(xiàn)的非局部采樣問題而導(dǎo)致估計(jì)精度下降的問題。改進(jìn)之處體現(xiàn)在對CKF算法的采樣點(diǎn)進(jìn)行了一次正交變換，沒有改變采樣點(diǎn)的個(gè)數(shù)和權(quán)值，在以上的仿真實(shí)驗(yàn)應(yīng)用中，改進(jìn)CKF算法所采用的采樣點(diǎn)只需要一次定義即可，沒有增加計(jì)算的復(fù)雜度，計(jì)算量與CKF相當(dāng)。

6 結(jié)束語

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

下一步的工作目標(biāo)是將所提算法與已有的各種CKF改進(jìn)算法相結(jié)合，更進(jìn)一步地提高ICKF的濾波性能，并拓展其在諸如動基座的初始對準(zhǔn)，慣性導(dǎo)航等領(lǐng)域中的實(shí)際工程應(yīng)用。