沈念如

（南京理工大學(xué) 南京 210094）

1 引言

現(xiàn)階段，卡爾曼濾波器（Kalman Filter，KF）［1～2］是在目標(biāo)實(shí)時(shí)跟蹤系統(tǒng)中應(yīng)用最為廣泛的一種方法，傳統(tǒng)卡爾曼濾波只適用于線(xiàn)性系統(tǒng)，而對(duì)于非線(xiàn)性目標(biāo)跟蹤領(lǐng)域［3］，擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）［4］是對(duì)非線(xiàn)性函數(shù)進(jìn)行一階泰勒級(jí)數(shù)展開(kāi)并忽略高階項(xiàng)，在非線(xiàn)性特性較強(qiáng)的時(shí)候，不可避免會(huì)引入較大地線(xiàn)性化誤差以至于會(huì)造成濾波發(fā)散。無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）［5～6］利 用 無(wú) 跡 變 換（Unscented Transform，UT）在估計(jì)點(diǎn)附近確定采樣點(diǎn)，對(duì)非線(xiàn)性分布的統(tǒng)計(jì)量具有較高的計(jì)算精度，然而UT變換需要對(duì)矩陣進(jìn)行喬利斯基分解容易導(dǎo)致算法停滯。平方根容積卡爾曼濾波（Square Root Cubature Kalman Filter，SRCKF）［7～9］采用協(xié)方差矩陣的三角分解因子代替協(xié)方差矩陣進(jìn)行遞推更新，成功避免了非正定矩陣喬利斯基分解的問(wèn)題。然而隨著非線(xiàn)性系統(tǒng)規(guī)模和復(fù)雜性的擴(kuò)大，由于各種難以測(cè)量的干擾，不確定的傳輸時(shí)延和損失廣泛的存在，其中之一便是隨機(jī)乘性噪聲，廣泛存在于系統(tǒng)的工程應(yīng)用當(dāng)中。針對(duì)具有此類(lèi)乘性相關(guān)噪聲［10，13］的非線(xiàn)性系統(tǒng)結(jié)合SRCKF濾波框架，可以有效地提高濾波地精度。

2 非線(xiàn)性濾波方法

2.1 擴(kuò)展卡爾曼濾波

EKF算法是建立在線(xiàn)性卡爾曼濾波基礎(chǔ)上，其主要思想是對(duì)非線(xiàn)性系統(tǒng)模型圍繞濾波值做Taylor級(jí)數(shù)展開(kāi)并略去二階及以上的高階項(xiàng)，得到一個(gè)近似的線(xiàn)性化的系統(tǒng)模型，然后對(duì)該模型做線(xiàn)性化求解。系統(tǒng)的離散非線(xiàn)性狀態(tài)及觀(guān)測(cè)模型如下：

其中wk∈Rn，vk∈Rm分別表示為過(guò)程噪聲和觀(guān)測(cè)噪聲，且它們?yōu)榛ゲ幌嚓P(guān)的均值為零，協(xié)方差分別為Qk和Rk的高斯白噪聲序列，Xk∈Rn，Yk∈Rm分別為k時(shí)刻狀態(tài)向量和觀(guān)測(cè)向量，f(·)和h(·)分別為非線(xiàn)性狀態(tài)運(yùn)動(dòng)方程和非線(xiàn)性觀(guān)測(cè)方程。應(yīng)用線(xiàn)性Kalman濾波原理其遞推步驟如下所示：

1）先驗(yàn)狀態(tài)一步預(yù)測(cè)：

2）先驗(yàn)狀態(tài)一步預(yù)測(cè)誤差協(xié)方差：

3）觀(guān)測(cè)一步預(yù)測(cè)：

4）卡爾曼濾波增益：

5）后驗(yàn)狀態(tài)更新：

6）后驗(yàn)狀態(tài)誤差協(xié)方差：

不難看出，EKF遞推算法公式與線(xiàn)性Kalman濾波公式近乎一致，只不過(guò)EKF的非線(xiàn)性運(yùn)動(dòng)方程和觀(guān)測(cè)方程因?yàn)橐浑A線(xiàn)性化，導(dǎo)致系統(tǒng)模型使用Jacobian矩陣代替狀態(tài)轉(zhuǎn)移矩陣和觀(guān)測(cè)矩陣；此外EKF算法是對(duì)非線(xiàn)性函數(shù)進(jìn)行一階Taylor級(jí)數(shù)展開(kāi)并忽略高階項(xiàng)，在非線(xiàn)性特性較強(qiáng)的時(shí)候，不可避免會(huì)引入較大地線(xiàn)性化誤差以至于會(huì)造成濾波發(fā)散。

2.2 改進(jìn)SRCKF濾波

基于乘性相關(guān)噪聲的平方根濾波方法，首先要建立帶有乘性相關(guān)噪聲的離散非線(xiàn)性系統(tǒng)模型［11～12］，如式（8）所示。

其中f(·)和h(·)分別為非線(xiàn)性狀態(tài)方程和觀(guān)測(cè)方程，xk∈Rn，yk∈Rm分別為系統(tǒng)的k時(shí)刻狀態(tài)向量和觀(guān)測(cè)向量，wk∈Rn，vk∈Rm為互不相關(guān)的過(guò)程噪聲和觀(guān)測(cè)噪聲離散序列，分別服從零均值方差為Qk和Rk的高斯分布，ξk∈R1為k時(shí)刻乘性噪聲，服從零均值方差為Uk的高斯分布，且ξk和vk互相關(guān)，互相關(guān)協(xié)方差矩陣為Gk，Γ為已知常數(shù)。

若X和Y滿(mǎn)足聯(lián)合高斯分布，根據(jù)相關(guān)理論知識(shí)，則其條件概率也滿(mǎn)足高斯分布，其均值和協(xié)方差如式（9）所示，由于狀態(tài)模型和觀(guān)測(cè)模型中乘性噪聲與觀(guān)測(cè)噪聲互相關(guān)，由此考慮乘性噪聲和觀(guān)測(cè)序列在聯(lián)合高斯分布情況下，其條件概率的分布也滿(mǎn)足高斯分布，其均值M和方差J如式（10）所示。

由式（10）可知M∈R1，J∈R1，因此在已知k時(shí)刻后驗(yàn)觀(guān)測(cè)條件下的狀態(tài)濾波值x?k|k以及誤差協(xié)方差值Pk|k，來(lái)預(yù)測(cè)k+1時(shí)刻的估計(jì)值，如式（11）所示：

其中協(xié)方差估計(jì)值Pk+1|k的系數(shù) (1+2ΓMk+Γ2Jk)∈R1，則對(duì)于離散非線(xiàn)性系統(tǒng)改進(jìn)的SRCKF算法，具體步驟如下：

1）初始化

其中chol(·)表示對(duì)矩陣進(jìn)行喬利斯基分解。

2）時(shí)間更新

若k時(shí)刻系統(tǒng)后驗(yàn)狀態(tài)估計(jì)量為，狀態(tài)估計(jì)誤差協(xié)方差平方根因子為Sk|k，過(guò)程噪聲誤差協(xié)方差矩陣Qk，觀(guān)測(cè)噪聲誤差協(xié)方差矩陣Rk，觀(guān)測(cè)向量yk，乘性噪聲和觀(guān)測(cè)噪聲互協(xié)方差Gk，乘性噪聲方差Uk，則一步狀態(tài)預(yù)測(cè)值和協(xié)方差平方根因子為

對(duì)于n維狀態(tài)變量，其容積點(diǎn)集，權(quán)重W(i)以及[1]i定義如下：

其中m=2n為容積點(diǎn)個(gè)數(shù)，表示對(duì)于三階球面徑向容積規(guī)則，采樣容積點(diǎn)數(shù)為狀態(tài)向量維度的兩倍，[1]i表示第i個(gè)容積點(diǎn)向量，m在此又可表示為列向量的個(gè)數(shù)，每個(gè)容積點(diǎn)的向量維度與狀態(tài)變量維度相同，κ(i)表示選取的m個(gè)容積采樣點(diǎn)和分別表示先驗(yàn)狀態(tài)向量估計(jì)值和先驗(yàn)誤差協(xié)方差平方根估計(jì)矩陣，sqrt(·)為求平方根函數(shù)，qr(·)表示對(duì)矩陣進(jìn)行QR分解，如式（15）所示Mk和Jk表示k時(shí)刻乘性噪聲在觀(guān)測(cè)序列條件下的乘性噪聲均值和協(xié)方差。

3）觀(guān)測(cè)更新

其中ε(i)表示根據(jù)先驗(yàn)狀態(tài)向量估計(jì)值和先驗(yàn)狀態(tài)誤差協(xié)方差平方根估計(jì)值產(chǎn)生觀(guān)測(cè)方程的m個(gè)容積采樣點(diǎn)（m=2n），δk+1|k(i)觀(guān)測(cè)方程的非線(xiàn)性傳播，和?分別表示觀(guān)測(cè)向量估計(jì)值和觀(guān)測(cè)誤差協(xié)方差平方根因子估計(jì)值表示狀態(tài)向量和觀(guān)測(cè)向量估計(jì)的誤差互協(xié)方差值。

其中Kk+1為卡爾曼增益，和分別為后驗(yàn)狀態(tài)向量估計(jì)值和后驗(yàn)誤差協(xié)方差平方根因子估計(jì)值。

3 仿真分析

為了驗(yàn)證改進(jìn)SRCKF濾波方法在乘性噪聲條件下的性能，本仿真實(shí)驗(yàn)對(duì)比了EKF、SRCKF、SRUKF濾波方法，以均方根誤差作為衡量標(biāo)準(zhǔn)，仿真實(shí)驗(yàn)添加了均方根值為1.8m的距離誤差，1.8°的角度誤差，0.9m/s的速度誤差，且運(yùn)動(dòng)模型添加了均方根值為0.012的乘性噪聲誤差，且與觀(guān)測(cè)噪聲的互協(xié)方差為（0.025，0.0166，0.0125），實(shí)驗(yàn)過(guò)程中采用勻加速運(yùn)動(dòng)模型和雷達(dá)系統(tǒng)觀(guān)測(cè)模型［14］，雷達(dá)觀(guān)測(cè)到的目標(biāo)初始徑向距離為20m，方位角為30°，速度為10m/s，運(yùn)動(dòng)模型分別在x和y方向上添加初始加速度為（2m/s，3.5m/s），目標(biāo)運(yùn)動(dòng)方程f(·)和觀(guān)測(cè)方程h(·)參照式（18）所示。

其中采樣周期dt本例設(shè)置為0.02，如圖1（a）和（b）所示采樣點(diǎn)數(shù)為350個(gè)x，y方向上距離，速度，加速度的均方根誤差曲線(xiàn)，圖2（a）和（b）所示為x，y方向上距離維上15000次蒙特卡洛濾波實(shí)驗(yàn)均方根誤差，可以看出改進(jìn)SRCKF濾波算法均方根誤差較其它濾波算法有更好的實(shí)驗(yàn)精度，統(tǒng)計(jì)結(jié)果如表1所示，基于乘性相關(guān)噪聲的改進(jìn)SRCKF濾波算法，在對(duì)復(fù)雜背景下受乘性相關(guān)噪聲影響的系統(tǒng)，相較于傳統(tǒng)的SRCKF算法在x方向上距離、速度、加速度的濾波精度分別提高了77.5%，41.0%，12.3%，y方向上則分別提高了77.6%，20.7%，54.8%，具有較好的濾波精度。

表1 EKF、SRCKF、SRUKF和改進(jìn)SRCKF濾波算法均方根誤差統(tǒng)計(jì)表

圖1 x，y方向上均方根誤差曲線(xiàn)圖

圖2 x，y方向上15000次蒙特卡洛均方根誤差實(shí)驗(yàn)

4 結(jié)語(yǔ)

本文介紹了幾種典型的非線(xiàn)性濾波算法，簡(jiǎn)單闡述其優(yōu)缺點(diǎn)，并推導(dǎo)了在乘性相關(guān)噪聲條件下的改進(jìn)SRCKF濾波方法實(shí)現(xiàn)，最后通過(guò)仿真實(shí)驗(yàn)，對(duì)四種濾波算法的性能進(jìn)行了對(duì)比分析，結(jié)果表明在隨機(jī)的乘性相關(guān)噪聲條件下，改進(jìn)的SRCKF濾波精度更高，可應(yīng)用于復(fù)雜環(huán)境下的目標(biāo)跟蹤。