基于FPGA的Kalman濾波器的設(shè)計

摘要：針對電路設(shè)計中經(jīng)常碰到數(shù)據(jù)的噪聲干擾現(xiàn)象，提出了一種Kalman濾波的FPGA實現(xiàn)方法。該方法采用了TI公司的高精度模數(shù)轉(zhuǎn)換器ADSl25l以及Altera公司的EPlCl2，首先用卡爾曼濾波算法設(shè)計了一個濾波器，然后將該濾波器分解成簡單的加、減、乘、除運算。通過基于FPGA平臺的硬件與軟件的合理設(shè)計，成功地實現(xiàn)了數(shù)據(jù)噪聲的濾除設(shè)計，并通過實踐仿真計算，驗證了所實現(xiàn)濾波的有效性。
關(guān)鍵詞：卡爾曼；FPGA；最小方差估計

    卡爾曼濾波是一個“Optimal Recursive Data Processing Algorithm(最優(yōu)化自回歸數(shù)據(jù)處理算法)”，對于解決很大部分的問題，是最優(yōu)化的，效率最高甚至是最有用的。傳統(tǒng)的卡爾曼濾波是在DSP上實現(xiàn)的。但是DSP成本相對較高，而且指令是串行執(zhí)行的，不能滿足有些要求較高的場合。而FPGA由于其硬件結(jié)構(gòu)決定了它的并行處理方式，無論在速度還是實時性都更勝一籌。文中以基于FPGA器件和A／D轉(zhuǎn)換器的數(shù)據(jù)采集系統(tǒng)為硬件平臺，進行了卡爾曼濾波算法設(shè)計，詳述了基于FPGA的卡爾曼濾波器的設(shè)計實現(xiàn)。

1 卡爾曼濾波算法
    工程中，為了了解工程對象(濾波中稱為系統(tǒng))的各個物理量(濾波中稱為狀態(tài))的確切數(shù)值，或為了達到對工程對象進行控制的目的，必須利用測量手段對系統(tǒng)的各個狀態(tài)進行測量。但是，量測值可能僅是系統(tǒng)的部分狀態(tài)或是部分狀態(tài)的線性組合，且量測值中有隨機誤差(常稱為量測噪聲)。最優(yōu)估計就是針對上述問題的一種解決方法。它能將僅與部分狀態(tài)有關(guān)的測量進行處理，得出從統(tǒng)計意義上講誤差最小的更多狀態(tài)的估值。誤差最小的標準常稱為估計準則，根據(jù)不同的估計準則和估計計算方法，有各種不同的最優(yōu)估計，卡爾曼濾波是一種遞推線性最小方差估計的最優(yōu)估計。
    系統(tǒng)的狀態(tài)方程可設(shè)定為

式（3）為系統(tǒng)噪聲。設(shè)設(shè)備的量測噪聲為Vk，系統(tǒng)得量測方程為

式中，是利用當前狀態(tài)預(yù)測的結(jié)果，是當前狀態(tài)最優(yōu)的結(jié)果，Pk+1／k是*對應(yīng)的covariance，Rk是對應(yīng)的covanance，表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的covariance。式(1)，式(2)就是卡爾曼濾波器5個公式中的前兩個，也就是對系統(tǒng)的預(yù)測?？梢缘玫綄頎顟B(tài)k+l的最優(yōu)化估算值。
    式(9)中I為單位矩陣，對于單模型單測量，I=1。當系統(tǒng)進入k+2狀態(tài)時，Pk+1就是式(2)的Pk。這樣，算法就可以自回歸的運算下去。
    R為測量數(shù)據(jù)的方差；Q為系統(tǒng)過程噪聲的方差，取值為，根據(jù)經(jīng)驗可設(shè)置為測量數(shù)據(jù)方差的1／10；P的初值可選為單位陣，I為單位陣，X的處置可選為(0，0)T。算法示意圖，如圖1所示。

    將算法矩陣展開并寫成乘加形式，令

    這些公式包含了一些加、減、乘、除的數(shù)學運算，用FPGA實現(xiàn)卡爾曼濾波就是用FPGA實現(xiàn)這些運算。

2 硬件設(shè)計
2．1 A／D選擇
    ADSl251是TI公司生產(chǎn)的一種高精度、寬動態(tài)范圍的24位△～∑乏結(jié)構(gòu)的模數(shù)轉(zhuǎn)換器。采樣保持器的輸入形式采用差分模擬輸入方式，設(shè)計了一個簡單的電平轉(zhuǎn)換電路使得端和端之間的電壓波動范圍為士2．5 V。ADSl251的時鐘頻率由8 MHz的外部晶振提供，系統(tǒng)時鐘由FPGA給出。ADSl251的內(nèi)部結(jié)構(gòu)圖，如圖2所示。

    輸出有3種模式：同步、異步及掉電模式，異步模式與Kalman濾波器數(shù)據(jù)輸入接口連接比較方便。其時序如圖3所示。

2．2 FPGA設(shè)計
    FPGA的特點在于靈活，通用。如果資源夠用，可以實現(xiàn)各種數(shù)字組合，包括CPU。本系統(tǒng)采用的：FPGA是Ahera公司低價位的EP1C12，該芯片內(nèi)含有兩個PLLs，12060LEs及239616 Total RAMbits。在本系統(tǒng)的設(shè)計中，F(xiàn)PGA內(nèi)主要實現(xiàn)以下幾個功能：Kalman狀態(tài)控制信號、Kalm-an濾波運算、A／D控制模塊、時鐘及波特率設(shè)置等。卡爾曼濾波器的系統(tǒng)結(jié)構(gòu)圖，如圖4所示。

    通過A／D轉(zhuǎn)換芯片采集轉(zhuǎn)換數(shù)據(jù)后，進入FPGA的A／D控制模塊，在Kalman狀態(tài)控制信號的作用下，進行濾波運算，然后通過控制接口將濾波數(shù)據(jù)輸出。由此可見，卡爾曼濾波的實現(xiàn)包括兩部分：Kalman狀態(tài)控制信號和Kalman濾波運算。

3 Kalmaft濾波器的設(shè)計
    在FPGA中實現(xiàn)Kalman濾波器，重點在于平衡資源利用率和處理速度、數(shù)據(jù)運算精度之間的矛盾，難點在于浮點加、減、乘、除的硬邏輯及卡爾曼濾波流程控制的實現(xiàn)。一般地，實際應(yīng)用中，數(shù)字采樣重復頻率是待采樣信號最高頻率成分的5倍以上能保證較好恢復原信號，本設(shè)計中出于后期升級的考慮，將采樣率設(shè)置在800次／s，是輸出信號帶寬的20倍。
    通過第2節(jié)中的分解，Kalman濾波器由16次加法、20次乘法、1次除法以及必要的輸入、輸出及循環(huán)控制組成，共45步。狀態(tài)控制器(Kal-man狀態(tài)控制信號)實際是一個有限狀態(tài)機，分別控制濾波器中的45步運算，決定其執(zhí)行順序，每一步利用計數(shù)器為其提供必要的時鐘周期。
    Kalman濾波運算主要是浮點計算，浮點表示常用的標準是IEEE 754，IEEE二進制浮點數(shù)算術(shù)標準(IEEE754)是最廣泛使用的浮點數(shù)運算標準，為許多CPU與浮點運算器所采用。IEEE 754規(guī)定了4種表示浮點數(shù)值的方式：單精確度(32位元)、雙精確度(64位元)、延伸單精確度(43位元以上，較少使用)與延伸雙精確度(79位元以上，通常以80位元實做)。
    二進制浮點數(shù)是以符號數(shù)值表示法格式儲存，將最高效位元指定為符號位元(Sign Bit)；“指數(shù)部分”，即次高效的e位元，為浮點數(shù)中經(jīng)指數(shù)偏差(Exponent Bias)處理過后的指數(shù)；“小數(shù)部分”，即剩下的f位元，為有效位數(shù)(Significand)減掉有效位數(shù)本身的最高效位元，如圖5所示。

    文中采用的單精度二進制小數(shù)，使用32個位元存儲，如圖6所示。

其中，31位是符號位；0表示正；1表示負；30～23位為階數(shù)；22～0表示數(shù)值的有效位。偏正值為+127。其表示的具體值可用式(27)表示
   
    指數(shù)部分采用一個無符號的正數(shù)值存儲。單精度的指數(shù)部分是-126～+127加上127，指數(shù)值的大小為1～254(0和255是特殊值)。浮點小數(shù)計算時，指數(shù)值減去偏正值將是實際的指數(shù)大小。
    由于資源的限制，濾波中的運算過程只能以有限的精度進行，但過低的精度容易引起濾波發(fā)散，同時為了兼顧A／D高達24 bit的分辨率精度，在IEEE754單精度浮點數(shù)格式的基礎(chǔ)上按照浮點表示的原則做了改造，將指數(shù)部分減少到6位，尾數(shù)增加到25位，25位尾數(shù)可以完全表示A／D輸出的24位采樣值，并削弱舍入誤差的影響，其格式如圖7所示。

    本系統(tǒng)對處理速度的要求并不高，運算部件的設(shè)計并不考慮速度優(yōu)化。
    加法器按以下通用的規(guī)則設(shè)計：
    (1)階碼相減：兩個操作數(shù)階碼相減，求得階差d=|Ea-Eb|；
    (2)尾數(shù)對齊：階碼小的操作數(shù)的尾數(shù)右移d位；
    (3)尾數(shù)相加：完成移位處理后的尾數(shù)相加運算；
    (4)轉(zhuǎn)換：尾數(shù)相加的結(jié)果為負數(shù)時，轉(zhuǎn)換為符號尾數(shù)的表示方式；
    (5)前導0／1的判定：判定由于減法結(jié)果產(chǎn)生的左移位數(shù)，對于加法判定是否右移一位或不移，對前導0／1判定的結(jié)果進行編碼以驅(qū)動規(guī)格化移位；
    (6)規(guī)格化：規(guī)格化有效位并且根據(jù)移位的方向和位數(shù)修改最終的階碼Ef；
    (7)舍入：根據(jù)IEEE標準判定最終結(jié)果的舍入，如果需要入，則最末有效位加1；如舍入引起溢出，則需要有效位右移1位，同時階碼Ef加1。
    乘法器的設(shè)計相對簡單，與加法器一樣采用簡單的設(shè)計規(guī)則：
    (1)指數(shù)相加：完成兩個操作數(shù)的指數(shù)相加運算；
    (2)尾數(shù)調(diào)整：將尾數(shù)f調(diào)整為l／f的補碼格式；
    (3)尾數(shù)相乘：完成兩個操作數(shù)的尾數(shù)相乘運算；
    (4)規(guī)格化：根據(jù)尾數(shù)運算結(jié)果調(diào)整指數(shù)位，對尾數(shù)進行舍入截位操作，規(guī)格化輸出結(jié)果。
    采用Altera提供的IP核，除法器能完成IEEE754單精度除法運算。數(shù)據(jù)進入該除法器前需要將自定義的浮點格式的階碼擴展為8位加上95，尾數(shù)右移兩位，轉(zhuǎn)換為標準單精度浮點，計算結(jié)果再由標準單精度浮點的按相反的方法轉(zhuǎn)換為自定義浮點。
    文中硬件編程語言是用Verilog實現(xiàn)的。

4 實測結(jié)果分析與結(jié)論
    從系統(tǒng)輸出數(shù)據(jù)中隨機抽取一段數(shù)據(jù)用Matlab分析，其結(jié)果如圖8所示。實線C表示的是濾波前的噪聲：虛線B表示的是濾波后的噪聲。很容易看出經(jīng)過濾波后噪聲降低了很多。仿真與實測對比分析如表1所示。

5 結(jié)束語
    卡爾曼濾波器在很多領(lǐng)域具有重要的作用，可以實現(xiàn)數(shù)字信號處理功能，并能滿足實時性的要求。用FPGA實現(xiàn)Kalman濾波過程，實現(xiàn)簡便、成本較低、濾波效果好，被廣泛應(yīng)用于圖像視頻、陀螺慣導去噪、電子羅盤、雷達接收機目標跟蹤處理、目標預(yù)測等圖像、數(shù)據(jù)采集與處理領(lǐng)域。