FPGA的Kalman濾波器的設(shè)計(jì)

FPGA的Kalman濾波器的設(shè)計(jì)

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

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

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

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

式中，是利用當(dāng)前狀態(tài)預(yù)測(cè)的結(jié)果，是當(dāng)前狀態(tài)最優(yōu)的結(jié)果，Pk+1／k是*對(duì)應(yīng)的covariance，Rk是對(duì)應(yīng)的covanance，表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程的covariance。式(1)，式(2)就是卡爾曼濾波器5個(gè)公式中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。可以得到將來(lái)狀態(tài)k+l的最優(yōu)化估算值。
??? 式(9)中I為單位矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+2狀態(tài)時(shí)，Pk+1就是式(2)的Pk。這樣，算法就可以自回歸的運(yùn)算下去。

??? R為測(cè)量數(shù)據(jù)的方差；Q為系統(tǒng)過(guò)程噪聲的方差，取值為，根據(jù)經(jīng)驗(yàn)可設(shè)置為測(cè)量數(shù)據(jù)方差的1／10；P的初值可選為單位陣，I為單位陣，X的處置可選為(0，0)T。算法示意圖，如圖1所示。

??? 將算法矩陣展開(kāi)并寫成乘加形式，令

?

?

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

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

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

?

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

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

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

??? 文中采用的單精度二進(jìn)制小數(shù)，使用32個(gè)位元存儲(chǔ)，如圖6所示。

其中，31位是符號(hào)位；0表示正；1表示負(fù)；30～23位為階數(shù)；22～0表示數(shù)值的有效位。偏正值為+127。其表示的具體值可用式(27)表示
???
??? 指數(shù)部分采用一個(gè)無(wú)符號(hào)的正數(shù)值存儲(chǔ)。單精度的指數(shù)部分是-126～+127加上127，指數(shù)值的大小為1～254(0和255是特殊值)。浮點(diǎn)小數(shù)計(jì)算時(shí)，指數(shù)值減去偏正值將是實(shí)際的指數(shù)大小。

??? 由于資源的限制，濾波中的運(yùn)算過(guò)程只能以有限的精度進(jìn)行，但過(guò)低的精度容易引起濾波發(fā)散，同時(shí)為了兼顧A／D高達(dá)24 bit的分辨率精度，在IEEE754單精度浮點(diǎn)數(shù)格式的基礎(chǔ)上按照浮點(diǎn)表示的原則做了改造，將指數(shù)部分減少到6位，尾數(shù)增加到25位，25位尾數(shù)可以完全表示A／D輸出的24位采樣值，并削弱舍入誤差的影響，其格式如圖7所示。

?

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

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

?

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