七階球面單形-徑向容積卡爾曼濾波方法*

趙明亮，汪立新，關(guān)永祥，單鈞麟

(火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，陜西 西安 710025)

0 引言

非線性卡爾曼濾波通過貝葉斯濾波理論轉(zhuǎn)化為計算多維高斯加權(quán)的非線性函數(shù)積分。對積分的閉式解劃分為對非線性函數(shù)的近似和高斯概率密度函數(shù)的近似。I.Arasaratnam等[1]基于三階球面-徑向準則提出的容積卡爾曼濾波(cubature Kalman filter,CKF)是一種基于高斯概率密度函數(shù)的近似的非線性濾波。CKF算法在高維非線性系統(tǒng)中能有效克服無跡卡爾曼濾波(unscented Kalman filter,UKF)等算法的濾波發(fā)散現(xiàn)象及粒子濾波(particle filter,PF)等方法存在的計算量大的問題，并且算法的計算復(fù)雜度僅隨狀態(tài)維數(shù)線性增加[2]。

文獻[3]推導(dǎo)了三階、五階球面單形-徑向準則，并與一般球面-徑向準則形成的算法作仿真比較，仿真結(jié)果表明采用了球面單形-徑向準則后，位置估計精度有明顯提升；三階CKF與五階球面單形-徑向CKF精度相近，但其均方根誤差(root mean square error,RMSE)仍比較大。文獻[4]推導(dǎo)了三階、五階球面單形-徑向準則，仿真表明，在估計精度上五階算法優(yōu)于三階算法及UKF算法，估計的魯棒性也好于其他算法，但其優(yōu)勢不明顯。文獻[5]采用高階高斯-拉蓋爾多項式求解徑向積分，但此種方法存在計算量大，算法設(shè)計較為復(fù)雜的缺點。

針對傳統(tǒng)CKF算法在非線性度較高的系統(tǒng)估計中，存在精度低、濾波穩(wěn)定性差；對于高階徑向準則，采用高斯-拉蓋爾公式計算求積點及權(quán)重系數(shù)較為復(fù)雜的問題，本文使用七階球面單形準則計算球面積分、采用矩匹配法來計算徑向積分，形成七階容積卡爾曼濾波算法。最后通過一個實際的目標跟蹤問題對本文算法的有效性進行了驗證。

1 非線性高斯濾波基本框架

已知非線性離散高斯系統(tǒng)：

xk=f(xk-1,uk-1)+wk-1，

(1)

zk=h(xk,uk)+vk,

(2)

式中：xk∈Rm為狀態(tài)向量；zk∈Rm為量測向量；uk-1為輸入向量；f(·)和h(·)分別為狀態(tài)轉(zhuǎn)移函數(shù)和量測函數(shù)；wk-1和vk為相互獨立的零均值高斯白噪聲，方差分別為Qk-1和Rk。

對于滿足均值為0，協(xié)方差為單位陣I的高斯分布N(x;0,I)可通過如下積分規(guī)則轉(zhuǎn)換式求解高斯積分：

(3)

式中：g(x)為非線性函數(shù)；I(g)為關(guān)于g(·)的高斯積分值；γi和wi分別為滿足高斯分布N(x;0,I)的積分點及對應(yīng)的權(quán)值；N為積分點總數(shù)；Rn為積分區(qū)域。

非線性高斯濾波基本框架為如下形式：

時間更新：

(4)

(5)

量測更新：

(6)

Pk=Pk|k-1-KkPzz,k|k-1KkT,

(7)

(8)

(9)

(10)

(11)

2 七階球面單形-徑向準則

由式(3)知，濾波器設(shè)計中必須求解高斯積分。I.Arasaratnam等人利用容積-徑向變換將高斯積分轉(zhuǎn)換為球面徑向積分，再通過球面-徑向容積準則進行積分近似[1]。

可將笛卡爾坐標的積分(3)轉(zhuǎn)化為如下球面-徑向坐標積分[2-5]：

(12)

此積分可以分解為球面積分S(r)和徑向積分R[6-10]，分別為

(13)

(14)

假定式(13)和式(14)由如下Ls和Lr個點的數(shù)值積分方法近似：

(15)

(16)

式中：{yi,ws,i}為計算球面積分求積點集合，Ls為相應(yīng)的求積點數(shù)；{rj,wr,j}為計算徑向積分求積點集合，Lr為相應(yīng)的求積點數(shù)。

則I(g)可由球面-徑向準則近似表示為

(17)

性質(zhì) 1：當(dāng)球面-徑向容積準則(17)的求積點完全對稱，徑向積分準則只需滿足偶數(shù)階代數(shù)精度。

2.1 基于正則單形變換群的球面單形準則

采用正則單形變換群對式(13)球面積分進行計算。

定義 1：單形(simplex)是某個n維以上的歐幾里得空間中(n+1)個仿射無關(guān)點集合的凸包，特指施萊夫利符號為{3,3,…,3}的正多胞形，代表考克斯特An群[11]。圖1為前10維正單形的二維拓撲映射圖。

圖1 前10維正單形的二維拓撲映射圖Fig.1 2D topology map of the first 10-dimensional positive simplex

(18)

(19)

(20)

b(1)(t)= (nT)-1/2(n-t(n+1),

(21)

式中：T=2(n+1)t2-2(n+1)t+n。

設(shè)G是n上所有正多面體到自身變換的群。ga域上點的集合稱為G-軌線。g包含著G群的所有變換，a是n上的不變點(即f(x)=x)。G-軌線若包含點a，則其可被定義為G(a)，軌線上點的數(shù)量由a決定。

定義TnG為Tn所有到自身變換的群。Tn群的階次為(n+1)!。群Tn的基底由以下n個不變多項式組成：

(22)

對群TnG以θ中心進行對稱變換得到群TnG*。群TnG*上的不變多項式與群TnG的偶次不變多項式一致。TnG*群的階次為2(n+1)!。超球體Sn的積分規(guī)則對群TnG*不變，且適用于所有不超過七階的多項式。

由于積分準則需精確滿足所有不大于七階的多項式，故當(dāng)n≤7時，其積分準則必定精確滿足以下8個不變多項式：

因此求積點的選擇至少需要8個參數(shù)。求積點依靠以下軌線來選擇：

(1)TnG*(λa(1)),(2)TnG*(βb(1)),

(5)TnG*(θ),

式中:λ,β,γ為未知參數(shù)；δ為一個不為0的給定參數(shù)；a(1)，b(1)，c(1)由式(18),(19),(20)確定；b(1)(1/4)由式(21)確定；θ是單形Tn的中心。

綜上所述，則積分準則為

(23)

和式(23)中存在9個參數(shù)，其中δ≠0為給定參數(shù)，其余8個參數(shù)D,A,B,C,E,λ,β,γ都可通過計算得到。聯(lián)立式(18)，(22)得到含8個未知量D,A,B,C,E,λ,β,γ的8個等式，如下所示：

(1)：D+N1A+N2B+N3C+N4E=1，

(π2)：r5N1Aλ2+r5N2Bβ2+r5N3Cγ2+

r1N4Eδ2/r4=(n+1)/(n+2)，

(π4)：r8N1Aλ4+r7N2Bβ4+r6N3Cγ4+

(π2π4)：r5r8N1Aλ6+r5r7N2Bβ6+r5r6N3Cγ6+

4)(n+6)]，

6r5(n-1)/[(n+2)(n+4)(n+6)].

設(shè)A1=N1Aλ6,B1=N2Bβ6,C1=N3Cγ6,

(24)

2.2 基于矩匹配法的徑向準則

采用數(shù)值積分方法計算徑向積分式(14)，得到如下等式[12-15]：

(25)

使用此種數(shù)值方法計算徑向積分，其代數(shù)精度為2p+1。由球面-徑向準則性質(zhì)1可知，只需滿足偶數(shù)階的徑向積分精度，則可使最終的球面-徑向積分求積點滿足對稱性。令S(r)=rl，對于代數(shù)精度為2p+1的徑向積分，式(25)只需對l=0,2,…,2p準確，即滿足p+1個等式。滿足p+1個等式中的最小求積分點數(shù)分別為(p+1)/2(p為奇數(shù))和p/2+1(p為偶數(shù))。

將S(r)=rl代入式(25)的左側(cè)，可得

(26)

當(dāng)p=3時，2p+1=7，此時即滿足七階徑向準則，最小求積分點數(shù)為Lr=(p+1)/2=2，則只需要精確到rl的偶數(shù)階，即l=0,2,4,6。將l=0,2,4,6代入式(26)，且已知Γ(z+1)=zΓ(z)=z×(z-1)!，則可由式(25)得到：

(27)

求解上述方程組，可得徑向準則求積點及其權(quán)重系數(shù)：

(28)

2.3 球面單形-徑向準則

利用2.1節(jié)球面單形準則和2.2節(jié)徑向準則組成球面-徑向容積準則，聯(lián)立式(24),(25)及式(28)并代入式(12)，即構(gòu)成七階球面單形-徑向容積準則為

(29)

式中：求積點數(shù)為L=4n3+6n2+6n+4。

3 仿真校驗

為驗證本文提出的七階球面單形-徑向容積卡爾曼濾波算法有效性，考慮如下目標跟蹤問題。飛行器M在坐標系Oxyz中以一未知的角速度進行機動飛行，飛行高度h已知。忽略地球自轉(zhuǎn)及扁率影響，可知飛行器旋轉(zhuǎn)運動的動力學(xué)模型由如下非線性方程描述[4]：

Xk+1=Φk+1|kXk+Γk+1|kWk=

(30)

假設(shè)坐標位置為(x0，y0)的雷達對飛行器M進行跟蹤，可得到雷達與飛行器M間的距離r和角度θ，則

(31)

X0=(-40 m，3 m·s-1，10 m，2 m·s-1)T,
P0=diag(4 m2,0.01 m2·s-2,4 m2,0.01 m2·s-2).

仿真結(jié)果如圖2所示，分別從定性及定量角度分析。圖2為飛行器位置估計的均方根誤差圖。由圖可知，飛行器位置的估計中七階算法明顯提高了估計精度，RMSE均值分別較五階算法提高1.58倍、三階算法提高3.23倍；RMSE方差分別較五階算法提高2.51倍、三階算法提高7.86倍。具體參數(shù)見表1。

圖3為飛行器速度估計的均方根誤差圖。由圖可知，飛行器速度的估計中七階算法明顯提高了估計精度，RMSE均值分別較五階算法提高1.58倍、三階算法提高2.29倍；RMSE方差分別較五階算法提高2.50倍、三階算法提高3.84倍。具體參數(shù)見表1。

圖2 位置估計的RMSEFig.2 RMSE of position estimate

表1 狀態(tài)估計的RMSE均值和方差Table 1 Mean and variance of RMSE

圖3 速度估計的RMSEFig.3 RMSE of velocity estimate

4 結(jié)束語

本文采用基于正則單形變換群的七階球面單形準則計算球面積分，使用矩匹配法求積分準則計算徑向積分，推導(dǎo)出七階球面單形-徑向容積求積分準則。通過一個目標跟蹤問題進行數(shù)值仿真，仿真結(jié)果表明，本文提出的算法能有效提高非線性濾波估計精度，且具有良好的穩(wěn)定性。在現(xiàn)實的高維復(fù)雜系統(tǒng)中有較好的應(yīng)用前景。