Kalman濾波器的硬件優化設計與仿真

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

(1.江蘇大學電氣信息工程學院，江蘇鎮江212013；2.南京工業大學自動化與電氣工程學院，江蘇南京211816)

0 引 言

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

在一些實時性要求較高的場所，傳統的器件由于其固有的結構特點，較難完成如并行計算等對性能要求較高的應用。而FPGA作為一種典型的以資源換取性能的器件，隨著工藝水平的不斷提高，其規模越來越大，性價比也越來越高。其內部的大容量本地存儲器、高速I/O等極大地提高了FPGA在數字信號處理領域中的應用潛力，特別適合實時性要求較高或計算量很大的場合[5]。如Vanderlei Bonato在全自動機器人上成功利用FPGA實現了浮點擴展Kalman濾波器[6]，實時地完成了復雜的運算；喻金華基于FPGA平臺設計和實現了圖像濾波的流水線結構[7]。隨著微電子技術和EDA技術的發展，FPGA的集成度和速度可以最大限度地滿足系統的靈活性和實時性。

近年來，很多學者基于FPGA平臺進行了Kalman濾波器的設計和應用[8-11]。周倩將車輛組合導航中的Kalman濾波器在FPGA上實現[12]。但是設計重點強調運算速度和實時性，利用FPGA的并行運算特點，以面積換速度，在提高系統實時性的同時，消耗了大量硬件資源，不利于系統的升級和靈活實現。馮安詳基于FPGA設計的目標運動預測Kalman濾波器保證實時性的同時考慮了資源占用問題[13]，但其采用單精度運算，在精度要求很高的環境下稍顯不足。

本設計針對Kalman濾波器的硬件構成結構進行了優化，采用雙精度浮點運算，利用IP核和分時復用技術，在提高運算精度的同時提高了系統的實時性，又盡可能地降低硬件資源消耗，使設計實現更靈活方便。

1 Kalman濾波算法過程描述

選取文獻[14]中的GPS系統方程和觀測方程，借鑒其分散濾波算法對無耦合的X、Y方向分別建立遞推的Kalman濾波方程，以X軸向為例，狀態變量選為=[ ]，系統方程為

系統噪聲矢量為

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

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

2 基于IP核和分時復用技術的濾波器設計

2.1 采用分時復用技術的硬件總體設計

Kalman濾波算法在此實現GPS數據的濾波，并輸出濾波后結果，減小誤差，提高定位精度。該濾波系統的外部接口信號如表1所示。

根據自頂向下的設計思路，構建頂層模塊、存儲模塊以及Kalman變量運算模塊。其中Kalman運算模塊是系統設計核心，實現Kalman各變量的遞推計算，由系統一步預測估計、系統狀態估計、系統濾波增益、系統一步預測估計誤差方差和系統濾波誤差方差5部分構成。

圖1 Kalman濾波計算框架

表1 Kalman濾波模塊的接口信號

為提高資源利用率，在頂層模塊設計中，將Kalman各個運算模塊進行分時復用，系統結構如圖2所示。分時復用過程中運算數據切換時，將觀測量和預測值存入First-In-First-Out(FIFO)中，利用FIFO先進先出的特點確保每次觀測量和預測量的匹配。

圖2 Kalman濾波器的分時復用結構

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

2.2 基于IP核和分時復用的運算模塊結構設計

為確保數據處理精度，系統設計中采用雙精度浮點運算。為提高實現效率，在濾波器各個模塊中調用了浮點加減、浮點乘除的IP核[15]。

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

圖3 乘法器IP核測試

由于浮點IP核占用資源較多，所以在滿足實時性的前提下，進行了各個模塊基于IP核的分時復用結構。以系統一步預測估計誤差方差(pre_p)模塊的結構設計為例。由濾波方程得

矩陣各元素如下

根據推算，完成系統一步預測估計誤差方差模塊需要做26次加減法運算和16次乘法運算。而通過時分復用，模塊結構中只需用到一個乘法IP核和一個加減IP核，設計該模塊的時序如圖4所示，得到圖5的模塊結構。可見，分時復用優化結構大大提高了資源的利用率，降低了硬件實現時對資源的要求。完成預測估計方差的運算共需要69個時鐘周期，實時性可以得到保障。

對于系統濾波增益模塊，由濾波方程可以得到

該模塊通過時分復用，用到一個加法器和一個除法器IP核，結構如圖6所示。

圖4 系統一步預測估計誤差方差模塊時序設計

圖5 系統一步預測估計誤差方差模塊結構

圖6 系統濾波增益(pre_k)模塊結構

圖7 系統濾波增益(pre_k)模塊時序

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

通過對其他運算模塊進行分時復用的時序分析和結構設計，完成整個Kalman變量的運算模塊需要234個時鐘周期，運算滿足系統的實時性要求。

3 實驗數據的獲取及系統仿真

GPS定位原始數據為江蘇大學電氣學院4樓實驗室的靜態定位數據，所用接收機為星網宇達公司的 GPS OEM板GPS-1000，RS232串口通信，數據格式為NMEA0183。實驗獲取的原始經緯度定位數據經高斯投影后轉換為平面坐標，以此作為濾波系統的輸入數據，即濾波對象。

基于Xilinx的xc5vtx240硬件平臺，在Modelsim環境下對所設計的Kalman濾波系統及GPS定位數據進行了半物理仿真，結果如圖8所示。由于在系統設計中，對于x,y方向采取了整個模塊的分時復用，則圖中xk有效值第一個對應的是y值，第二個對應的是x值，以此類推。

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

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

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

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

圖10 雙精度浮點的資源消耗

Modelsim仿真結果表明，該濾波器的設計可以滿足系統實時性要求。以提高GPS的精度為背景，其Matlab仿真結果表明，該濾波器可以有效降低定位誤差，提高了精度，具有實際應用價值。

4 結束語

基于FPGA平臺設計的浮點Kalman濾波器，根據算法優化了底層硬件結構，充分利用了IP核、模塊的分時復用技術，以速度換面積，提高了資源利用率，且結構利于硬件實現。更重要的是，FPGA具有靈活的可移植特性和優良的抗干擾能力，因此所設計的濾波器易于修改、測試和算法升級。在保證精度的前提下很好地解決實時性問題和資源占用問題，具有更廣泛應用性和實際工程意義，其設計思想和方法具有通用性。

[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開發指南——邏輯設計篇[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的圖像濾波的流水線結構設計和實現[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]周倩.車輛組合導航中卡爾曼濾波器的設計及FPGA實現[D].北京:北京交通大學,2009.

[13]馮安詳,肖陽輝,王玉良,等.基于FPGA的目標運動預測實時kalman濾波器設計[J].電子技術應用,2008(7):49-51.

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

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