基于FPGA的Kalman濾波器實現(xiàn)研究

摘要：卡爾曼(Kalman)濾波計算精度和速度是工程應(yīng)用中是否成功的決定性條件，為進一步提高Kalman濾波算法在更復(fù)雜的環(huán)境下使用的性能，并能夠同時滿足實時性和精度的要求，采用現(xiàn)場可編程邏輯陣列(FPGA)技術(shù)，設(shè)計了Kaiman濾波算法在FPGA上的實現(xiàn)方案，選擇了一種可以同時滿足精度和實時性的方案進行實現(xiàn)，對算法中的矩陣相乘、狀態(tài)機的應(yīng)用以及資源分時復(fù)用等關(guān)鍵技術(shù)進行了設(shè)計。通過與Matlab及DSP的計算結(jié)果相對比，驗證了在FPGA內(nèi)實現(xiàn)Kalman濾波器的優(yōu)勢。
關(guān)鍵詞：FPGA；Kalman濾波器；IP核；實時性

    Kalman濾波理論在20世紀60年代一經(jīng)提出，便得到了軍事、控制、通信等領(lǐng)域的極廣泛的應(yīng)用。它可以實現(xiàn)隨機干擾下的線性動態(tài)系統(tǒng)的最優(yōu)估計，目前Kalman濾波器的實現(xiàn)方式主要有兩種，一是在PC機上實現(xiàn)，可以同時滿足計算精度和實時性的要求，但是PC機體積大，質(zhì)量重，成本高；二是通過DSP等芯片來實現(xiàn)，用這種方式實現(xiàn)的Kalman濾波器雖然體積小，質(zhì)量輕，但是因其指令順序執(zhí)行的CPU架構(gòu)，在系統(tǒng)復(fù)雜時無法滿足系統(tǒng)的實時性要求。隨著控制系統(tǒng)的復(fù)雜性的提高，系統(tǒng)的階次變大，如組合導航系統(tǒng)的濾波，其濾波的階次一般都要18階，如果對系統(tǒng)進一步細化建模或增加其復(fù)雜性，其濾波階次可以達到幾十階。因此，Kalman濾波器在工程應(yīng)用中的實現(xiàn)遇到了系統(tǒng)體積、重量、成本和系統(tǒng)精度、速度等性能不能兼顧的問題。隨著現(xiàn)代電子技術(shù)的發(fā)展，F(xiàn)PGA具有系統(tǒng)結(jié)構(gòu)和邏輯單元靈活、集成度高以及適用范圍寬等特點，可以很好地解決這個難題。因為FPGA采用的是硬件并行算法，能很好的解決速度和實時性的問題，并且其具有靈活的可配置特性和優(yōu)良的抗干擾能力，使得FPGA構(gòu)成的數(shù)字信號處理系統(tǒng)非常易于修改、測試及硬件升級。隨著FPGA技術(shù)的不斷成熟，其內(nèi)嵌資源不斷豐富，硬核乘法器和塊RAM的數(shù)目不斷增長，使得FPGA實現(xiàn)復(fù)雜的數(shù)字信號處理算法變得更為簡單和快速。因此，本文對FPGA技術(shù)和Kalman濾波算法進行結(jié)合研究，探索Kalman濾波算法在FPGA中的實現(xiàn)方式并進行性能驗證，以對基于FPGA的Kalman濾波算法的工程實現(xiàn)提供參考。

1 Kalman濾波算法理論
    Kalman濾波是在時域內(nèi)以信號的一、二階統(tǒng)計特性已知為前提、以均方誤差極小為判據(jù)，能自動跟蹤信號統(tǒng)計性質(zhì)的非平穩(wěn)變化，具有遞歸性質(zhì)的一種算法。它處理的對象是隨機系統(tǒng)，并能正確估計出有用信號。設(shè)離散系統(tǒng)差分方程如下：
   
    則Kalman濾波方程組如下：
    狀態(tài)一步預(yù)測方程：

    從式(1)～(6)可知，若利用傳統(tǒng)的處理器實現(xiàn)Kalman濾波算法，由于其指令執(zhí)行的順序性，至少需要分為5步來實現(xiàn)，其中每一步還都需要進行至少1次的加法和乘法等運算，每次運算都要順序執(zhí)行，其執(zhí)行速度和效率很低；如果利用FPGA來進行Kalman濾波，根據(jù)其各步的邏輯關(guān)系，可以分為3步來實現(xiàn)，即第一步計算狀態(tài)一步預(yù)測值和一步預(yù)測均方誤差Pk+1／k，第二步計算濾波增益Kk+1，第三步計算狀態(tài)最優(yōu)估值和估計均方誤差Pk+1／k+1。由此可知，利用FPGA技術(shù)可以實現(xiàn)Kalman濾波的并行計算，壓縮計算時間，提高解算速度。因此，對FPGA的Kalman濾波進行研究開發(fā)，可實現(xiàn)基于FPGA的快速Kalman濾波解算，滿足在對實時性要求更高的環(huán)境中使用。
[!--empirenews.page--]
2 在FPGA中實現(xiàn)Kalman濾波算法研究
    由于FPGA實現(xiàn)Kalman濾波解算速度非?？?，若利用FPGA的串行口依次輸入觀測值，由于數(shù)據(jù)串行輸入的特點，會使FPGA的解算部分等待數(shù)據(jù)接收完畢才能執(zhí)行濾波解算，導致整體的解算時間過長。為檢驗FPGA實現(xiàn)Kalman濾波器的計算性能，本文預(yù)先將觀測值輸入并保存于FPGA內(nèi)的ROM中，以使FPGA可以連續(xù)地進行濾波解算，實現(xiàn)方案原理如圖1所示。

    圖1中，Kalman濾波解算在FPGA內(nèi)完成，RAM和ROM使用FPGA內(nèi)嵌的硬件RAM存儲器，其中RAM暫存每步的中間結(jié)果，ROM存放濾波中的固定系數(shù)，如觀測矩陣、噪聲系數(shù)陣等。Kalman濾波的解算過程主要利用內(nèi)嵌的硬核乘法器等資源來完成。因解算速度較快，解算結(jié)果暫存于一個稍大的存儲器內(nèi)，同時通過串行口輸出到PC機上保存用于分析。該方案的關(guān)鍵問題是在FPGA中實現(xiàn)Kalman濾波算法。
    FPGA實現(xiàn)Kalman濾波器，其實質(zhì)就是控制數(shù)據(jù)的轉(zhuǎn)移和存儲并實現(xiàn)矩陣的相乘、加、減、求逆等運算。其中，數(shù)據(jù)的轉(zhuǎn)移控制需要有限狀態(tài)機(FSM)來完成，同時FPGA設(shè)計中，不可避免的會遇到資源與速度的問題。因此，需要對上述各關(guān)鍵技術(shù)進行研究和實現(xiàn)。
2．1 矩陣相乘在FPGA中的實現(xiàn)
    Kalman濾波計算中最基本的步驟就是矩陣相乘。對于其中最常見的D=A×B×C型的矩陣相乘，有兩種實現(xiàn)方式：方式一，分步相乘；方式二，直接相乘。事先將矩陣A，B，C分別存入ROM1，ROM2，ROM3中，方式一中，首先進行兩個矩陣的相乘，多路選擇開關(guān)MUX選通ROM1和ROM2，依次讀取其中的數(shù)據(jù)進行乘加，完成前面兩個矩陣的相乘，結(jié)果存入ROMTEMP中；然后，MUX選通ROMTEMP和ROM3，利用前面同樣的資源，完成三個矩陣的連乘。方式二中，ROM1，ROM2，ROM3同時輸出數(shù)據(jù)，MUX根據(jù)解算需要配置乘法器和加法器的輸入，所有的過程同時進行。從上面的執(zhí)行過程可知，方式一的執(zhí)行需要占用更多的時間，而方式二的執(zhí)行會占用更多的資源。對于上述N階的3個矩陣相乘，其占用資源和所需時間如表1所示。

    由表1可知，對于維數(shù)越大的矩陣相乘，需要的浮點加法器越多。由于浮點加法器的生成利用FPGA內(nèi)的基本邏輯單元——可配置邏輯塊(CLB)，所以其占用的CLB等資源也越多。這種現(xiàn)象在上述方式二中尤為突出。本文中研究擬先實現(xiàn)二階Kalman濾波器，階次較低，資源相對充足，為檢驗FPGA實現(xiàn)Kalman濾波器的快速性，選用第二種方式進行矩陣相乘，以得到最快的解算速度。
2．2 有限狀態(tài)機的運用
    有限狀態(tài)機是一種用來進行對象行為建模的工具，其作用主要是描述對象在它的生命周期內(nèi)所經(jīng)歷的狀態(tài)序列，以及如何響應(yīng)來自外界的各種事件。Kalman濾波中，由于解算過程中的邏輯關(guān)系，需要分步進行，所以需要用有限狀態(tài)機來控制各步的轉(zhuǎn)移。根據(jù)各步間的邏輯關(guān)系，可以將其大致分為4個狀態(tài)：S0，S1，S2和S3狀態(tài)。其中，S0為初始化狀態(tài)，之后進入S1狀態(tài)，計算和Pk+1／k，然后進入S2狀態(tài)，計算Kk+1，最后計算解算結(jié)果和Pk+1／k+1。計算Kk+1時，也需要將其分步實現(xiàn)。若將各步的解算歸于同一個狀態(tài)機內(nèi)，則顯得邏輯復(fù)雜。為使得各步的邏輯更加清晰，并且增加狀態(tài)機的穩(wěn)定性和安全性，使用交互狀態(tài)機，如圖2所示。

    圖2中，狀態(tài)機的交互過程中，沒置標志信號enble和finish分別用于啟動和終止計算K值的狀態(tài)，初始化時其值均為0。當計算完第一步進入S2時，enble置為1，啟動計算K值的狀態(tài)；當K值計算完成時，finish置為1，進入S3，enble和finish置為初值0，為下次狀態(tài)交互做準備。進入原狀態(tài)機繼續(xù)進行下面的計算。
    書寫狀態(tài)機時，采用三段式寫法，一個模塊采用同步時序描述狀態(tài)轉(zhuǎn)移，另一個模塊采用組合邏輯判斷轉(zhuǎn)移條件并進行狀態(tài)轉(zhuǎn)移，第三個模塊實現(xiàn)同步輸出。三段式描述方法的狀態(tài)機，做到了同步寄存器輸出，消除了組合邏輯輸出的不穩(wěn)定和毛刺現(xiàn)象，而且更利于時序路徑分組，綜合與布局布線效果更佳。
2．3 資源分時復(fù)用
    FPGA設(shè)計中，資源與速度是個矛盾體。FPGA中的資源是有限的，所以必須考慮資源的節(jié)省問題。由于Kalman濾波可以分3步進行，所以每一步可以利用其它步中相同的資源。此種方法可以在不降低總體速度的情況下，減少資源利用量；而對于階次較高的Kalman濾波，此方法可以最大限度的增加并行性，提高速度。該設(shè)計中用到大量的乘法器、加法器以及CLB等資源，計算第一步時用到的資源會在第二步和第三步中用到，即同一資源被用到3次。以其中用到的某乘法器的分時復(fù)用為例，其輸入端口在不同的時刻可以有不同的賦值，實現(xiàn)語句如下：
   
[!--empirenews.page--]
3 性能對比及分析
    為驗證本文研究利用FPGA實現(xiàn)Kalman濾波算法的性能，采用二階Kalman濾波器進行實際性能對比測試。建立數(shù)學模型如下：選取狀態(tài)轉(zhuǎn)移矩陣，無控制量，即控制陣Uk=0，噪聲系數(shù)矩陣，Wk是系統(tǒng)動態(tài)噪聲，是均值為0、方差陣為的白噪聲隨機序列，觀測系數(shù)矩陣Hk+1=[1，0]，Vk+1表示觀測噪聲，為均值為0，方差為2的白噪聲；初始狀態(tài)協(xié)方差陣為。
    對于該濾波器分別利用三種方式實現(xiàn)：采用Matlab在PC機上實現(xiàn)、利用DSP實現(xiàn)和利用FPGA實現(xiàn)。其中，PC機為DELL Dimension4700臺式機，安裝Matlab 7．0軟件；DSP選用雙精度浮點型TMS320C6713型號的芯片；FPGA型號為XC2VP30，主頻100 MHz，內(nèi)嵌多達136個硬核乘法器和2 MB硬件RAM。為得到更準確合理的結(jié)果，分別截取第1～3次、11～13次、21～23次的計算結(jié)果來對比。因為在PC機上利用Matlab實現(xiàn)的Kalman濾波器通過軟件設(shè)置可以達到很高的計算精度，所以將其得到的結(jié)果作為標準值(真值)，分別用FPGA實現(xiàn)和DSP實現(xiàn)的結(jié)果與其進行對比分析。選取估計均方誤差陣Pk+1／k+1的第一個元素來進行對比分析各實現(xiàn)方式的性能，其解算結(jié)果對比如表2和表3所示。

    由表2可知，利用FPGA實現(xiàn)的濾波結(jié)果，其精度與用Matlab實現(xiàn)的精度相差無幾，且略高于利用DSP實現(xiàn)的單精度的結(jié)果。由表3可知，在解算時間方面，利用FPGA實現(xiàn)方式的快速性遠遠高于利用其他兩種方式。而且，對于階次越高的Kalman濾波器的實現(xiàn)，傳統(tǒng)處理器所需的時間越長，利用FPGA實現(xiàn)方式的時間的優(yōu)越性越顯著。

4 結(jié)語
    針對Kalman濾波的傳統(tǒng)實現(xiàn)方法的不足，本文提出利用具有可并行計算特點的FPGA來實現(xiàn)Kalman濾波的方案，并對FPGA實現(xiàn)Kalman濾波的三種方式進行了研究，確定了利用IP核作為主要計算元件的實現(xiàn)方式。對Kalman濾波實現(xiàn)過程中的關(guān)鍵問題進行了研究和實現(xiàn)。通過對PC機、DSP和FPGA三種實現(xiàn)方式的結(jié)果對比，驗證了利用FGPA實現(xiàn)的Kalman濾波器具有較高的精度和極高的實時性。