基于粗大誤差檢測和補償?shù)母倪M型EKF動態(tài)目標跟蹤算法

(溫州大學 電氣數(shù)字化設計技術國家地方聯(lián)合工程實驗室，浙江 溫州 325035)

0 引言

卡爾曼濾波理論[1-2]是目前最重要的最優(yōu)估計理論之一，因其目標跟蹤算法實用性強，濾波效率高，狀態(tài)估計結果較為優(yōu)化，在尋的制導、路徑識別、視頻監(jiān)控等領域得到廣泛應用。卡爾曼濾波算法在線性系統(tǒng)中為最優(yōu)估計算法，但工業(yè)中實際的系統(tǒng)往往為非線性系統(tǒng)，因此，針對非線性系統(tǒng)提出了擴展卡爾曼濾波算法(Extended Kalman Filter，EKF)[3]。擴展卡爾曼濾波的思想是基于非線性方程的線性化。擴展卡爾曼濾波狀態(tài)估計的精確度與系統(tǒng)方程非線性程度密切相關。系統(tǒng)的非線性程度越小，濾波結果越精確。

目前，國內外學者基于擴展卡爾曼濾波解決了很多工業(yè)中的工程問題。趙佳等為解決無人飛行器姿態(tài)傳感器低成本高精度的要求，引入擴展卡爾曼濾波算法，有效地消除了無人機飛行器運動過程對姿態(tài)解算精度的影響[4]。陳清華等針對鋰電池剩余電量(SOC)無法精確估算的問題，提出了一種基于改進Thevenin模型的EKF算法[5]，實現(xiàn)了鋰電池SOC的精確估算。

上述擴展卡爾曼濾波算法是基于動態(tài)目標跟蹤過程中觀測值只含隨機誤差的情況，但是實際觀測值中難免會出現(xiàn)各種粗大誤差(gross errors)[6]，比如壞值(outlier)、靜差(bias)和漂移(drift)等[6]。受到粗大誤差的影響，動態(tài)目標跟蹤的濾波狀態(tài)容易出現(xiàn)異常，這直接影響狀態(tài)估計的準確性。本文提出一種基于粗大誤差觀測值檢測和補償?shù)母倪M型EKF動態(tài)目標跟蹤算法，通過對粗大誤差觀測值的精確檢測和優(yōu)化分類補償，彌補了傳統(tǒng)的EKF算法對于實際應用中的不足。實驗仿真結果表明，改進型EKF算法能夠實現(xiàn)粗大誤差觀測值下的動態(tài)系統(tǒng)目標跟蹤，且具有較好的準確性。

1 擴展卡爾曼濾波算法

非線性離散系統(tǒng)的動態(tài)方程如下：

X(k+1)=f[k,X(k)]+G(k)W(k)

(1)

Z(k)=h[k,X(k)]+V(k)

(2)

式(1)為系統(tǒng)狀態(tài)方程，式(2)為觀測方程。X(k)是n維系統(tǒng)狀態(tài)向量；Z(k)是m維觀測向量；f[k,X(k)]和h[k,X(k)]是X(k)和Z(k)對應的非線性系統(tǒng)函數(shù)；G(k)是系統(tǒng)噪聲驅動矩陣；W(k)則是零均值系統(tǒng)白噪聲矩陣；V(k)是零均值觀測白噪聲，且{W(k)}和{V(k)}互不相關。將式(1)中X(k+1)做一階泰勒展開，得：

W(k)+φ(k)

(3)

(4)

(5)

將式(2)中的Z(k)做一階泰勒展開，得：

V(k)=H(k)X(k)+y(k)+V(k)

(6)

(7)

(8)

通過式(1)～(8)的變換，n維的非線性狀態(tài)方程和m維的非線性觀測方程可以轉化為下式：

X(k+1)=F(k+1|k)X(k)+G(k)W(k)+φ(k)

(9)

Z(k)=H(k)X(k)+y(k)+V(k)

(10)

式(9)和式(10)是將非線性方程線性化之后的一階線性方程，與傳統(tǒng)的卡爾曼濾波方程相比，狀態(tài)轉移矩陣F(k+1|k)和觀測矩陣H(k)都是根據(jù)f[k,X(k)]和h[k,X(k)]的Jacobian矩陣代替。根據(jù)傳統(tǒng)線性卡爾曼濾波的遞推方程，對系統(tǒng)進行濾波處理，得到非線性系統(tǒng)近似的最優(yōu)估計值。

與卡爾曼濾波的基本方程[1]類比，可以得到擴展卡爾曼濾波遞推方程：

(11)

P(k+1|k)=F(k+1|k)P(k|k)

FT(k+1|k)+Q(k+1)

(12)

K(k+1)=P(k+1|k)HT(k+1|k)

[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1

(13)

(14)

P(k+1)=[I-K(k+1)H(k+1)]P(k+1|k)

(15)

式中，P(k+1|k)表示根據(jù)上一個k時刻的協(xié)方差值來估計k+1時刻的協(xié)方差值。P(k+1)表示k+1時刻更新的協(xié)方差值。

2 觀測值的粗大誤差

2.1 粗大誤差的類型

觀測數(shù)據(jù)中通常出現(xiàn)的粗大誤差大致有3種，分別是壞值(outlier)，靜差(bias)和漂移(drift)，如圖1所示。

圖1 存在粗大誤差情況下的觀測值

壞值：也稱異常值或離群值，如圖1(a)所示，壞值是觀測數(shù)據(jù)中明顯偏離其他觀測數(shù)據(jù)的異常觀測值，通常情況下為個別觀測數(shù)據(jù)。假設第n個觀測變量在ki時刻出現(xiàn)了壞值，那么ki時存在壞值的觀測值可以被記為式(16)的形式。

Z(ki)=h[ki,X(ki)]+outliern,ki+V(ki)

(16)

其中:outliern,ki表示在n個觀測變量在ki時刻出現(xiàn)的壞值與實際值的偏差量大小。

靜差：屬于穩(wěn)態(tài)誤差的一種。如圖1(b)所示，由于測控設備本身的問題，導致觀測值與真實值之間存在誤差近似為常數(shù)的現(xiàn)象，偏差量可正可負，可能發(fā)生在所有觀測數(shù)據(jù)中，也可能發(fā)生在部分觀測數(shù)據(jù)中。假設在n個觀測變量在ki至kj時刻存在靜差，則存在靜差的觀測值可以被記為式(17)所示的形式。

Z(k)=h[k,X(k)]+biasn,ki～kj+V(k)

(17)

其中:k=ki,ki+1,...,kj，biasn,ki～kj表示在n個觀測變量在ki至kj時刻存在靜差的觀測值與實際值的偏差量大小。

漂移：如圖1(c)所示，與前兩種粗大誤差不同，觀測數(shù)據(jù)的漂移表現(xiàn)為觀測值與真實值之間存在偏差量為非線性函數(shù)的異常情況。假設第n個觀測變量在ki至kj時刻存在漂移，則存在漂移的觀測值可以被記為式(18)所示的形式。

Z(k)=h[k,X(k)]+driftn,ki～kj+V(k)

(18)

其中:k=ki,ki+1,...,kj，driftn,ki～kj是復雜變化的函數(shù)，通常情況下為非線性的形式。

2.2 粗大誤差的檢測

對于動態(tài)目標識別中傳感器得到觀測數(shù)據(jù)中存在的粗大誤差值，需要對其進行準確檢測，從而進行相應的補償。

2.2.1 壞值(outlier)的檢測

對于壞值的檢測：壞值是一組觀測數(shù)據(jù)中存在的孤立異常值，因此需要對每個時刻每個觀測數(shù)據(jù)進行檢測，從而判斷壞值存在與否。

壞值的檢測通常可采用依拉達準則[7]。假設動態(tài)目標傳感器觀測值數(shù)據(jù)與擴展卡爾曼濾波方程中預估的觀測數(shù)據(jù)偏差量的絕對值為Residualn,k，即殘差，可表示為式(19)。

(19)

(20)

根據(jù)貝塞爾法求得標準差σ。

(21)

T為設定的檢測區(qū)間長度，當Residual服從正態(tài)分布時，一組觀測數(shù)據(jù)中，Residual<3σ的占比為99.73%，這說明觀測數(shù)據(jù)中殘差Residual<3σ的情況是占大多數(shù)的。

若Residual>3σ，此時出現(xiàn)該觀測數(shù)據(jù)的可能性為0.07%，可信度低，為高度異常的壞值，可以確定該觀測值為壞值，將壞值標記為outlier_Flag=1，非壞值標記為outlier_Flag=0。當每次檢測出一個壞值時，就對壞值進行一次標記和補償。

需要注意的是，依拉達準則只適用于觀測次數(shù)較多的情況(觀測次數(shù)n>10)[8]。

2.2.2 靜差(bias)的檢測

靜差與壞值不同，靜差的存在使得連續(xù)多個觀測值與真實值之間的差值近似為常數(shù)，且差值可正可負。靜差可能發(fā)生在所有的觀測值中，也可能發(fā)生在部分觀測值中。因此，準確檢測靜差時不能只對k時刻第n個觀測值進行檢測，而是對k時刻前的第T-l時刻觀測值到第T時刻觀測值進行檢測，其中，l為設定的檢測區(qū)間長度，可長可短。先設定個靜差檢測移動窗口[9]，長度為l。那么對于該靜差檢測移動窗口，則標準差和殘差的平均值分別為：

(22)

(23)

(24)

由于對應于非線性系統(tǒng)的隨機誤差服從正態(tài)分布，因此當存在靜差的觀測值與原正態(tài)分布進行比較時就是存在一個平移量為bias的正態(tài)分布。bias為靜差偏移量，是一個常數(shù),可正可負。式(25)和式(26)分別表示正常觀測時和存在靜差時的殘差情況。

Residual(k)～N(0,σ2)

(25)

Residual(k)～N(μ,σ2),μ=bias

(26)

式中，μ為一段時間內殘差的平均值。

在靜差檢測移動窗口內，根據(jù)求得的標準差和殘差，可以對觀測值中是否存在靜差進行判斷。如果移動窗口移動3個時間點，3次移動窗口中殘差的平均值可以近似表示為一個常數(shù)量，即殘差的平均值μ=bias，這里μ=bias表示一個近似的常數(shù)，并且移動窗口內的標準差滿足依拉達法則，即Residual>3σ。移動窗口內標準差的浮動范圍小于一定的閾值，即|σ|

2.2.3 漂移(drift)的檢測

與壞值和靜差不同，漂移表現(xiàn)為連續(xù)多個觀測值與真實值的偏差量很大且難以用具體的函數(shù)來表示。

漂移的檢測是基于靜差檢測基礎之上的，當移動檢測窗口的觀測值中已經(jīng)檢測出Residual>3σ且殘差的平均值μ不為一個常數(shù)時，如果滿足|σ|≥Threshold，即標準差大于一定的閾值[8]，則檢測到該時間的觀測值為漂移，標記為drift_Flag=1，否則，標記為drift_Flag=0，對于漂移，可以采用局部線性擬合的方式，在局部構建一個線性方程近似漂移量，如(27)所示:

drift=p1k+p2

(27)

其中:drift表示k時刻的觀測漂移偏差量，p1表示局部線性區(qū)線性方程的斜率大小，p2表示局部線性區(qū)線性方程的截距大小。

2.3 粗大誤差的補償

粗大誤差值已經(jīng)可以通過上述討論的方法進行檢測，下一步就需要對已經(jīng)檢測到的不同類型的粗大誤差進行分類補償。

2.3.1 壞值(outlier)的補償

根據(jù)依拉達法則，可以檢測出此時的觀測值是否存在壞值，并將其標記為outlier_Flag=1。

對于標記為outlier_Flag=1的觀測值，可以通過式(21)中的殘差Residualn,ki來對此時的觀測值進行補償，即此時的觀測值減去殘差，使得最后觀測值得以修正。若用Zrec(k)表示補償后的觀測值，則可得到式(28):

Zrec(k)=Z(k)-Residualn,k

(28)

2.3.2 靜差(bias)的補償

(29)

2.3.3 漂移(drift)的補償

通過|μ|>0,|σ|>Threshold可以檢測到觀測值中的漂移，并標記為drift_Flag=1。與壞值和靜差不同，漂移的偏移量不為一個常數(shù)，如果采用移動檢測窗口中的殘差量對漂移的觀測值進行補償，觀測值的偏移量依然跟真實差距很大，這會導致EKF濾波后的狀態(tài)估計量依然偏離真實狀態(tài)量，補償結果并不理想，不利于動態(tài)目標跟蹤。

雖然漂移量不為具體的函數(shù)形式，但在局部位置偏移量仍然可以近似為一個線性擬合的方程。因此，對于存在漂移的觀測值，可以通過局部線性擬合的方式對觀測值進行近似補償。補償過程可以表示為式(30):

Zrec(k)=Z(k)-drift

(30)

式中,drift為式(27)中線性方程近似漂移量。如果此時p1近似為0，則可以認為此時的drift是bias，因此，當兩種粗大誤差均存在的情況下，為了更佳地補償效果，兩種粗大誤差均可以采用線性擬合進行補償。

3 實驗仿真

三維非線性系統(tǒng)狀態(tài)模型：

X(k)=[rx(k),ry(k),rz(k),vx(k),

vy(k),vz(k),ax(k),ay(k),az(k)]T

(31)

三維系統(tǒng)運動狀態(tài)方程可以表示為:

X(k+1)=FX(k)+GU(k)+W(k)

(32)

其中，F(xiàn)為狀態(tài)轉移矩陣，G為過程噪聲驅動矩陣，矩陣如下所示:

(33)

(34)

在此三維系統(tǒng)中，導彈對對動態(tài)目標采用純方位觀測，觀測為俯仰角和水平方向偏向角，實際觀測中雷達具有加性觀測噪聲，觀測方程為:

Z(k)=h[X(k)]+V(k)

(35)

式中，

(36)

在三維尋的制導的初始化中，采樣時間Δt=0.01 s，t=4.0 s。

導彈的初始狀態(tài):

x(0)=[3500,1500,1000,-1100,-150,-50,10,10,10]T

EKF濾波估計的初始化狀態(tài):

ex(0)=[3000,1000,800,-950,-100,-100,0,0,0]T

初始化EKF濾波估計的狀態(tài)協(xié)方差矩陣:

P0=[104×I6,06×3;03×6,102×I3]

實驗仿真中，在傳感器觀測值中加入粗大誤差(壞值、靜差和漂移)，對比優(yōu)化前后EKF的跟蹤狀態(tài)情況。得到經(jīng)粗大誤差檢測前后的觀測值如圖2所示。

圖2 粗大誤差檢測結果

圖2為三維尋的跟蹤的俯仰角和水平方向偏向角隨時間變化的曲線。圖2中，俯仰角的觀測值在0.8 s、1.3 s和3.6 s有壞值點存在，水平方向偏向角在0.4 s、1.6 s、2.0 s和3.0 s存在壞值點，2.2～2.6 s存在靜差，2.1～2.2 s存在漂移。改進型EKF能將所有的粗大誤差精確檢測和分類標記出來。精確檢測之后，改進型EKF算法可以通過式子(28)～(30)對不同的粗大誤差進行分類補償。

對傳統(tǒng)EKF和改進型EKF進行50組仿真實驗后，得到每組實驗的均方根誤差(RMSE)，經(jīng)統(tǒng)計后，所得結果如表1所示。

表1 傳統(tǒng)EKF與改進型EKF濾波后的50組仿真實驗均方根誤差(RMSE)統(tǒng)計結果

根據(jù)表1中傳統(tǒng)EKF和改進EKF在50次仿真實驗下的位置、速度和加速度的均方根誤差(RMSE)的數(shù)據(jù)，可以繪制出如圖3所示改進前后均方根誤差比較圖。

圖3 傳統(tǒng)EKF與改進型EKF濾波后的均方根誤差比較圖

由表1和圖3可知，在50次的仿真實驗中，改進型EKF算法的位置和速度的均方根誤差明顯比優(yōu)化前的均方根誤差要小，其中，改進型EKF濾波后位置均方根誤差的平均值比傳統(tǒng)EKF小33.9287，標準差比傳統(tǒng)EKF小0.9113。改進型EKF濾波后速度均方根誤差的平均值比傳統(tǒng)EKF小24.6123，標準差比傳統(tǒng)EKF小0.8696.這說明改進型EKF算法濾波受到粗大誤差干擾的影響更小，性能更穩(wěn)定，跟蹤的精確度更高。經(jīng)過傳統(tǒng)EKF和改進型EKF算法后，實驗仿真得到圖4所示三維尋的跟蹤狀態(tài)圖。

圖4 三維尋的跟蹤狀態(tài)圖

根據(jù)圖4所示尋的制導跟蹤狀態(tài)圖可知，在粗大誤差干擾情況下，傳統(tǒng)EKF的狀態(tài)估計值已經(jīng)嚴重偏離實際的狀態(tài)值。相比傳統(tǒng)EKF，改進型EKF濾波后的尋的制導跟蹤偏差明顯較小，沒有出現(xiàn)偏離實際狀態(tài)值，這說明三維動態(tài)目標的跟蹤精度較高。

5 結束語

本文針對動態(tài)系統(tǒng)目標跟蹤觀測過程中存在的壞值、靜差和漂移3種粗大誤差，提出了一種基于粗大誤差檢測和補償?shù)母倪M型EKF動態(tài)目標跟蹤算法。該方法能實時檢測粗大誤差并補償觀測量，能有效地減小粗大誤差對濾波結果的影響，從而提高跟蹤的精確度。仿真結果表明，相比于傳統(tǒng)EKF，改進型EKF算法濾波抗干擾能力較強，濾波性能較為穩(wěn)定，實現(xiàn)了對動態(tài)系統(tǒng)目標的準確跟蹤。