有色噪聲下的目標跟蹤卡爾曼濾波新算法

林 旭，劉俊釗

（成都理工大學 地球科學學院，成都 610059）

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

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

1 一步相關卡爾曼濾波算法

在KF算法基礎上，OCKF算法通過計算相鄰歷元噪聲間的協方差值，并在時間更新和測量更新過程中抵消，以減弱有色噪聲對狀態估計的影響[8]。

現將卡爾曼濾波的離散線性模型表示如下：

式中，Wk-1為狀態噪聲，Vk為觀測噪聲。

當機動目標跟蹤系統的數據為有色噪聲時，若假設相鄰歷元噪聲之間存在相關性，不相鄰的歷元噪聲間不相關，則有色噪聲協方差矩陣D為分塊三對角矩陣[6-7]。

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

將k-1時刻狀態估值的一步預測值定義如下：

假設xk|k-1表示一步預測值與真值之差，表示狀態估值與真值之差[6-7]。

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

因此，由協方差傳播定律[7]，求得預測協方差：

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

同樣，構建觀測噪聲與預測狀態值的表達式，利用協方差傳播定律，得到：

將公式(8)代入傳統卡爾曼濾波公式，得到改進的估計狀態協方差矩陣Pk和增益矩陣Lk如下：

2 多步相關卡爾曼濾波新算法

OCKF算法假設了相鄰歷元噪聲間存在相關性，其協方差矩陣為分塊三對角矩陣。該假設簡化了濾波協方差改正項的計算過程，當歷元噪聲間相關性較弱時，能取得較好的濾波結果，但對于相關性較強的噪聲，由于高階項噪聲間協方差數值較大且衰減緩慢，若將其忽略，濾波結果的準確性將難以保證。

為改善這一情況，本文對式(2)的協方差矩陣D加以改進。考慮到有色噪聲各歷元間存在相關性[9]，即滿足：

因此，構建改進的有色噪聲協方差矩陣如下：

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

首先k時刻狀態估值的函數模型為[10]：

用εk表示估計狀態誤差，定義如下：

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

同時，多步相關卡爾曼濾波滿足以下兩條性質。

性質1狀態估值k應為狀態真值Xk的無偏估計[11]

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

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

將式(1)(13)(18)代入式(14)可得估計狀態誤差的遞推表達式：

令預測狀態誤差為εk|k-1，其定義為：

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

將式(1)(3)代入式(20)，得到k-1時刻估計狀態誤差與預測狀態誤差的線性表達式：

以Pk|k-1表示k-1時刻預測狀態協方差，Pk-1表示k-1時刻狀態估計協方差，由協方差傳播定律得到：

將式(19)在k-1時刻展開，得到由全局狀態噪聲與觀測噪聲構成的εk-1表達式。假設狀態噪聲與觀測噪聲之間互不相關[12]，即cov(WkVj)=0，則存在：

將式(21)代入到式(14)得到k-1時刻預測狀態誤差與k時刻估計狀態誤差的表達式：

故k時刻的估計狀態協方差為：

利用預測狀態誤差與觀測噪聲間的線性函數，得到其協方差展開式：

性質2 存在增益矩陣Lk使得狀態估計的方差最小[13]，故有：

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

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

3 數值仿真實驗

為驗證本文所提算法的有效性，假設跟蹤的目標物體在三維空間中運動，系統狀態為x、y、z軸方向的位置和速度，濾波模型可描述為：

其中，

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

設定以下三種方案進行分析。

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

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

采用狀態估計均方誤差RMS作為評價指標，RMS值越小，濾波精度越高，其定義如下：

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

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

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

表1 方案1下狀態估計的平均RMSTab.1 Average RMS of state estimation in case 1

表2 方案2下狀態估計的平均RMSTab.2 Average RMS of state estimation in case 2

表3 方案3下狀態估計的平均RMSTab.3 Average RMS of state estimation in case 3

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

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

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

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

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

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

實驗結果表明：

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

② 相同條件下，OCKF算法和MCKF算法對有色狀態噪聲的濾除效果優于有色觀測噪聲。

③ OCKF和MCKF算法在濾波過程中均利用了有色噪聲的協方差矩陣對狀態估計協方差矩陣加以改進，從而減小有色噪聲的影響，而 MCKF算法因其構建的有色噪聲協方差模型更加嚴密，故在濾波過程中擁有更高的精度。

4 結 論

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