有色噪聲下的目標(biāo)跟蹤卡爾曼濾波新算法

林 旭，劉俊釗

（成都理工大學(xué) 地球科學(xué)學(xué)院，成都 610059）

現(xiàn)有的目標(biāo)跟蹤估計(jì)算法大多采用的是傳統(tǒng)卡爾曼濾波（Kalman Filtering,KF）算法，而KF是一種建立在狀態(tài)噪聲和觀測噪聲均為白噪聲條件下的最優(yōu)估計(jì)方法。在實(shí)際的觀測過程中，目標(biāo)系統(tǒng)通常是連續(xù)的動態(tài)過程，導(dǎo)致噪聲歷元間存在時(shí)間相關(guān)性[1-3]，例如，全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）的導(dǎo)航解決方案中就包含了誤差源的影響，如大氣延遲、時(shí)鐘誤差，它們會隨時(shí)間緩慢變化。噪聲的時(shí)間相關(guān)性使得KF算法的處理精度受到嚴(yán)重制約。對于有色噪聲的處理，許多學(xué)者提出了解決方法，如狀態(tài)擴(kuò)增法、量測擴(kuò)增法[4]、函數(shù)模型擬合法以及抗差 M-M 濾波等。其中，狀態(tài)擴(kuò)增法和量測擴(kuò)增法是通過構(gòu)造由白噪聲所驅(qū)動的模型，將有色噪聲作為模型參數(shù)在濾波中一并估計(jì)；函數(shù)模型擬合法在濾波解算前利用有色噪聲預(yù)報(bào)值修正狀態(tài)量和觀測量，以控制有色噪聲的影響；文獻(xiàn)[5]采用自適應(yīng)卡爾曼濾波，通過構(gòu)建觀測量等價(jià)權(quán)或自適應(yīng)因子降低有色噪聲在數(shù)據(jù)處理中的權(quán)重，從整體上控制了有色噪聲，因而有著廣泛的應(yīng)用。這些方法皆是在傳統(tǒng)卡爾曼濾波模型下，將有色噪聲看作白噪聲的簡單線性函數(shù)，并結(jié)合濾波模型參與解算。但實(shí)際上，有色噪聲不一定能由簡單的函數(shù)模型表達(dá)，若仍將其視為白噪聲的簡單線性函數(shù)，上述方法的精度將難以保證。

文獻(xiàn)[6]利用有色噪聲協(xié)方差矩陣改進(jìn)了卡爾曼濾波的預(yù)測狀態(tài)協(xié)方差和估計(jì)狀態(tài)協(xié)方差，并在協(xié)方差更新過程中減弱有色噪聲對狀態(tài)估值的擾動，得到較好的估計(jì)效果。文獻(xiàn)[7]在此基礎(chǔ)上，采用極大驗(yàn)后濾波公式，得到改進(jìn)的狀態(tài)估值方程，并利用協(xié)方差傳播定律改進(jìn)濾波的估計(jì)狀態(tài)協(xié)方差和增益矩陣，進(jìn)一步提高濾波精度。但文獻(xiàn)[6]和文獻(xiàn)[7]都是基于相鄰歷元噪聲相關(guān)的假設(shè)，提出的一步相關(guān)卡爾曼濾波（One-step Correlated Kalman Filtering,OCKF）算法忽視了非相鄰歷元噪聲間的相關(guān)性對濾波估計(jì)的影響。對此，本文首先給出了OCKF算法的基本過程，分析了有色噪聲協(xié)方差的假設(shè)模型，然后從參數(shù)估計(jì)的角度，利用非相鄰歷元噪聲間的相關(guān)性特性，建立了嚴(yán)密的有色噪聲協(xié)方差矩陣，在線性無偏最小方差準(zhǔn)則下，推導(dǎo)出多步相關(guān)卡爾曼濾波（Multi-step Correlated Kalman Filtering,MCKF）算法。

1 一步相關(guān)卡爾曼濾波算法

在KF算法基礎(chǔ)上，OCKF算法通過計(jì)算相鄰歷元噪聲間的協(xié)方差值，并在時(shí)間更新和測量更新過程中抵消，以減弱有色噪聲對狀態(tài)估計(jì)的影響[8]。

現(xiàn)將卡爾曼濾波的離散線性模型表示如下：

式中，Wk-1為狀態(tài)噪聲，Vk為觀測噪聲。

當(dāng)機(jī)動目標(biāo)跟蹤系統(tǒng)的數(shù)據(jù)為有色噪聲時(shí)，若假設(shè)相鄰歷元噪聲之間存在相關(guān)性，不相鄰的歷元噪聲間不相關(guān)，則有色噪聲協(xié)方差矩陣D為分塊三對角矩陣[6-7]。

用D1,D2,,Dn表示歷元噪聲的自協(xié)方差矩陣，σ1,σ2,,σn-1表示相鄰歷元噪聲間協(xié)方差，得到：

將k-1時(shí)刻狀態(tài)估值的一步預(yù)測值定義如下：

假設(shè)xk|k-1表示一步預(yù)測值與真值之差，表示狀態(tài)估值與真值之差[6-7]。

將公式(4)代入公式(3)，得到xk|k-1的矩陣表達(dá)式：

因此，由協(xié)方差傳播定律[7]，求得預(yù)測協(xié)方差：

由公式(6)可知，與KF算法相比，OCKF算法的預(yù)測協(xié)方差公式增加了對協(xié)方差改正項(xiàng)PX?k-1Wk-1的計(jì)算。通過構(gòu)建狀態(tài)噪聲與狀態(tài)估值的表達(dá)式，利用協(xié)方差傳播定律，得到PX?k-1Wk-1的表達(dá)式如下：

同樣，構(gòu)建觀測噪聲與預(yù)測狀態(tài)值的表達(dá)式，利用協(xié)方差傳播定律，得到：

將公式(8)代入傳統(tǒng)卡爾曼濾波公式，得到改進(jìn)的估計(jì)狀態(tài)協(xié)方差矩陣Pk和增益矩陣Lk如下：

2 多步相關(guān)卡爾曼濾波新算法

OCKF算法假設(shè)了相鄰歷元噪聲間存在相關(guān)性，其協(xié)方差矩陣為分塊三對角矩陣。該假設(shè)簡化了濾波協(xié)方差改正項(xiàng)的計(jì)算過程，當(dāng)歷元噪聲間相關(guān)性較弱時(shí)，能取得較好的濾波結(jié)果，但對于相關(guān)性較強(qiáng)的噪聲，由于高階項(xiàng)噪聲間協(xié)方差數(shù)值較大且衰減緩慢，若將其忽略，濾波結(jié)果的準(zhǔn)確性將難以保證。

為改善這一情況，本文對式(2)的協(xié)方差矩陣D加以改進(jìn)?？紤]到有色噪聲各歷元間存在相關(guān)性[9]，即滿足：

因此，構(gòu)建改進(jìn)的有色噪聲協(xié)方差矩陣如下：

式中，Pi表示第k個(gè)歷元噪聲同間隔為i的歷元噪聲k間的協(xié)方差值。

首先k時(shí)刻狀態(tài)估值的函數(shù)模型為[10]：

用εk表示估計(jì)狀態(tài)誤差，定義如下：

將式(1)(13)代入到式(14)中，計(jì)算期望值，顧及E（Wk）=0、E（Vk）=0，得到：

同時(shí)，多步相關(guān)卡爾曼濾波滿足以下兩條性質(zhì)。

性質(zhì)1狀態(tài)估值k應(yīng)為狀態(tài)真值Xk的無偏估計(jì)[11]

將式(14)(16)代入式(15)中，得到：

顧及式(17)，在式(15)的等式兩邊皆為0的條件下得到：

將式(1)(13)(18)代入式(14)可得估計(jì)狀態(tài)誤差的遞推表達(dá)式：

令預(yù)測狀態(tài)誤差為εk|k-1，其定義為：

將式(3)(18)代入式(13)，得到：

將式(1)(3)代入式(20)，得到k-1時(shí)刻估計(jì)狀態(tài)誤差與預(yù)測狀態(tài)誤差的線性表達(dá)式：

以Pk|k-1表示k-1時(shí)刻預(yù)測狀態(tài)協(xié)方差，Pk-1表示k-1時(shí)刻狀態(tài)估計(jì)協(xié)方差，由協(xié)方差傳播定律得到：

將式(19)在k-1時(shí)刻展開，得到由全局狀態(tài)噪聲與觀測噪聲構(gòu)成的εk-1表達(dá)式。假設(shè)狀態(tài)噪聲與觀測噪聲之間互不相關(guān)[12]，即cov(WkVj)=0，則存在：

將式(21)代入到式(14)得到k-1時(shí)刻預(yù)測狀態(tài)誤差與k時(shí)刻估計(jì)狀態(tài)誤差的表達(dá)式：

故k時(shí)刻的估計(jì)狀態(tài)協(xié)方差為：

利用預(yù)測狀態(tài)誤差與觀測噪聲間的線性函數(shù)，得到其協(xié)方差展開式：

性質(zhì)2 存在增益矩陣Lk使得狀態(tài)估計(jì)的方差最小[13]，故有：

將式(28)代入式(26)，整理得到：

綜上，本文提出的多步相關(guān)卡爾曼濾波的公式為：

3 數(shù)值仿真實(shí)驗(yàn)

為驗(yàn)證本文所提算法的有效性，假設(shè)跟蹤的目標(biāo)物體在三維空間中運(yùn)動，系統(tǒng)狀態(tài)為x、y、z軸方向的位置和速度，濾波模型可描述為：

其中，

其中，ω～N(0,1)，ξ～N(0,1)。

設(shè)定以下三種方案進(jìn)行分析。

方案1：a=0.8，b=0.5，c=0，d=0，狀態(tài)噪聲為有色噪聲，觀測噪聲為白噪聲。方案2：a=0，b=0，c=0.8，d=0.5，狀態(tài)噪聲為白噪聲，觀測噪聲為有色噪聲。方案3：a=0.8，b=0.5，c=0.8，d=0.5，狀態(tài)噪聲和觀測噪聲皆為有色噪聲。

設(shè)定數(shù)據(jù)長度為1000，目標(biāo)物體保持勻速運(yùn)動，分別采用KF算法、OCKF算法和本文提出的MCKF算法按上述三種方案進(jìn)行200次Monte-Carlo仿真。

采用狀態(tài)估計(jì)均方誤差RMS作為評價(jià)指標(biāo)，RMS值越小，濾波精度越高，其定義如下：

其中，N為Monte-Carlo仿真的次數(shù)，i表示第i次仿真，k表示第k個(gè)歷元。圖1給出了方案1下位置和速度的RMS，圖2給出了方案2下位置和速度的RMS。

圖1 3種濾波在方案1下的結(jié)果Fig.1 Results of three filters in case 1

圖2 3種濾波在方案2下的結(jié)果Fig.2 Results of three filters in case 2

表1 方案1下狀態(tài)估計(jì)的平均RMSTab.1 Average RMS of state estimation in case 1

表2 方案2下狀態(tài)估計(jì)的平均RMSTab.2 Average RMS of state estimation in case 2

表3 方案3下狀態(tài)估計(jì)的平均RMSTab.3 Average RMS of state estimation in case 3

圖3給出了方案3下位置和速度的RMS。三種方案的位置和速度的平均RMS值見表1～3。需說明的是，位置和速度均是通過x、y、z軸方向的位置和速度計(jì)算所得，公式為：

圖3 三種濾波在方案3下的結(jié)果Fig.3 Results of three filters in case 3

由圖1和表1可見，當(dāng)狀態(tài)噪聲為有色噪聲時(shí)，KF算法得到的位置和速度的平均 RMS分別為 1.6246、2.3584，精度明顯低于OCKF算法和MCKF算法。

由圖2和表2可見，當(dāng)觀測噪聲為有色噪聲時(shí)，OCKF算法和MCKF算法得到的位置和速度的RMS低于KF算法，其對速度的估計(jì)精度略有提升。

由圖3和表3可看出，當(dāng)狀態(tài)噪聲和觀測噪聲皆為有色噪聲時(shí)，OCKF算法和MCKF算法的RMS明顯低于KF算法，具有較好的濾波效果。

由表1～3可得，在三種方案下，MCKF算法的狀態(tài)估計(jì)RMS值均低于OCKF算法和KF算法。

實(shí)驗(yàn)結(jié)果表明：

① KF算法本身是基于白噪聲所提出的，其利用白噪聲協(xié)方差參與狀態(tài)更新過程，而有色噪聲則會對狀態(tài)更新產(chǎn)生擾動，從而降低了KF算法的精度。

② 相同條件下，OCKF算法和MCKF算法對有色狀態(tài)噪聲的濾除效果優(yōu)于有色觀測噪聲。

③ OCKF和MCKF算法在濾波過程中均利用了有色噪聲的協(xié)方差矩陣對狀態(tài)估計(jì)協(xié)方差矩陣加以改進(jìn)，從而減小有色噪聲的影響，而 MCKF算法因其構(gòu)建的有色噪聲協(xié)方差模型更加嚴(yán)密，故在濾波過程中擁有更高的精度。

4 結(jié) 論

當(dāng)機(jī)動目標(biāo)跟蹤系統(tǒng)的數(shù)據(jù)中存在有色噪聲時(shí)，傳統(tǒng)卡爾曼濾波算法的精度和適用性會受到明顯制約，難以實(shí)現(xiàn)高精度的數(shù)據(jù)解算。本文基于一步相關(guān)卡爾曼濾波算法的基本思想，提出了多步相關(guān)卡爾曼濾波算法，并針對有色狀態(tài)噪聲、有色觀測噪聲以及兩者皆為有色噪聲的三種情況進(jìn)行了仿真實(shí)驗(yàn)，證明了多步相關(guān)卡爾曼濾波算法能夠有效控制有色噪聲，提高卡爾曼濾波在實(shí)際應(yīng)用中的解算精度，可廣泛應(yīng)用于目標(biāo)跟蹤、導(dǎo)航定位、傳感器融合等領(lǐng)域，具有一定的工程應(yīng)用價(jià)值。