無跡卡爾曼濾波算法在目標跟蹤中的研究

郝曉靜，李國新，李明珠，張亞粉，常曉鳳

（長安大學 信息工程學院，陜西 西安 710064）

在目標跟蹤中，可靠而精確的目標跟蹤是目標跟蹤系統設計的主要目的。而對目標跟蹤技術的研究，在軍事以及民用領域中都有著十分重要的意義。對于目標跟蹤問題的解決，不同的學者提出過不同的解決方案。在應用卡爾曼濾波方法解決許多實際問題時，狀態方程或量測方程表現為非線性，那么濾波問題也表現為非線性。根據卡爾曼濾波方法，解決非線性濾波問題的最優方案需要得到其系統方程的完整描述。然而這種精確的描述需要無盡的參數而無法進行真正實際應用。為此人們提出了大量次優的近似方法[1-2]。對于非線性濾波問題的次優近似，共有兩大途徑：1）將系統方程中，非線性部分線性化，對高階項采用忽略或逼近的方法；2）利用采樣的方法，對近似非線性分布和非線性函數進行線性化近似。

1 無跡卡爾曼濾波（UKF）

UKF方法是采用采樣策略逼近非線性分布的方法。UKF方法以UT變換[3-5]為基礎，采用卡爾曼線性濾波框架，具體采樣形式為確定性采樣，而非粒子濾波方法[6]的隨機采樣。UKF方法采樣的粒子點（一般稱為Sigma點）的個數很少，具體個數根據所選的采樣策略而定。UKF采用的是確定性采樣，從而避免了粒子濾波方法的粒子退化問題。UT變換是UKF算法的核心和基礎。它是計算進行非線性傳遞的隨機向量概率的一種方法。具體變換方法可用圖1解釋。

圖1 UT變換原理Fig.1 UT transform principle

UT變換的思想是：在確保采樣均值和協方差為x和Px的前提下，選擇一組點集（Sigma點集），將非線性變換應用于采樣的每個Sigma點，得到非線性轉換后的點集y和Py是變換后Sigma點集的統計量。

UT變換算法框架[7-8]如下：

1）根據輸入變量x的統計量x和 Px，選擇一種 Sigma點采樣策略，得到輸入變量的 Sigma 點集{χi}，i=1，…，2n+1，以及對應的權值Wmi和Wci。其中：2n+1為所采用的采樣策略的采樣Sigma點個數，Wmi為均值加權所用權值，Wci為協方差加權所用權值。如果不采用比例修正，則Wmi=Wci=Wi。

2）對所采樣的輸入變量Sigma點集{χi}中的每個 Sigma點進行f（·）非線性變換，得到變換后的Sigma點集{yi}。

3）對變換后的Sigma點集{yi}進行加權處理，從而得到輸出變量y的統計量y和Py，具體的權值仍然依據對輸入變量x進行采樣的各個Sigma點的對應權值Wmi和Wci。

在對稱采樣中，由2n+1個對稱點來近似估計均值為x和協方差為Px。其中滿足要求的附帶相應權值的采樣點，由下式確定：

其中κ是一調節參數，用來控制每個點到均值的距離，僅影響二階之后的高階矩帶來的偏差；是矩陣（n+κ）Px平方根的第i列，可以利用喬勒斯基分解進行計算。若Px的矩陣平方根為A，可表示為Px=ATA的形式，則采樣點為A的行向量，若Px=AAT，則采樣點為A的列向量。Wi是與第i個Sigma點相應的權值。且有：

隨著系統維數的增加，Sigma點到中心x的距離越來越遠，會產生采樣的非局部效應，對于許多非線性函數會產生一些問題，如κ為負數會導致方差矩陣的非正定。比例采樣，可有效地解決采樣非局部效應問題，并可適用于修正多種采樣策略。比例采樣修正算法如下：

式中：α為正值的比例縮放因子，可通過調整α的取值來調節Sigma點與的距離；β為引入f（·）高階項信息的參數，當不使用高階項信息時，β=2。

將比例修正算法應用于對稱采樣中，得到比例對稱采樣方法。具體的Sigma點采樣公式為：

其中 λ=α2（n+κ）-n 是調節參數，控制 Sigma 點與均值的距離。參數確定的一般取值的范圍：α決定了采樣點x在附近的分布程度，通常取值的范圍是10-4≤α≤1；參數β用來描述x的分布新息，對于正態高斯分布情況，β=2是最有效的；而κ是一個比例參數，通常設置為0或3-n。

綜上所述，UT變換是將輸入變量的統計特性通過非線性系統傳播。傳播后的均值和協方差矩陣是狀態方程真實統計特性的更好逼近，它的計算精度比線性化近似有了很大的提高。由十在計算過程中，它無需進行雅克比矩陣計算，因而實現起來更加簡便快捷。

UKF不必線性化非線性狀態方程或量測方程，它直接利用非線性狀態方程來估算狀態向量的概率密度函數。UKF規定一組確定的采樣點，當狀態向量的概率密度函數是高斯型的，利用這組采樣點能獲取高斯密度函數的均值和協方差。當高斯型狀態向量經非線性系統進行傳遞時，對任何一種非線性系統，利用這組采樣點能獲取精確到二階矩的后驗均值和協方差。

設 x 是 nx維隨機向量，f：Rnx→Rnx：是非線性函數 y=g（x），假定x的均值和協方差分別是x和Px，利用UT方法計算y的一、二階矩的步驟[7]如下：

1）計算2n+1個采樣點 χi及相應的權值Wmi和Wci。

2）通過非線性方程傳遞采樣點。

3）估算y的均值和協方差。

在UKF算法中，當需要對噪聲進行估計時，需要對狀態進行擴維處理。假定在時刻k，狀態估計和協方差分別為x^k/k，Pk/k，狀態方程和量測方程分別為：

狀態初始條件為：

狀態的初始條件擴維：

當不要求對噪聲進行估計時，即不需要對狀態進行擴維時就可以得到簡化UKF算法，簡化UKF算法只對狀態進行Sigma點采樣。簡化的UKF算法的步驟如下：

1）Sigma點采樣，根據UT變換計算狀態向量矩陣

采用采樣策略，得到 k時刻狀態估計的Sigma點集{χk/k（i）}，i=1，…，2n+1和相應的權值Wmi和Wci。其中2n+1為所采用的采樣策略的采樣Sigma點個數。Wmi和Wci的值如上所述。

2）利用狀態方程傳遞采樣點 χk+1/k（i）=f（ χk/k（i））

3）利用預測采樣點 χk+1/k（i），權值 Wmi，Wci計算預測均值x^k+1/k和協方差矩陣Pk+1/k

4）利用2）預測量測采樣點

5）預測量測值和協方差

這里，Pzz是量測向量協方差矩陣；Pxz是狀態向量與量測向量的協方差矩陣。

6）計算UKF增益，更新狀態向量和方差

分析上述過程可以看出算法過程主要有2個方面，一方面是對時間的更新另一方面是對量測的更新。該算法適用于任意非線性系統模型，它不需要估算雅克比矩陣，實現簡便。

2 仿真實驗

仿真一：目標從（1 000 m，800 m，600 m）處飛行，以（200 m/s，250 m/s，300 m/s）的速度和（50 m/s2，30 m/s2，40 m/s2）的加速度加速飛行，仿真飛行的時間為120 s；機動頻率α為0.5；采樣周期T為1 s；過程噪聲的方差為1 600；距離、方位角、俯仰角的測量誤差分別為100，1，1；狀態向量的維數n=9；初始值的方差矩陣為 diag[1 1 1 1 1 1 1 1 1]。UKF 參數設置 α′=1，κ=0，β=2。 仿真結果如圖 2～圖 5所示。

圖2 位置的真實值和估計值Fig.2 Position real value and estimated value

圖3 速度的真實值和估計值Fig.3 Velocity real value and estimated value

圖4 加速度的真實值和估計值Fig.4 Acceleration real value and estimated value

圖5 加速度估計誤差Fig.5 Acceleration estimate error

從圖2～圖4可以看出UKF濾波算法對機動目標的跟蹤的位置、速度有很高的精度，對加速度大概在60 s時跟蹤上機動目標運動的加速度；圖5說明加速度的跟蹤誤差隨著時間的增加而在不斷減小，在120 s左右誤差就接近為0，因此可以得出UKF濾波算法具有很好的跟蹤效果。

仿真二：目標從（1 000 m，800 m，600 m）處飛行，以（200 m/s，250 m/s，300 m/s）的速度勻速飛行，仿真飛行時間為 120 s，在前60 s內做勻速運動， 在60 s到120 s以內 （38 m/s2，30 m/s2，40 m/s2）以做勻加速運動。機動頻率為0.5；采樣周期為1 s；過程噪聲的方差為1 600；距離、方位角、俯仰角的測量誤差分別為100，1，1；狀態向量的維數n=9；初始值的方差矩陣為diag[1 1 1 1 1 1 1 1 1]。UKF 參數設置 α′=1，κ=0，β=2。仿真結果如圖6～圖9所示。

圖6 位置的真實值和估計Fig.6 Position real value and estimated value

圖7 速度的真實值和估計值Fig.7 Velocity real value and estimated value

圖8 加速度的真實值和估計值Fig.8 Acceleration real value and estimated value

圖9 加速度估計誤差Fig.9 Acceleration estimate error

由圖6～8可以看出，當機動目標在不同的時間段內分別作兩種運動時，UKF濾波算法對狀態向量在坐標軸x軸方向的位置、速度同樣有很高的精度。加速度的估計值在第一段運動的過程中，大概經過30 s就能跟蹤上機動目標的加速度，在第二段運動的過程中，在經過30 s左右的時間就能對機動的目標又很好的跟蹤。圖9表明目標在發生機動的時刻60 s左右由很大的跟蹤誤差，但經過10左右的時間誤差就會相應的減少。因此，可以看出機動目標在不同時間段做不同的運動時，UKF算法對機動目標的跟蹤同樣有很高的精度和穩定性。

3 結 論

文中對無跡卡爾曼濾波方法進行了分析和總結，無跡卡爾曼濾波方法能有效處理系統方程非線性強度大的情況，濾波精度和穩定性也好于卡爾曼濾波方法，但要求噪聲是高斯白噪聲。并通過兩個仿真例子，對目標跟蹤的位置、速度、加速度進行深入分析討論。仿真結果表明無跡卡爾曼濾波算法對機動目標的跟蹤有很高的精度和穩定性。

[1]Maybeck P S.Stochastic models estimation and control[M].New York:Academic，1982.

[2]Jazwinski A H.Stochastic processes and filtering thery[M].New York:Academic，1970.

[3]王志剛，芮國勝，胡昊.UPF算法及其在目標跟蹤中的應用[J].雷達科學與技術，2005（2）:21－23.WANG Zhi-gang，RUI Guo-sheng，HU Hao.Unscented particle filter algorithm and its application to target tracking[J].Radar Science and Technology，2005（2）:21－23.

[4]Julier S J，UhlmannJ K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE，2004，92（3）:401－422.

[5]韓春雷，陳赤聯，宋明，等.卡爾曼濾波在被動目標跟蹤系統中的應用[J].電子科技，2012（4）：47－50，53.HAN Chun-lei，CHEN Chi-lian，SONG Ming，et al.Application of Kalman filter in passive target tracking[J].Electronic Science and Technology，2012（4）：47－50，53.

[6]朱志宇.粒子濾波算法及其應用 [M].北京：科學出版社，2010.

[7]Julier S J.The scaled unscented transformation.Proc of American Control Conf[C]//Jedderson City，2002:4555－4559.

[8]Julier S J，Durrant-Whyte H F.Navigation and parameter estimation of high speed road vehicles[C]//In Robotics and Automation Conference，1995:101－105.