Kalman濾波器的硬件優(yōu)化設(shè)計(jì)與仿真

白 雪， 王德明， 屠君君， 趙不賄

(1.江蘇大學(xué)電氣信息工程學(xué)院，江蘇鎮(zhèn)江212013；2.南京工業(yè)大學(xué)自動(dòng)化與電氣工程學(xué)院，江蘇南京211816)

0 引 言

濾波是從帶有干擾的信號(hào)中得到有用信號(hào)的準(zhǔn)確估計(jì)值，Kalman濾波是一種時(shí)域內(nèi)的濾波方法，它把狀態(tài)空間的概念引入到隨機(jī)估計(jì)理論中，估計(jì)過程中利用狀態(tài)方程、觀測(cè)方程和系統(tǒng)噪聲及觀測(cè)噪聲的統(tǒng)計(jì)特性形成的一種濾波算法[1]。根據(jù)實(shí)時(shí)遞推的思想，Kalman濾波過程是一個(gè)不斷“預(yù)測(cè)-修正”的過程[2-4]，求解中無需存儲(chǔ)大量數(shù)據(jù)，一旦觀測(cè)到新的數(shù)據(jù)就可以計(jì)算出新的濾波值，因此方便硬件實(shí)現(xiàn)，在實(shí)際工程中得到了廣泛應(yīng)用。因此對(duì)Kalman濾波算法的實(shí)現(xiàn)是很多工程應(yīng)用中首要解決的問題。

在一些實(shí)時(shí)性要求較高的場(chǎng)所，傳統(tǒng)的器件由于其固有的結(jié)構(gòu)特點(diǎn)，較難完成如并行計(jì)算等對(duì)性能要求較高的應(yīng)用。而FPGA作為一種典型的以資源換取性能的器件，隨著工藝水平的不斷提高，其規(guī)模越來越大，性價(jià)比也越來越高。其內(nèi)部的大容量本地存儲(chǔ)器、高速I/O等極大地提高了FPGA在數(shù)字信號(hào)處理領(lǐng)域中的應(yīng)用潛力，特別適合實(shí)時(shí)性要求較高或計(jì)算量很大的場(chǎng)合[5]。如Vanderlei Bonato在全自動(dòng)機(jī)器人上成功利用FPGA實(shí)現(xiàn)了浮點(diǎn)擴(kuò)展Kalman濾波器[6]，實(shí)時(shí)地完成了復(fù)雜的運(yùn)算；喻金華基于FPGA平臺(tái)設(shè)計(jì)和實(shí)現(xiàn)了圖像濾波的流水線結(jié)構(gòu)[7]。隨著微電子技術(shù)和EDA技術(shù)的發(fā)展，F(xiàn)PGA的集成度和速度可以最大限度地滿足系統(tǒng)的靈活性和實(shí)時(shí)性。

近年來，很多學(xué)者基于FPGA平臺(tái)進(jìn)行了Kalman濾波器的設(shè)計(jì)和應(yīng)用[8-11]。周倩將車輛組合導(dǎo)航中的Kalman濾波器在FPGA上實(shí)現(xiàn)[12]。但是設(shè)計(jì)重點(diǎn)強(qiáng)調(diào)運(yùn)算速度和實(shí)時(shí)性，利用FPGA的并行運(yùn)算特點(diǎn)，以面積換速度，在提高系統(tǒng)實(shí)時(shí)性的同時(shí)，消耗了大量硬件資源，不利于系統(tǒng)的升級(jí)和靈活實(shí)現(xiàn)。馮安詳基于FPGA設(shè)計(jì)的目標(biāo)運(yùn)動(dòng)預(yù)測(cè)Kalman濾波器保證實(shí)時(shí)性的同時(shí)考慮了資源占用問題[13]，但其采用單精度運(yùn)算，在精度要求很高的環(huán)境下稍顯不足。

本設(shè)計(jì)針對(duì)Kalman濾波器的硬件構(gòu)成結(jié)構(gòu)進(jìn)行了優(yōu)化，采用雙精度浮點(diǎn)運(yùn)算，利用IP核和分時(shí)復(fù)用技術(shù)，在提高運(yùn)算精度的同時(shí)提高了系統(tǒng)的實(shí)時(shí)性，又盡可能地降低硬件資源消耗，使設(shè)計(jì)實(shí)現(xiàn)更靈活方便。

1 Kalman濾波算法過程描述

選取文獻(xiàn)[14]中的GPS系統(tǒng)方程和觀測(cè)方程，借鑒其分散濾波算法對(duì)無耦合的X、Y方向分別建立遞推的Kalman濾波方程，以X軸向?yàn)槔?，狀態(tài)變量選為=[ ]，系統(tǒng)方程為

系統(tǒng)噪聲矢量為

式中：——加速度相關(guān)時(shí)間常數(shù)， —— (0,2)的高斯白噪聲。系統(tǒng)觀測(cè)方程為，觀測(cè)矩陣為=[1 0 0 1]，觀測(cè)噪聲矢量為Vx=[ ]， 為(0,)的高斯白噪聲。根據(jù)上述系統(tǒng)方程、觀測(cè)方程和Kalman濾波算法，得到X軸向上Kalman濾波方程

以上公式描述的Kalman濾波計(jì)算過程如圖1所示。

2 基于IP核和分時(shí)復(fù)用技術(shù)的濾波器設(shè)計(jì)

2.1 采用分時(shí)復(fù)用技術(shù)的硬件總體設(shè)計(jì)

Kalman濾波算法在此實(shí)現(xiàn)GPS數(shù)據(jù)的濾波，并輸出濾波后結(jié)果，減小誤差，提高定位精度。該濾波系統(tǒng)的外部接口信號(hào)如表1所示。

根據(jù)自頂向下的設(shè)計(jì)思路，構(gòu)建頂層模塊、存儲(chǔ)模塊以及Kalman變量運(yùn)算模塊。其中Kalman運(yùn)算模塊是系統(tǒng)設(shè)計(jì)核心，實(shí)現(xiàn)Kalman各變量的遞推計(jì)算，由系統(tǒng)一步預(yù)測(cè)估計(jì)、系統(tǒng)狀態(tài)估計(jì)、系統(tǒng)濾波增益、系統(tǒng)一步預(yù)測(cè)估計(jì)誤差方差和系統(tǒng)濾波誤差方差5部分構(gòu)成。

圖1 Kalman濾波計(jì)算框架

表1 Kalman濾波模塊的接口信號(hào)

為提高資源利用率，在頂層模塊設(shè)計(jì)中，將Kalman各個(gè)運(yùn)算模塊進(jìn)行分時(shí)復(fù)用，系統(tǒng)結(jié)構(gòu)如圖2所示。分時(shí)復(fù)用過程中運(yùn)算數(shù)據(jù)切換時(shí)，將觀測(cè)量和預(yù)測(cè)值存入First-In-First-Out(FIFO)中，利用FIFO先進(jìn)先出的特點(diǎn)確保每次觀測(cè)量和預(yù)測(cè)量的匹配。

圖2 Kalman濾波器的分時(shí)復(fù)用結(jié)構(gòu)

系統(tǒng)初始化時(shí)，由GPS提取出來的數(shù)據(jù)信息lx不寫入FIFO(lx)中，而是將作為X0的初始值。分別用pre_pk，pre_k和pre_xk表示預(yù)測(cè)量P、K和XK，將預(yù)測(cè)得到的結(jié)果分別存入各自的FIFO中。首先存入的是Y坐標(biāo)預(yù)測(cè)的值，接著是X坐標(biāo)預(yù)測(cè)的值。然后啟動(dòng)整個(gè)系統(tǒng)的工作，等觀測(cè)值的輸入之后，首先從各自的FIFO中讀取出預(yù)測(cè)值進(jìn)行PK和XK值的計(jì)算，再根據(jù)這兩個(gè)值預(yù)測(cè)P、K和XK的值，依次類推。Y和X的預(yù)測(cè)值依次存入FIFO中，與輸入的觀測(cè)值一一對(duì)應(yīng)。

2.2 基于IP核和分時(shí)復(fù)用的運(yùn)算模塊結(jié)構(gòu)設(shè)計(jì)

為確保數(shù)據(jù)處理精度，系統(tǒng)設(shè)計(jì)中采用雙精度浮點(diǎn)運(yùn)算。為提高實(shí)現(xiàn)效率，在濾波器各個(gè)模塊中調(diào)用了浮點(diǎn)加減、浮點(diǎn)乘除的IP核[15]。

下面對(duì)主要的IP核加/減法器IP和乘法器IP進(jìn)行了功能測(cè)試。浮點(diǎn)的加法是先將一個(gè)數(shù)的尾數(shù)右移或者左移幾位，當(dāng)兩個(gè)數(shù)的指數(shù)相同時(shí)將尾數(shù)相加得到。通過operation端口來實(shí)現(xiàn)加法或者減法的切換。乘法器的IP核有兩種類型：用專用乘法器實(shí)現(xiàn)和用 Slice搭建乘法器。由于一個(gè)浮點(diǎn)乘法器的實(shí)現(xiàn)需要16個(gè)專用乘法器，整個(gè)設(shè)計(jì)中共使用到6個(gè)浮點(diǎn)乘法器，考慮到資源和模塊的通用性，設(shè)計(jì)中選擇通過slice實(shí)現(xiàn)乘法器的搭建。圖3是仿真測(cè)試結(jié)果，結(jié)果表明該乘法器IP正確的實(shí)現(xiàn)了乘法運(yùn)算功能，其中輸出滯后輸入9個(gè)時(shí)鐘周期。

圖3 乘法器IP核測(cè)試

由于浮點(diǎn)IP核占用資源較多，所以在滿足實(shí)時(shí)性的前提下，進(jìn)行了各個(gè)模塊基于IP核的分時(shí)復(fù)用結(jié)構(gòu)。以系統(tǒng)一步預(yù)測(cè)估計(jì)誤差方差(pre_p)模塊的結(jié)構(gòu)設(shè)計(jì)為例。由濾波方程得

矩陣各元素如下

根據(jù)推算，完成系統(tǒng)一步預(yù)測(cè)估計(jì)誤差方差模塊需要做26次加減法運(yùn)算和16次乘法運(yùn)算。而通過時(shí)分復(fù)用，模塊結(jié)構(gòu)中只需用到一個(gè)乘法IP核和一個(gè)加減IP核，設(shè)計(jì)該模塊的時(shí)序如圖4所示，得到圖5的模塊結(jié)構(gòu)?？梢?，分時(shí)復(fù)用優(yōu)化結(jié)構(gòu)大大提高了資源的利用率，降低了硬件實(shí)現(xiàn)時(shí)對(duì)資源的要求。完成預(yù)測(cè)估計(jì)方差的運(yùn)算共需要69個(gè)時(shí)鐘周期，實(shí)時(shí)性可以得到保障。

對(duì)于系統(tǒng)濾波增益模塊，由濾波方程可以得到

該模塊通過時(shí)分復(fù)用，用到一個(gè)加法器和一個(gè)除法器IP核，結(jié)構(gòu)如圖6所示。

圖4 系統(tǒng)一步預(yù)測(cè)估計(jì)誤差方差模塊時(shí)序設(shè)計(jì)

圖5 系統(tǒng)一步預(yù)測(cè)估計(jì)誤差方差模塊結(jié)構(gòu)

圖6 系統(tǒng)濾波增益(pre_k)模塊結(jié)構(gòu)

圖7 系統(tǒng)濾波增益(pre_k)模塊時(shí)序

整個(gè)模塊的時(shí)序如圖7所示。當(dāng)檢測(cè)到en為1時(shí)，即外部輸入P預(yù)測(cè)值已經(jīng)運(yùn)算完畢，則開始工作。工作狀態(tài)用work信號(hào)表示，若work為1時(shí)表示模塊處于工作狀態(tài)，否則為閑置狀態(tài)。在復(fù)用 IP核的過程中通過用中間變量暫存需要的結(jié)果。add_a、add_b、add_result、div_a、div_b 以及 div_result分別是加法器和除法器的輸入輸出端口。計(jì)算濾波的增益模塊共需要109個(gè)時(shí)鐘周期。

通過對(duì)其他運(yùn)算模塊進(jìn)行分時(shí)復(fù)用的時(shí)序分析和結(jié)構(gòu)設(shè)計(jì)，完成整個(gè)Kalman變量的運(yùn)算模塊需要234個(gè)時(shí)鐘周期，運(yùn)算滿足系統(tǒng)的實(shí)時(shí)性要求。

3 實(shí)驗(yàn)數(shù)據(jù)的獲取及系統(tǒng)仿真

GPS定位原始數(shù)據(jù)為江蘇大學(xué)電氣學(xué)院4樓實(shí)驗(yàn)室的靜態(tài)定位數(shù)據(jù)，所用接收機(jī)為星網(wǎng)宇達(dá)公司的 GPS OEM板GPS-1000，RS232串口通信，數(shù)據(jù)格式為NMEA0183。實(shí)驗(yàn)獲取的原始經(jīng)緯度定位數(shù)據(jù)經(jīng)高斯投影后轉(zhuǎn)換為平面坐標(biāo)，以此作為濾波系統(tǒng)的輸入數(shù)據(jù)，即濾波對(duì)象。

基于Xilinx的xc5vtx240硬件平臺(tái)，在Modelsim環(huán)境下對(duì)所設(shè)計(jì)的Kalman濾波系統(tǒng)及GPS定位數(shù)據(jù)進(jìn)行了半物理仿真，結(jié)果如圖8所示。由于在系統(tǒng)設(shè)計(jì)中，對(duì)于x,y方向采取了整個(gè)模塊的分時(shí)復(fù)用，則圖中xk有效值第一個(gè)對(duì)應(yīng)的是y值，第二個(gè)對(duì)應(yīng)的是x值，以此類推。

圖8 Kalman濾波模塊的Modelsim仿真波形

由于圖8中數(shù)據(jù)為雙精度浮點(diǎn)表示，不便于觀察，為了更直觀分析濾波效果，將濾波后的 Modelsim仿真數(shù)據(jù)導(dǎo)入到Matlab環(huán)境，與濾波前的原始數(shù)據(jù)進(jìn)行對(duì)比，如圖9所示。兩種仿真結(jié)果表明：設(shè)計(jì)的浮點(diǎn)Kalman濾波器滿足系統(tǒng)實(shí)時(shí)性要求，若時(shí)鐘頻率為50MHz，則完成一個(gè)周期運(yùn)算只要4.68 s，而同樣的算法用DSP或單片機(jī)通過軟件實(shí)現(xiàn)一個(gè)運(yùn)算周期則需要幾百 s甚至更長(zhǎng)時(shí)間；設(shè)計(jì)的Kalman濾波器可以減小定位誤差，提高定位精度，達(dá)到系統(tǒng)設(shè)計(jì)的目的。

圖9 Kalman濾波前后GPS定位誤差比較

本設(shè)計(jì)使用雙精度浮點(diǎn)運(yùn)算，能夠滿足在數(shù)據(jù)精度方面要求高的場(chǎng)合。在Xilinx的xc5vtx240上編譯綜合后，顯示其資源占用情況如圖10所示。若應(yīng)用在精度要求相對(duì)較低的環(huán)境中，將數(shù)據(jù)位寬降低，資源消耗將會(huì)大量減少。

圖10 雙精度浮點(diǎn)的資源消耗

Modelsim仿真結(jié)果表明，該濾波器的設(shè)計(jì)可以滿足系統(tǒng)實(shí)時(shí)性要求。以提高GPS的精度為背景，其Matlab仿真結(jié)果表明，該濾波器可以有效降低定位誤差，提高了精度，具有實(shí)際應(yīng)用價(jià)值。

4 結(jié)束語

基于FPGA平臺(tái)設(shè)計(jì)的浮點(diǎn)Kalman濾波器，根據(jù)算法優(yōu)化了底層硬件結(jié)構(gòu)，充分利用了IP核、模塊的分時(shí)復(fù)用技術(shù)，以速度換面積，提高了資源利用率，且結(jié)構(gòu)利于硬件實(shí)現(xiàn)。更重要的是，F(xiàn)PGA具有靈活的可移植特性和優(yōu)良的抗干擾能力，因此所設(shè)計(jì)的濾波器易于修改、測(cè)試和算法升級(jí)。在保證精度的前提下很好地解決實(shí)時(shí)性問題和資源占用問題，具有更廣泛應(yīng)用性和實(shí)際工程意義，其設(shè)計(jì)思想和方法具有通用性。

[1]SimonD.Kalmanfilteringwithstateconstraints:asurveyoflinear and nonlinear algorithms[J].Control Theory&Applications,2010,4(8):1303-1318.

[2]Welch G,Bishop G.An introduction to the Kalman Filter[Z].University of North Carolina,2004.

[3]David M Bevly,Parkinson Bradford.Cascaded Kalman filters for accurate estimation of multiple biases,dead-reckoning navigation,and full state feedback control of ground vehicles[J].IEEE Transactions on Control Systems Technology,2007,15(2):199-208.

[4]Fang Jiancheng,Gong Xiaolin.Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation[J].IEEE Transactions on Instrumentation and Measurement,2010,59(4):909-915.

[5]田耘,徐文波,胡彬,等.Xilinx ISE Design Suite 10.x FPGA開發(fā)指南——邏輯設(shè)計(jì)篇[M].北京:人民郵電出版社,2008.

[6]Bonato V,Marques E,Constantinides G A.A floating-point extended kalman filter implementation for autonomous mobile robots[C].International Conference on Field Programmable Logic and Applications,2007:576-579.

[7]喻金華,肖鐵軍.基于FPGA的圖像濾波的流水線結(jié)構(gòu)設(shè)計(jì)和實(shí)現(xiàn)[J].計(jì)算機(jī)工程與設(shè)計(jì),2009,30(18):4192-4194.

[8]Bonato V,Peron R,Wolf D F,et al.An FPGA implementation for a Kalman filter with application to mobile robotics[C].International Symposium on Industrial Embedded Systems,2007:148-155.

[9]Idkhajine L,Monmasson E.Optimized FPGA-based extended Kalman filter application to an AC drive sensorless speed controller[C].International Symposium on Power Electronics Electrical Drives Automation and Motion,2010:1012-1017.

[10]Idkhajine L,Monmasson E,Maalouf A.Fully FPGA-based sensorless control for AC drive using an extended Kalman filter[C].35th Annual Conference of IEEE Industrial Electronics,2009:2925-2930.

[11]Yang Liu,Bouganis C S,Cheung P Y K.Efficient mapping of a Kalman filter into an FPGA using Taylor expansion[C].International Conference on Field Programmable Logic and Applications,2007:345-350.

[12]周倩.車輛組合導(dǎo)航中卡爾曼濾波器的設(shè)計(jì)及FPGA實(shí)現(xiàn)[D].北京:北京交通大學(xué),2009.

[13]馮安詳,肖陽輝,王玉良,等.基于FPGA的目標(biāo)運(yùn)動(dòng)預(yù)測(cè)實(shí)時(shí)kalman濾波器設(shè)計(jì)[J].電子技術(shù)應(yīng)用,2008(7):49-51.

[14]李康吉,劉國(guó)海.一種提高GPS定位精度的組合定位方法[J].東南大學(xué)學(xué)報(bào),2004,34(11):88-91.

[15]Xilinx Corporation.Spartan-3E Starter Kit User Guide[Z].2006.