基于拉依達準則與線性擬合的改進型無跡卡爾曼濾波粗大誤差補償算法

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

0 引言

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

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

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

本文提出的改進型UKF采用拉依達法則對卡爾曼濾波中的粗大誤差進行檢測，并將不同種類的粗大誤差進行分類標記，采用估計值替代法和殘差擬合法補償粗大誤差。基于勻加速直線運動雙觀測目標跟蹤仿真實驗，將該改進型UKF應用于目標跟蹤上。結果表明，改進型UKF能有效地減小了粗大誤差觀測值對目標跟蹤的影響，明顯地提高了無跡卡爾曼濾波器的準確性與可靠性。

1 無跡卡爾曼濾波UKF

1.1 UKF原理

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

1.2 無跡卡爾曼濾波算法實現過程

具有白噪聲的狀態變量X和有觀測噪聲的觀測變量Z構成的非線性系統描述為：

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

(1)

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

(2)

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

(3)

λ=α2(n+κ)-n

(4)

(5)

(6)

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

(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點集來估計協方差值，Q和R分別為白噪聲和觀測噪聲中的協方差矩陣，Pzkzk和Pxkzk為系統觀測的協方差，K(k+1)為卡爾曼增益矩陣.

使用狀態函數f對sigma點集進行狀態預測，利用預測結果，進行加權求均值，得到系統狀態量的一步預測以及協方差矩陣。根據得到的預測值，再求取一組新的sigma點集。將新的sigma點集代入觀測方程函數h，得到觀測預測值，從而通過加權求和得到系統觀測預測的均值和協方差，計算卡爾曼增益矩陣。最后計算系統的預測狀態和協方差，得到的均值和協方差至少具有二階精度。

2 觀測中的粗大誤差

2.1 粗大誤差產生的原因

粗大誤差是指在觀測過程中，產生的不符合正態分布的數據的誤差。目標跟蹤過程中觀測是必不可少的內容，目標跟蹤的準確性取決于觀測數據的準確性，粗大誤差會直接影響觀測的結果，從而導致目標跟蹤軌跡紊亂。

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

2.2 粗大誤差的種類

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

圖1 含有不同粗大誤差的觀測數據

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

3 基于拉依達準則的粗大誤差檢測方法

3.1 拉依達準則

拉依達準則是以三倍觀測值的標準差σ作為極限取舍的標準[1-5]，因此拉依達準則也被稱作3σ準則。標準差σ為已經經過大量重復觀測后計算出的參數[3]，其計算式為：

(19)

拉依達準則判別依據如下：

(20)

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

3.2 粗大誤差檢測標記算法實現

本節針對離群值、靜差和漂移3種粗大誤差進行檢測標記，判定法則采用拉依達法則。

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

Z(k)=mark_outlier

(21)

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

(22)

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

為區分靜差和漂移，計算窗口內的標準差[6]，表達式為：

(23)

(24)

式中，ε是設定的閾值。若移動窗口內標準差小于或等于設定的閾值，表示窗口內的粗大誤差的是一個恒值誤差，則將窗口內的T個采樣點標記為靜差。若標準差大于設定的閾值，表示粗大誤差不符合正態分布，則標記為漂移。

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

由于目標跟蹤是一個即時更新的過程，在過程中每個檢測到的粗大誤差都需要即時補償，以免影響之后的目標跟蹤軌跡。因此可以認為在當前時刻之前的所有采樣點中將不存在含有粗大誤差，無跡卡爾曼濾波不再受到之前粗大誤差的影響，對當前時刻的狀態的預測是準確的。

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

Zrec(k)=Zukf(k)

(25)

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

若檢測的粗大誤差為靜差，由于靜差的殘差滿足正態分布且其標準差бT≤ε。窗口內采樣點的誤差符合正態分布，當前時刻的誤差近似于當前窗口的殘差平均值，因此利用該窗口的殘差平均值來補償，觀測值更新為：

Zrec(k)=Z(k)-μ

(26)

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

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

(27)

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

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

5 勻加速直線運動雙觀測目標跟蹤

5.1 案例分析

本節以勻加速直線運動為例，對雙觀測目標跟蹤進行分析。首先建立勻加速直線運動的模型，目標做勻加速直線運動，位置表達式為：

(28)

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

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

(29)

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

(30)

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

5.2 仿真與分析

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

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

圖2 目標狀態跟蹤軌跡圖

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

粗大誤差影響了傳統UKF對目標的跟蹤，從而導致對傳統UKF觀測估計值不準確，圖3中將粗大誤差標記并補償，改進型UKF觀測值能準確地跟蹤到目標，準確地反映目標的實時位置信息。

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

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

圖3 目標觀測軌跡圖

表1 均方根誤差(RMSE)統計結果

圖4 目標軌跡均方根誤差圖

6 結論

本文提出了一種采用基于拉依達法則檢測粗大誤差并分類補償3種粗大誤差的改進型無跡卡爾曼濾波算法。將其應用于目標跟蹤，通過對勻加速直線運動雙觀測目標跟蹤的仿真，結果顯示出該改進型算法能有效地增強無跡卡爾曼濾波對粗大誤差的抗干擾能力，實現對目標的準確跟蹤。