基於Hdl Coder實現卡爾曼濾波算法

• 2020 年 5 月 22 日
• 筆記

    總所周知，FPGA極其不擅長複雜算法的運算，但是如果項目中又涉及一些高級算法的實現，在沒有可封裝IP核調用的形式下，我們應該如何進行程序開發呢？今夕已經是2020年，我們一味依賴於用verilog寫代碼無異於用彙編寫程序，這種方式無異於古時鑽木取火的原始時代。如今用Matlab聯調FPGA，基於simulink的Hdl Coder模塊搭建算法模型，再自動生成代碼才是高階有效的終極玩法。尤其在一些信號處理領域，掌握Matlab聯調FPGA的技術更是必備技能。

 下面以卡爾曼濾波為例，具體講述如何基於Hdl Coder實現卡爾曼濾波算法實現。首先簡單介紹下卡爾曼濾波算法：

卡爾曼濾波(Kalman filtering)是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由於觀測數據中包含系統噪聲和干擾的影響，所以最優估計也可看作是濾波過程。簡單來說，它就是利用過去的狀態值和現在的測量值來更正現在的狀態值，利用卡爾曼增益不停在估計和測量中尋找最優化的平衡值。

Kalman filtering的經典五方程，在進行卡爾曼濾波的程序設計前必須要充分理解這五個方程的定義、推導以及相關變量的設定。

 (1) 對於現在狀態的預測方程：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

P(k|k-1)=A P(k-1|k-1) A』+Q ……… (2)

(2)對於現在狀態的更新方程：（拿過去的真實值得到現在的預測值，再集合現在的測量值Z（k），進行現在狀態值的更新）

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

Kg(k)= P(k|k-1) H』 / (H P(k|k-1) H』 + R) ……… (4)

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

其中Z(k)=H X(k)+V(k)為觀測方程。

對於上述方程要明確以下幾個變量的賦值：

1） 狀態轉移矩陣：A

2） 系統控制變量：U(k)

3） 觀測矩陣：H

4）系統狀態初值：X（0|0）

5）系統協方差：P(0|0)

6）R：測量噪聲

7）Q：過程噪聲

8）Z：觀測值

這7個變量要根據不同的產品以及應用場景進行具體賦值，其中對於單系統輸入，變量賦值為：

1）A=1（狀態轉移矩陣），U(k)=0（系統控制變量）, H=1（觀測矩陣），B=1（對於一維變量，全為1）

2）Z觀測值就是系統外部灌進來的實時變量。

3）系統協方差：P(0|0)，可以採樣一段時間後進行運算保證在kalman濾波器工作前進行賦值即可。

4）對於卡爾曼應用，最難確定的就是Q、R這兩個噪聲，只能根據實際模型，不停調整以逼近最優解。

基於上述的卡爾曼濾波搭建simulink仿真模型

 下面對上述simulink模型進行進一步封裝

 具體設置kalman模塊的Hdl Coder參數模式

 最後點擊「Generate HDL」與「Generate Test Bench」，生成verilog源程序與測試代碼

要對執行文件進行仿真，只需要在modelsim命令框自動執行以下後綴文件即可「kalman_compile.do」,”kalman_tb_compile.do”,”kalman_tb_sim.do”即可。

下面進行卡拉曼濾波效果展示：

結果展示：

結果展示：