Kaiman濾波算法在FPGA上的設計與實現(xiàn)

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

1 Kalman濾波算法理論

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

則Kalman濾波方程組如下：

狀態(tài)一步預測方程：

從式(1)～(6)可知，若利用傳統(tǒng)的處理器實現(xiàn)Kalman濾波算法，由于其指令執(zhí)行的順序性，至少需要分為5步來實現(xiàn)，其中每一步還都需要進行至少1次的加法和乘法等運算，每次運算都要順序執(zhí)行，其執(zhí)行速度和效率很低；如果利用FPGA來進行Kalman濾波，根據其各步的邏輯關系，可以分為3步來實現(xiàn)，即第一步計算狀態(tài)一步預測值和一步預測均方誤差Pk+1／k，第二步計算濾波增益Kk+1，第三步計算狀態(tài)最優(yōu)估值和估計均方誤差Pk+1／k+1。由此可知，利用FPGA技術可以實現(xiàn)Kalman濾波的并行計算，壓縮計算時間，提高解算速度。因此，對FPGA的Kalman濾波進行研究開發(fā)，可實現(xiàn)基于FPGA的快速Kalman濾波解算，滿足在對實時性要求更高的環(huán)境中使用。

2 在FPGA中實現(xiàn)Kalman濾波算法研究

由于FPGA實現(xiàn)Kalman濾波解算速度非?？欤衾肍PGA的串行口依次輸入觀測值，由于數據串行輸入的特點，會使FPGA的解算部分等待數據接收完畢才能執(zhí)行濾波解算，導致整體的解算時間過長。為檢驗FPGA實現(xiàn)Kalman濾波器的計算性能，本文預先將觀測值輸入并保存于FPGA內的ROM中，以使FPGA可以連續(xù)地進行濾波解算，實現(xiàn)方案原理如圖1所示。

圖1中，Kalman濾波解算在FPGA內完成，RAM和ROM使用FPGA內嵌的硬件RAM存儲器，其中RAM暫存每步的中間結果，ROM存放濾波中的固定系數，如觀測矩陣、噪聲系數陣等。Kalman濾波的解算過程主要利用內嵌的硬核乘法器等資源來完成。因解算速度較快，解算結果暫存于一個稍大的存儲器內，同時通過串行口輸出到PC機上保存用于分析。該方案的關鍵問題是在FPGA中實現(xiàn)Kalman濾波算法。

FPGA實現(xiàn)Kalman濾波器，其實質就是控制數據的轉移和存儲并實現(xiàn)矩陣的相乘、加、減、求逆等運算。其中，數據的轉移控制需要有限狀態(tài)機(FSM)來完成，同時FPGA設計中，不可避免的會遇到資源與速度的問題。因此，需要對上述各關鍵技術進行研究和實現(xiàn)。

2．1 矩陣相乘在FPGA中的實現(xiàn)

Kalman濾波計算中最基本的步驟就是矩陣相乘。對于其中最常見的D=A×B×C型的矩陣相乘，有兩種實現(xiàn)方式：方式一，分步相乘；方式二，直接相乘。事先將矩陣A，B，C分別存入ROM1，ROM2，ROM3中，方式一中，首先進行兩個矩陣的相乘，多路選擇開關MUX選通ROM1和ROM2，依次讀取其中的數據進行乘加，完成前面兩個矩陣的相乘，結果存入ROMTEMP中；然后，MUX選通ROMTEMP和ROM3，利用前面同樣的資源，完成三個矩陣的連乘。方式二中，ROM1，ROM2，ROM3同時輸出數據，MUX根據解算需要配置乘法器和加法器的輸入，所有的過程同時進行。從上面的執(zhí)行過程可知，方式一的執(zhí)行需要占用更多的時間，而方式二的執(zhí)行會占用更多的資源。對于上述N階的3個矩陣相乘，其占用資源和所需時間如表1所示。

由表1可知，對于維數越大的矩陣相乘，需要的浮點加法器越多。由于浮點加法器的生成利用FPGA內的基本邏輯單元——可配置邏輯塊(CLB)，所以其占用的CLB等資源也越多。這種現(xiàn)象在上述方式二中尤為突出。本文中研究擬先實現(xiàn)二階Kalman濾波器，階次較低，資源相對充足，為檢驗FPGA實現(xiàn)Kalman濾波器的快速性，選用第二種方式進行矩陣相乘，以得到最快的解算速度。