交互多模型擴展卡爾曼濾波算法的FPGA實現

,

(清華大學 自動化系,北京 100084)

0 引 言

目標跟蹤(target tracking)利用探測器對運動目標的某些參數進行測量，以此確定關注區域內目標個數、位置以及運動參數等狀態,其在國防與民用領域均有重要應用價值[1]。根據探測器是否主動發射信號，又可分為主動跟蹤與被動跟蹤。在被動跟蹤方法中，純方位角度跟蹤因其具有隱蔽性，備受各國軍方重視。

純方位角度跟蹤利用單站測向設備測量目標的運動方位信息，并用這些方位信息序列來實時估計目標軌跡[2]。目標跟蹤的經典方法有卡爾曼濾波(KF)、擴展卡爾曼濾波(EKF)以及粒子濾波(PF)等。交互多模型擴展卡爾曼濾波(IMMEKF)算法[3]是多模型算法中的一種經典算法，其基本思想是建立一個由若干模型組成的模型集，目標以一定概率符合某個運動模型。各模型均采用EKF算法進行濾波。IMMEKF算法具有一定的非線性處理能力，且運算速度較快。

當前，對于純方位角度跟蹤問題的研究大多集中在算法和仿真方面[4～6]，對于算法的硬件實現方面的研究則較少。而實際應用中，大量的移動平臺亟需準確度、實時性均高的跟蹤算法。將跟蹤算法硬件化，乃至設計出專用處理芯片是目標跟蹤領域的一個重要研究方向。現場可編程門陣列(field-programmable gate array,FPGA)是一種擁有大量計算和存儲資源的高性能芯片，其最大的優點是可以通過可重復配置快速實現設計驗證。硬件算法如果要轉化成專用處理芯片，可以先通過FPGA驗證其設計的正確性。文獻[7]對數種PF算法進行了修改，提出新的重采樣方法，以期得到適于硬件實現并提升處理速度的結構。同時，提出了一種分布式PF結構，提高了濾波器的可擴展性。文獻[2]針對純方位角度跟蹤問題提出了一種簡化的PF算法，并給出了FPGA上的實現方法。但是，上述研究都是基于單模型算法，對于多模型算法的硬件實現卻很少有人研究。作為多模型算法中最經典的算法，研究IMMEKF算法的硬件實現能夠為其他多模型算法的硬件實現奠定基礎。本文針對IMMEKF算法，將其轉化為易于硬件實現的結構，并用Verilog HDL語言將其在FPGA上硬件實現。

1 純方位角度跟蹤問題

目標跟蹤問題的本質是根據被噪聲污染的觀測數據來估計目標的狀態[8]。假設目標在二維水平面上運動，則目標的運動狀態可以用如下向量來表示

(1)

(2)

其中，Φ為狀態轉移矩陣，而Γ為系統噪聲轉移矩陣，H為觀測方程狀態轉移矩陣，G為觀測方程噪聲轉移矩陣，Wk-1和Vk均為均值為零的高斯白噪聲，方差分別為Qk-1和Rk。觀測方程是

(3)

觀測值為k時刻目標與觀測點連線和Y軸夾角的正切值。

2 IMMEKF算法與仿真

2.1 IMMEKF算法

IMMEKF算法的每個模型都匹配擴展卡爾曼濾波器。計算流程可分為4個步驟：

(4)

(5)

(6)

(7)

4)合成：將每個模型的估計結果合成

(8)

(9)

IMMEKF算法流程如圖1所示。

圖1 IMMEKF算法流程圖

2.2 仿真結果

表1 運動過程

從圖2可以看出:估計軌跡與真實軌跡基本重合，算法較準確地跟蹤了目標。從圖3可以看到:在前1/3時間內，模型1(CV)的概率值分布在0.8～0.9之間，處于絕對優勢地位;第二個1/3時間內，模型2(CT)的概率值分布在0.7～0.8之間;而在最后1/3時間內，模型1(CV)的概率值又分布在0.8～0.9之間。這說明算法能夠較準確地對模型進行估計。而在模型轉換時刻，概率值迅速發生跳變，這也保證了跟蹤精度。

圖2 IMMEKF算法跟蹤效果圖

圖3 IMMEKF算法各模型概率圖

3 IMMEKF算法FPGA實現方案與仿真結果

本節介紹IMMEKF算法硬件實現的過程與在FPGA上進行硬件實現的方案和仿真結果,IMMEKF硬件算法模塊框圖如圖4所示。

圖4 IMMEKF硬件算法模塊框圖

算法所需的常量，如狀態轉移矩陣等，全部存儲在FPGA的ROM中,當接收到新測量值Zk時，將所有數據輸入濾波器進行處理。經過混合、濾波、更新權重、合成4個步驟后得到估計值k,然后等待下一次測量值的輸入。

3.1 FPGA浮點數矩陣運算

IMMEKF算法需要用到大量的浮點數矩陣運算[9,10]。在FPGA上進行浮點數矩陣運算，如果采用芯片廠商提供的矩陣運算IP核，會消耗大量的硬件資源。所以,本文以芯片廠商提供的32位浮點數運算IP核為基礎，編寫了浮點數矩陣運算模塊。計算4維向量內積和矩陣相乘的模塊算法圖見圖5、圖6。其中，圖5為4維向量內積計算流程，圖6為4維矩陣乘積計算流程。

圖5 4維向量內積計算流程

圖6 4維矩陣乘積計算流程

向量內積和矩陣乘積計算的第一步都是將向量或矩陣中的元素讀取出來。對于向量內積計算，分別調用浮點數乘加模塊進行計算,而對于矩陣乘積計算，則調用向量內積計算模塊得到結果矩陣中的每一個元素。

3.2 仿真結果

在用Verilog HDL語言對上述硬件算法進行描述之后，對上文中所示純方位角度跟蹤問題，運用硬件算法進行跟蹤。選擇了Altera公司的Cyclone IV GX系列的FPGA芯片EP4CGX150DF31I7進行了綜合和時序仿真，系統時鐘設為50 MHz。

硬件仿真與軟件仿真結果對比如圖7、圖8、圖9所示。其中，圖7表示軟件仿真、硬件仿真跟蹤結果，圖8表示軟、硬件仿真得到的跟蹤結果之差，圖9表示模型概率之間的差。

圖7 軟硬件仿真跟蹤結果對比

圖8 軟硬件仿真跟蹤結果之差

圖7表明：IMMEKF算法較準確地跟蹤了目標，沒有出現失跟，同時，軟硬件算法得到的坐標估計值基本重合。這說明硬件算法的跟蹤效果與軟件算法相當。從圖8可以看到,位置估計的誤差在10-4數量級，而速度估計的誤差在10-5數量級，而圖9顯示模型概率誤差也在10-6數量級。

圖9 軟硬件仿真模型概率之差

軟硬件算法跟蹤誤差絕對值最大值如表2所示。

表2 軟硬件算法跟蹤最大誤差絕對值

這表明硬件算法能夠達到與軟件仿真相當的精度。在50 MHz脈沖激勵下，每一步運算耗費時間530 898 876 ps。而同樣的算法在Matlab環境運行時，每一步耗費時間約為0.01 s。可見，硬件算法能夠實現對本文中機動目標的跟蹤，而且相比軟件仿真具有明顯的速度優勢，這在對運算速度有較高要求的軍事場合是極為重要的。

4 結 論

本文針對純方位角度跟蹤問題，將IMMEKF算法分解成為適于硬件處理的結構，并給出了該算法在FPGA上硬件實現的方案。FPGA上的仿真實驗表明：IMMEKF硬件算法能夠對機動目標進行跟蹤，并取得與軟件仿真相當的精度，同時，運算時間大大減少。IMMEKF算法在FPGA上的硬件實現為該算法流片奠定了基礎。

參考文獻:

[1]權太范.目標跟蹤新理論與技術[M].北京：國防工業出版社,2009.

[2]洪少華,史治國,陳抗生.用于純方位跟蹤的簡化粒子濾波算法及其硬件實現[J].電子與信息學報,2009,31(1):96-100.

[3]Blom H,Bar-Shalom Y.The interacting multiple model algorithm for systems with Markovian switching coefficients[J].IEEE Transactions on Automatic Control,1988,33(3):780-783.

[4]Habtemariam B,Aravinthan A,Tharmarasa R,et al.Distributed tra-cking with a PHD filter using effcient measurement encoding[J].Journal of Advances in Information Fusion,2012,7(2):114-130.

[5]Georgescu R,Willett P,Marano S,et al.Predetection fusion in large sensor networks with unknown target locations[J].Journal of Advances in Information Fusion,2012,7(1):61-77.

[6]Yuan Ting,Bar-Shalom Y,Tian Xin. Heterogeneous track-to-track[J].Journal of Advances in Information Fusion,2011,6(2):131-149.

[7]Bolic M.Architectures for efficient implementation of particle filters[D].Stony Brook:Stony Brook University,2004.

[8]Arulampalam M,Ristic B,Gordon N,et al.Bearings-only tracking of maneuvering targets using particle filters[J].EURASIP Journal on Applied Signal Processing,2004(15):2351-2365.

[9]鄔貴明.FPGA矩陣計算并行算法與結構[D].長沙:國防科學技術大學,2011.

[10] Dou Yong,Vassiliadis S,Kuzmanov G,et al.64-bit floating-point FPGA matrix multiplication[C]∥Proceedings of the 13th ACM/SIGDA International Symposium on Field Programmable Gate Arrays(FPGA’05)，Monterey:ACM,2005:86-95.