基于拉依達(dá)準(zhǔn)則與線性擬合的改進(jìn)型無跡卡爾曼濾波粗大誤差補(bǔ)償算法

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

0 引言

無跡卡爾曼濾波在目標(biāo)跟蹤方面得到了廣泛的應(yīng)用。不論是在軍事領(lǐng)域還是民用領(lǐng)域，目標(biāo)跟蹤技術(shù)都是十分重要的。但目前目標(biāo)跟蹤技術(shù)仍存在一些問題，傳感器等儀器觀測錯誤、儀器故障等情況，降低了目標(biāo)跟蹤的準(zhǔn)確性，需要通過濾波技術(shù)濾波、檢測并補(bǔ)償，從而降低目標(biāo)跟蹤的誤差。

目前，卡爾曼濾波技術(shù)主要有線性卡爾曼濾波(Kalman Filter, KF)、擴(kuò)展卡爾曼濾波 (Extended Kalman Filter, EKF)、無跡卡爾曼濾波(Unscented Kalman Filter, UKF)和粒子濾波(Particle Filter, PF)等。KF只適用于隨機(jī)線性動態(tài)系統(tǒng)。 EKF使KF能適用于非線性系統(tǒng)，但其線性化的過程存在近似誤差，精度不高。隨后， UKF克服了EKF估計精度低、穩(wěn)定相差的問題。本文采用UKF對目標(biāo)跟蹤進(jìn)行狀態(tài)預(yù)測估計。

目前，亦有大量學(xué)者對卡爾曼濾波在目標(biāo)跟蹤中出現(xiàn)的誤差進(jìn)行了深入研究。熊艷艷等人歸納了4種粗大誤差的判別準(zhǔn)則，并對其特點(diǎn)進(jìn)行了對比分析[1-5]。Bretas提出了狀態(tài)估計的兩步方法，用估計值將粗大誤差代替，實現(xiàn)了粗大誤差的補(bǔ)償[6]。Zhu等人提出了一種魯棒粒子濾波的粗大誤差識別算法，有效地消除或補(bǔ)償了粗大誤差[7]。Ting等人介紹了一種改進(jìn)的卡爾曼濾波器，使用加權(quán)最小二乘的方法，對觀測數(shù)據(jù)進(jìn)行魯棒的實時離群檢測[8]。燕必希等人做了單目相機(jī)對目標(biāo)跟蹤定位的實驗，當(dāng)運(yùn)動目標(biāo)發(fā)生遮擋時,能夠完成對目標(biāo)的定位和跟蹤[9]。劉柯在卡爾曼濾波對目標(biāo)跟蹤的基礎(chǔ)上，統(tǒng)合了傳統(tǒng)的均值漂移算法[10]。

本文提出的改進(jìn)型UKF采用拉依達(dá)法則對卡爾曼濾波中的粗大誤差進(jìn)行檢測，并將不同種類的粗大誤差進(jìn)行分類標(biāo)記，采用估計值替代法和殘差擬合法補(bǔ)償粗大誤差?；趧蚣铀僦本€運(yùn)動雙觀測目標(biāo)跟蹤仿真實驗，將該改進(jìn)型UKF應(yīng)用于目標(biāo)跟蹤上。結(jié)果表明，改進(jìn)型UKF能有效地減小了粗大誤差觀測值對目標(biāo)跟蹤的影響，明顯地提高了無跡卡爾曼濾波器的準(zhǔn)確性與可靠性。

1 無跡卡爾曼濾波UKF

1.1 UKF原理

無跡卡爾曼濾波采用卡爾曼線性濾波框架，對于一步預(yù)測方程，使用UT變換來處理均值和協(xié)方差的非線性傳遞問題。無跡卡爾曼濾波對非線性函數(shù)的概率密度分布進(jìn)行近似，用一系列確定樣本來逼近狀態(tài)的后驗概率密度。無跡卡爾曼濾波沒有把高階項忽略，因此對非線性問題有較高的準(zhǔn)確度，克服了擴(kuò)展卡爾曼濾波估計精度低、穩(wěn)定性差的問題。

1.2 無跡卡爾曼濾波算法實現(xiàn)過程

具有白噪聲的狀態(tài)變量X和有觀測噪聲的觀測變量Z構(gòu)成的非線性系統(tǒng)描述為：

X(k+1)=f(X(k),W(k))

(1)

Z(k+1)=h(X(k),V(k))

(2)

式中，f是非線性狀態(tài)方程函數(shù)，h是非線性觀測方程，W(k)是白噪聲，V(k)是觀測噪聲，k是當(dāng)前時刻。無跡卡爾曼濾波的核心是UT變換，UT變換需要先產(chǎn)生2n+1個sigma點(diǎn)，并計算其權(quán)值，得到：

(3)

λ=α2(n+κ)-n

(4)

(5)

(6)

從而可得到無跡卡爾曼濾波算法實現(xiàn)過程，如下：

(7)

X(i)(k+1|k)=f[k,X(i)(k|k)]

(8)

(9)

P(k+1|k)=

-X(i)(k+1|k)]T+Q

(10)

(11)

Z(i)(k+1|k)=h[X(i)(k+1|k)]

(12)

(13)

(14)

(15)

K(k+ 1) =Pxk zkPzk zk-1

(16)

(17)

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

PZKZKKT(k+1)

(18)

式中，P(k+1|k)表示利用上一步sigma點(diǎn)集來估計協(xié)方差值，Q和R分別為白噪聲和觀測噪聲中的協(xié)方差矩陣，Pzkzk和Pxkzk為系統(tǒng)觀測的協(xié)方差，K(k+1)為卡爾曼增益矩陣.

使用狀態(tài)函數(shù)f對sigma點(diǎn)集進(jìn)行狀態(tài)預(yù)測，利用預(yù)測結(jié)果，進(jìn)行加權(quán)求均值，得到系統(tǒng)狀態(tài)量的一步預(yù)測以及協(xié)方差矩陣。根據(jù)得到的預(yù)測值，再求取一組新的sigma點(diǎn)集。將新的sigma點(diǎn)集代入觀測方程函數(shù)h，得到觀測預(yù)測值，從而通過加權(quán)求和得到系統(tǒng)觀測預(yù)測的均值和協(xié)方差，計算卡爾曼增益矩陣。最后計算系統(tǒng)的預(yù)測狀態(tài)和協(xié)方差，得到的均值和協(xié)方差至少具有二階精度。

2 觀測中的粗大誤差

2.1 粗大誤差產(chǎn)生的原因

粗大誤差是指在觀測過程中，產(chǎn)生的不符合正態(tài)分布的數(shù)據(jù)的誤差。目標(biāo)跟蹤過程中觀測是必不可少的內(nèi)容，目標(biāo)跟蹤的準(zhǔn)確性取決于觀測數(shù)據(jù)的準(zhǔn)確性，粗大誤差會直接影響觀測的結(jié)果，從而導(dǎo)致目標(biāo)跟蹤軌跡紊亂。

粗大誤差主要由觀測過程中某些不確定或者不可控的意外因素引起，有多種的可能性，比如儀器故障、觀測設(shè)備不準(zhǔn)確等問題。粗大誤差是隨機(jī)的，它的產(chǎn)生是難以避免的。因此在無跡卡爾曼濾波中需要將其檢測、標(biāo)記并補(bǔ)償，以此來減小甚至是完全剔除粗大誤差的影響。

2.2 粗大誤差的種類

粗大誤差有3種典型的類型，分別為離群值(Outlier)、靜差(Bias)和漂移(Drift)。離群值如圖1(a)所示，離群值一般為觀測數(shù)據(jù)隨機(jī)產(chǎn)生的誤差超過正常范圍的個別分散的點(diǎn)。靜差如圖1(b)所示，靜差是一段連續(xù)的具有粗大誤差的連續(xù)的點(diǎn)，其粗大誤差符合正態(tài)分布，靜差也可以看作一段連續(xù)的離群值。儀器故障易產(chǎn)生靜差。漂移可以是線性的、非線性的，也可以是周期性的。漂移如圖1(c)所示，漂移影響比其他兩種粗大誤差的影響大，漂移的影響隨時間增大。當(dāng)觀測有漂移時，觀測裝置的校準(zhǔn)就會更加困難。靜差可以看成是一種特殊情況下的漂移。

圖1 含有不同粗大誤差的觀測數(shù)據(jù)

帶有以上粗大誤差的觀測都會導(dǎo)致目標(biāo)跟蹤不準(zhǔn)確，導(dǎo)致無跡卡爾曼濾波性能下降。因此，需要時刻檢測觀測過程中的粗大誤差，考慮到以上3種粗大誤差，本文提出了一種無跡卡爾曼濾波粗大誤差檢測方法，可以將粗大誤差分類標(biāo)記。

3 基于拉依達(dá)準(zhǔn)則的粗大誤差檢測方法

3.1 拉依達(dá)準(zhǔn)則

拉依達(dá)準(zhǔn)則是以三倍觀測值的標(biāo)準(zhǔn)差σ作為極限取舍的標(biāo)準(zhǔn)[1-5]，因此拉依達(dá)準(zhǔn)則也被稱作3σ準(zhǔn)則。標(biāo)準(zhǔn)差σ為已經(jīng)經(jīng)過大量重復(fù)觀測后計算出的參數(shù)[3]，其計算式為：

(19)

拉依達(dá)準(zhǔn)則判別依據(jù)如下：

(20)

相別于其他準(zhǔn)則，拉依達(dá)法則適用于大量觀測次數(shù)的情況，σ經(jīng)過大量數(shù)據(jù)計算更加準(zhǔn)確。根據(jù)概率論統(tǒng)計，誤差服從正態(tài)分布時，誤差大于3σ的概率極小[4]。因此采用拉依達(dá)法則判別粗大誤差時，雖然可能會有不合理的離群值不能被檢測出來，但是概率極小，因此本文方法采用拉依達(dá)法則。

3.2 粗大誤差檢測標(biāo)記算法實現(xiàn)

本節(jié)針對離群值、靜差和漂移3種粗大誤差進(jìn)行檢測標(biāo)記，判定法則采用拉依達(dá)法則。

根據(jù)圖1(a)可知，離群值為孤立的一個點(diǎn)，前一個采樣點(diǎn)和后一個采樣點(diǎn)均為正常值。因此利用式(20)計算每個采樣點(diǎn)的殘差Rn，若前一個采樣點(diǎn)和后一個采樣點(diǎn)的Rn≤3σ，則該點(diǎn)標(biāo)記為離群值。

Z(k)=mark_outlier

(21)

根據(jù)圖1(b)可知，靜差為一段連續(xù)的離群點(diǎn)，且靜差段的殘差不隨時間改變。而根據(jù)圖1(c)可知，漂移也為一段連續(xù)的離群點(diǎn)，但其漂移段的殘差大小不斷增大，不符合正態(tài)分布，期望值μ大于upp。upp為設(shè)定的閥值。因此先判定粗大誤差為靜差還是漂移。若前一個采樣點(diǎn)和當(dāng)前采樣點(diǎn)均滿足拉依達(dá)法則，則設(shè)置一個時間步長為T移動窗口，該移動窗口包含T個采樣點(diǎn)，隨著時刻移動。計算窗口內(nèi)的正態(tài)分布期望值μ，即殘差平均值，表達(dá)式為：

(22)

若μ>upp，則判定窗口內(nèi)的誤差為靜差或者漂移。若μ< upp，則表示窗口內(nèi)的誤差符合正態(tài)分布，是正常誤差。

為區(qū)分靜差和漂移，計算窗口內(nèi)的標(biāo)準(zhǔn)差[6]，表達(dá)式為：

(23)

(24)

式中，ε是設(shè)定的閾值。若移動窗口內(nèi)標(biāo)準(zhǔn)差小于或等于設(shè)定的閾值，表示窗口內(nèi)的粗大誤差的是一個恒值誤差，則將窗口內(nèi)的T個采樣點(diǎn)標(biāo)記為靜差。若標(biāo)準(zhǔn)差大于設(shè)定的閾值，表示粗大誤差不符合正態(tài)分布，則標(biāo)記為漂移。

4 無跡卡爾曼濾波粗大誤差補(bǔ)償方法

由于目標(biāo)跟蹤是一個即時更新的過程，在過程中每個檢測到的粗大誤差都需要即時補(bǔ)償，以免影響之后的目標(biāo)跟蹤軌跡。因此可以認(rèn)為在當(dāng)前時刻之前的所有采樣點(diǎn)中將不存在含有粗大誤差，無跡卡爾曼濾波不再受到之前粗大誤差的影響，對當(dāng)前時刻的狀態(tài)的預(yù)測是準(zhǔn)確的。

離群值一般為觀測數(shù)據(jù)隨機(jī)產(chǎn)生的不符合正態(tài)分布的點(diǎn)，對其他觀測數(shù)據(jù)的影響較小。當(dāng)檢測到一個離群值時，直接用該時刻采樣點(diǎn)的無跡卡爾曼濾波估計值代替離群值。這樣可在離群值對之后的狀態(tài)預(yù)測產(chǎn)生影響前，用一個接近于真實值的估計值將其代替，使無跡卡爾曼濾波能繼續(xù)準(zhǔn)確地預(yù)測之后的狀態(tài)。離群值是單個離散的點(diǎn)，其殘差無法估計，只能采用替代的補(bǔ)償方法。

Zrec(k)=Zukf(k)

(25)

式中，Zrec是補(bǔ)償之后的觀測值，Zukf是無跡卡爾曼濾波觀測估計值。

若檢測的粗大誤差為靜差，由于靜差的殘差滿足正態(tài)分布且其標(biāo)準(zhǔn)差бT≤ε。窗口內(nèi)采樣點(diǎn)的誤差符合正態(tài)分布，當(dāng)前時刻的誤差近似于當(dāng)前窗口的殘差平均值，因此利用該窗口的殘差平均值來補(bǔ)償，觀測值更新為：

Zrec(k)=Z(k)-μ

(26)

若檢測的粗大誤差為漂移，對移動窗口內(nèi)T個采樣點(diǎn)的殘差進(jìn)行線性擬合。在當(dāng)前k時刻，采用一種基于線性回歸的快速簡便的方法，將移動窗口內(nèi)的殘差擬合成一條直線，預(yù)測當(dāng)前時刻的殘差為這條直線上當(dāng)前時刻的值。[7]因此利用預(yù)測的殘差補(bǔ)償粗大誤差，觀測值更新為：

Zrec(k)=Z(k)-Y(i)

(27)

靜差和漂移的影響遠(yuǎn)大于離群值，一般由儀器故障產(chǎn)生。若是仍用觀測估計值替代粗大誤差，補(bǔ)償效果不明顯。移動窗口可以通過前幾個采樣點(diǎn)的對當(dāng)前的粗大誤差進(jìn)行預(yù)測，綜合分析連續(xù)粗大誤差的影響，預(yù)測當(dāng)前的粗大誤差，以此來補(bǔ)償比直接替代更加準(zhǔn)確。

式(25)、式(26)、式(27)中，每一次粗大誤差補(bǔ)償，只是將有誤的觀測值進(jìn)行修正，狀態(tài)值和協(xié)方差尚未修正。所以每次觀測值更新之后需要重新進(jìn)行UT變換，剔除粗大誤差的影響，準(zhǔn)確地跟蹤目標(biāo)的位置。

5 勻加速直線運(yùn)動雙觀測目標(biāo)跟蹤

5.1 案例分析

本節(jié)以勻加速直線運(yùn)動為例，對雙觀測目標(biāo)跟蹤進(jìn)行分析。首先建立勻加速直線運(yùn)動的模型，目標(biāo)做勻加速直線運(yùn)動，位置表達(dá)式為：

(28)

目標(biāo)在一個二維平面內(nèi)的勻加速直線運(yùn)動，可以分解為同時在x軸和y軸方向上做勻加速直線運(yùn)動。根據(jù)式(1)和式(2)的得出狀態(tài)方程和觀測方程分別為：

X(k+1)=ΦX(k)+W(k)

(29)

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

(30)

式中，觀測變量有兩個，一個與勻速直線運(yùn)動相同是目標(biāo)相對于觀測點(diǎn)的距離，另一個是目標(biāo)相對于觀測點(diǎn)的角度。勻加速直線運(yùn)動如果加速度方向于初速度方向不同，目標(biāo)將會做曲線運(yùn)動，僅觀測相對距離無法確定具體位置，增加一個相對角度，可以在笛卡爾坐標(biāo)系上確定目標(biāo)的位置。

5.2 仿真與分析

建立勻加速直線運(yùn)動模型。設(shè)置觀測次數(shù)N=50，采樣時間T=0.5 s，初始狀態(tài)X(0)=[1000,5000,10,50,2,-4]T。設(shè)置UT變換參數(shù)α=le-2，β=2，κ=0，維數(shù)L為狀態(tài)X的維數(shù)L=6。分別在k=5、k=30處設(shè)置離群值，在k=10-20時刻設(shè)置靜差，在k=35-45時刻處設(shè)置漂移。

設(shè)置粗大誤差后，目標(biāo)跟蹤軌跡如圖2(a)所示，由于靜差和漂移的影響，傳統(tǒng)UKF對目標(biāo)的跟蹤完全偏離了實際的目標(biāo)運(yùn)動軌跡，粗大誤差對目標(biāo)跟蹤的影響很嚴(yán)重。采取補(bǔ)償之后的目標(biāo)跟蹤軌跡如圖2(b)所示，與圖2(a)相比，將粗大誤差補(bǔ)償之后，改進(jìn)型UKF能準(zhǔn)確地跟蹤到目標(biāo)的位置。

圖2 目標(biāo)狀態(tài)跟蹤軌跡圖

在補(bǔ)償前先將3種粗大誤差分類標(biāo)記，粗大誤差標(biāo)記結(jié)果如圖3所示，圖中圓形圈出的是離群值,矩形圈出的是靜差，三角形圈出的是漂移。在進(jìn)入靜差和漂移的第一個點(diǎn)，因為實際情況中后續(xù)的實際位置和觀測數(shù)據(jù)是無法預(yù)知的，因此判斷為離群值，該離群值為靜差和漂移的起始點(diǎn)，當(dāng)粗大誤差為靜差時，移動窗口中靜差有跳變，會誤判為漂移，可以通過增大移動窗口時間步長T，來減小跳變對判斷的影響。

粗大誤差影響了傳統(tǒng)UKF對目標(biāo)的跟蹤，從而導(dǎo)致對傳統(tǒng)UKF觀測估計值不準(zhǔn)確，圖3中將粗大誤差標(biāo)記并補(bǔ)償，改進(jìn)型UKF觀測值能準(zhǔn)確地跟蹤到目標(biāo)，準(zhǔn)確地反映目標(biāo)的實時位置信息。

RMSE是均方根誤差，能夠反映出觀測的精密度，傳統(tǒng)UKF與改進(jìn)型UKF的100組仿真實驗統(tǒng)計結(jié)果如表1和圖4所示。

由表1和圖4可知，在100次的仿真實驗中，改進(jìn)型UKF的均方根誤差遠(yuǎn)小于傳統(tǒng)UKF，x軸均方根誤差減小了6.5569，y軸均方根誤差減小了18.3280.這說明改進(jìn)型UKF算法的效果很明顯。其中，粗大誤差對y軸分量的影響更大，因此對y軸分量的補(bǔ)償效果也更加明顯。相比于傳統(tǒng)UKF，改進(jìn)型UKF明顯增強(qiáng)了UKF對粗大誤差的抗干擾能力，使UKF更加穩(wěn)定、準(zhǔn)確。

圖3 目標(biāo)觀測軌跡圖

表1 均方根誤差(RMSE)統(tǒng)計結(jié)果

圖4 目標(biāo)軌跡均方根誤差圖

6 結(jié)論

本文提出了一種采用基于拉依達(dá)法則檢測粗大誤差并分類補(bǔ)償3種粗大誤差的改進(jìn)型無跡卡爾曼濾波算法。將其應(yīng)用于目標(biāo)跟蹤，通過對勻加速直線運(yùn)動雙觀測目標(biāo)跟蹤的仿真，結(jié)果顯示出該改進(jìn)型算法能有效地增強(qiáng)無跡卡爾曼濾波對粗大誤差的抗干擾能力，實現(xiàn)對目標(biāo)的準(zhǔn)確跟蹤。