(溫州大學(xué) 電氣數(shù)字化設(shè)計(jì)技術(shù)國(guó)家地方聯(lián)合工程實(shí)驗(yàn)室，浙江 溫州 325035)

0 引言

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

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

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

1 擴(kuò)展卡爾曼濾波算法

非線性離散系統(tǒng)的動(dò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)為觀測(cè)方程。X(k)是n維系統(tǒng)狀態(tài)向量；Z(k)是m維觀測(cè)向量；f[k,X(k)]和h[k,X(k)]是X(k)和Z(k)對(duì)應(yīng)的非線性系統(tǒng)函數(shù)；G(k)是系統(tǒng)噪聲驅(qū)動(dòng)矩陣；W(k)則是零均值系統(tǒng)白噪聲矩陣；V(k)是零均值觀測(cè)白噪聲，且{W(k)}和{V(k)}互不相關(guān)。將式(1)中X(k+1)做一階泰勒展開(kāi)，得：

W(k)+φ(k)

(3)

(4)

(5)

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

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

(6)

(7)

(8)

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

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)轉(zhuǎn)移矩陣F(k+1|k)和觀測(cè)矩陣H(k)都是根據(jù)f[k,X(k)]和h[k,X(k)]的Jacobian矩陣代替。根據(jù)傳統(tǒng)線性卡爾曼濾波的遞推方程，對(duì)系統(tǒng)進(jìn)行濾波處理，得到非線性系統(tǒng)近似的最優(yōu)估計(jì)值。

與卡爾曼濾波的基本方程[1]類(lèi)比，可以得到擴(kuò)展卡爾曼濾波遞推方程：

(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ù)上一個(gè)k時(shí)刻的協(xié)方差值來(lái)估計(jì)k+1時(shí)刻的協(xié)方差值。P(k+1)表示k+1時(shí)刻更新的協(xié)方差值。

2 觀測(cè)值的粗大誤差

2.1 粗大誤差的類(lèi)型

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

圖1 存在粗大誤差情況下的觀測(cè)值

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

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

(16)

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

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

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

(17)

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

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

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

(18)

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

2.2 粗大誤差的檢測(cè)

對(duì)于動(dòng)態(tài)目標(biāo)識(shí)別中傳感器得到觀測(cè)數(shù)據(jù)中存在的粗大誤差值，需要對(duì)其進(jìn)行準(zhǔn)確檢測(cè)，從而進(jìn)行相應(yīng)的補(bǔ)償。

2.2.1 壞值(outlier)的檢測(cè)

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

壞值的檢測(cè)通?？刹捎靡览_(dá)準(zhǔn)則[7]。假設(shè)動(dòng)態(tài)目標(biāo)傳感器觀測(cè)值數(shù)據(jù)與擴(kuò)展卡爾曼濾波方程中預(yù)估的觀測(cè)數(shù)據(jù)偏差量的絕對(duì)值為Residualn,k，即殘差，可表示為式(19)。

(19)

(20)

根據(jù)貝塞爾法求得標(biāo)準(zhǔn)差σ。

(21)

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

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

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

2.2.2 靜差(bias)的檢測(cè)

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

(22)

(23)

(24)

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

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

(25)

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

(26)

式中，μ為一段時(shí)間內(nèi)殘差的平均值。

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

2.2.3 漂移(drift)的檢測(cè)

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

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

drift=p1k+p2

(27)

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

2.3 粗大誤差的補(bǔ)償

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

2.3.1 壞值(outlier)的補(bǔ)償

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

對(duì)于標(biāo)記為outlier_Flag=1的觀測(cè)值，可以通過(guò)式(21)中的殘差Residualn,ki來(lái)對(duì)此時(shí)的觀測(cè)值進(jìn)行補(bǔ)償，即此時(shí)的觀測(cè)值減去殘差，使得最后觀測(cè)值得以修正。若用Zrec(k)表示補(bǔ)償后的觀測(cè)值，則可得到式(28):

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

(28)

2.3.2 靜差(bias)的補(bǔ)償

(29)

2.3.3 漂移(drift)的補(bǔ)償

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

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

Zrec(k)=Z(k)-drift

(30)

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

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

三維非線性系統(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)運(yùn)動(dòng)狀態(tài)方程可以表示為:

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

(32)

其中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，G為過(guò)程噪聲驅(qū)動(dòng)矩陣，矩陣如下所示:

(33)

(34)

在此三維系統(tǒng)中，導(dǎo)彈對(duì)對(duì)動(dòng)態(tài)目標(biāo)采用純方位觀測(cè)，觀測(cè)為俯仰角和水平方向偏向角，實(shí)際觀測(cè)中雷達(dá)具有加性觀測(cè)噪聲，觀測(cè)方程為:

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

(35)

式中，

(36)

在三維尋的制導(dǎo)的初始化中，采樣時(shí)間Δt=0.01 s，t=4.0 s。

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

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

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

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

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

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

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

圖2 粗大誤差檢測(cè)結(jié)果

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

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

表1 傳統(tǒng)EKF與改進(jìn)型EKF濾波后的50組仿真實(shí)驗(yàn)均方根誤差(RMSE)統(tǒng)計(jì)結(jié)果

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

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

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

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

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

5 結(jié)束語(yǔ)

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