基于FPU的高速卡爾曼濾波器公式推導(dǎo)法硬件設(shè)計

劉超 嚴(yán)偉

?

基于FPU的高速卡爾曼濾波器公式推導(dǎo)法硬件設(shè)計

劉超 嚴(yán)偉?

北京大學(xué)軟件與微電子學(xué)院, 北京 100871; ? 通信作者, E-mail: yanwei@ss.pku.edu.cn

基于卡爾曼濾波器的傳統(tǒng)硬件實現(xiàn)方式, 根據(jù)濾波模型和矩陣運算, 將濾波公式進行推導(dǎo)和化簡, 然后利用“自底向上”的設(shè)計思路, 設(shè)計濾波公式需要的底層FPU (float point unit), 從而實現(xiàn)整個卡爾曼濾波系統(tǒng)。以這種方法設(shè)計的卡爾曼濾波器, 不僅擺脫了傳統(tǒng)實現(xiàn)方式對于平臺的依賴性, 增加了系統(tǒng)的可移植性和應(yīng)用范圍, 并且濾波速度比傳統(tǒng)矩陣運算法有明顯提升。對于勻加速濾波模型, 給出公式推導(dǎo)法和矩陣運算法的詳細數(shù)據(jù)對比, 采用該方法設(shè)計的卡爾曼濾波器, 濾波精度保持原來的水平, 濾波速度提升為傳統(tǒng)矩陣運算法的2.1倍。

卡爾曼濾波器; 目標(biāo)跟蹤; IEEE754; 浮點數(shù)運算; 實時性

卡爾曼濾波理論[1]在 20 世紀(jì) 60 年代提出后, 在雷達跟蹤、導(dǎo)航和控制等領(lǐng)域獲得廣泛應(yīng)用。多維卡爾曼濾波算法的實現(xiàn)需要復(fù)雜的矩陣運算, 在卡爾曼濾波器的實際應(yīng)用中, 軟件實現(xiàn)方式很難滿足系統(tǒng)對高實時性的要求, 硬件實現(xiàn)方式可以利用并行處理的優(yōu)點, 加快濾波速度。在卡爾曼濾波器的傳統(tǒng)硬件實現(xiàn)方式中, 通常利用第三方開發(fā)平臺, 如借助LABVIEW FPGA和DSP Builder的圖形化開發(fā)工具或者 QuartusⅡ的浮點數(shù)運算 IP 核[2-4], 或者通過構(gòu)建矩陣運算模塊來實現(xiàn)濾波算法[5], 不僅增加了濾波算法對平臺的依賴性, 降低了系統(tǒng)的可移植性和應(yīng)用范圍, 而且復(fù)雜的浮點數(shù)矩陣運算成為進一步提升濾波速度的瓶頸。

本文基于卡爾曼濾波器的傳統(tǒng)實現(xiàn)方式, 首先根據(jù)濾波變量之間的關(guān)系將高維模型拆分為低維濾波模型; 再根據(jù)低維濾波模型的濾波參數(shù)設(shè)置情況, 將組成卡爾曼濾波器的5條濾波公式進行徹底推導(dǎo)并化簡, 以降低濾波算法的時間復(fù)雜度; 然后設(shè)計基于IEEE754標(biāo)準(zhǔn)的浮點數(shù)運算單元FPU[6], 并利用“自底向上”的設(shè)計思路, 以FPU作為底層模塊, 對推導(dǎo)后的濾波公式加以實現(xiàn), 進而構(gòu)成整個濾波系統(tǒng)。

1 卡爾曼濾波器介紹和算法優(yōu)化

卡爾曼濾波器用于估計離散時間過程的狀態(tài)變量, 這個離散時間過程由一個線性隨機差分方程描述[7], 稱為系統(tǒng)方程:

系統(tǒng)的測量方程為

矩陣,,,由實際濾波模型決定, 可以是確定的, 也可能隨時間變化[8],和分別是系統(tǒng)噪聲和測量噪聲, 為相互獨立的高斯白噪聲。

從形式上看, 卡爾曼濾波器由 5 個公式組成。其中式(1)完成狀態(tài)預(yù)測, 式(2)完成協(xié)方差預(yù)測, 式(3)用來計算卡爾曼增益, 式(4)完成狀態(tài)的濾波估計, 式(5)用來更新濾波協(xié)方差。

狀態(tài)預(yù)測方程:

協(xié)方差預(yù)測方差:

卡爾曼增益方程:

濾波估計方程:

協(xié)方差更新方程:

我們考慮一個雷達實時跟蹤系統(tǒng), 假設(shè)有一個在三維空間--內(nèi)運動的飛行器, 設(shè),和分別為的距離、方位角誤差和仰角誤差。考慮,Dq和Dj是時間的函數(shù), 并且一階導(dǎo)數(shù)和二階導(dǎo)數(shù)分別表示為,,和,,,為采樣時間單位。對該系統(tǒng)有

對于該模型, 系統(tǒng)無額外控制量輸入, 即有控制量為零矩陣, 此處基于算法完整性考慮, 假設(shè)控制矩陣的維度為×1, 控制量為1×1矩陣。所以有

假設(shè)原點位置有一雷達對飛行器進行目標(biāo)跟蹤, 有

。

容易證明該系統(tǒng)可以分解為3個具有相似狀態(tài)空間描述的子系統(tǒng)[9], 即這個九維濾波問題可以拆分成3個三維濾波模型: 距離跟蹤模型、方位角跟蹤模型和仰角跟蹤模型, 并且每個濾波模型的轉(zhuǎn)換矩陣和測量矩陣都相同, 分解之后的三維模型為

,

,

根據(jù)矩陣運算的性質(zhì), 將上述濾波參數(shù)帶入濾波公式, 對三維濾波模型的 5 個公式做徹底推導(dǎo)并簡化, 從而將矩陣運算轉(zhuǎn)化為加、減、乘、除運算, 這里給出式(1)的推導(dǎo)結(jié)果, 其余 4 個公式可做類似推導(dǎo):

將矩陣運算展開得各元素如下:

,

2 電路設(shè)計

2.1 系統(tǒng)結(jié)構(gòu)

系統(tǒng)的結(jié)構(gòu)如圖 1 所示, 需要說明的是, 圖中所有與濾波算法有關(guān)的變量或者信號均是矩陣形式, 此處為方便表示, 只用一個信號引腳表示整個矩陣。

CLKGEN 模塊利用外來時鐘信號生成系統(tǒng)時鐘信號, 并產(chǎn)生系統(tǒng)初始化信號。CAL_1, CAL_2, CAL_3, CAL_4和CAL_5分別完成濾波算法 5 個公式的內(nèi)容, 即狀態(tài)預(yù)測, 協(xié)方差預(yù)測, 卡爾曼增益的計算, 狀態(tài)的濾波估計以及濾波協(xié)方差的更新。MUX_X和MUX_P分別用來控制算法迭代過程中的狀態(tài)更新和協(xié)方差更新。寄存器組 REGS_X和 REGS_P 由若干寄存器構(gòu)成, 用來完成不同模塊之間的數(shù)據(jù)寄存和時序調(diào)整。AFIFO 為異步 FIFO (first in first out), 用來緩存輸入數(shù)據(jù), 匹配數(shù)據(jù)輸入速度和濾波速度[10]。

FPU 的結(jié)構(gòu)如圖 2 所示, FPU 模塊完成基于IEEE754標(biāo)準(zhǔn)的32位浮點數(shù)的加、減、乘、除運算, 支持對兩個操作數(shù)的加、減、乘、除運算, 是卡爾曼濾波器的底層基礎(chǔ)運算模塊。

2.2 系統(tǒng)設(shè)計思路

系統(tǒng)采用“自底向上”的層次化設(shè)計方法, 首先設(shè)計濾波算法所需的浮點數(shù)運算單元FPU，然后以 FPU 為底層模塊分別構(gòu)建5個濾波公式模塊CAL_1, CAL_2, CAL_3, CAL_4和CAL_5, 進而構(gòu)成最頂層的卡爾曼濾波系統(tǒng)。

以矩陣乘法運算為例, 一個×與×的矩陣相乘, 如果所有元素參與運算的話, 需要的乘法運算次數(shù)是××, 需要的加法運算次數(shù)是×× (–1)。針對傳統(tǒng)矩陣運算法的不足, 我們根據(jù)濾波模型的具體參數(shù)設(shè)置和矩陣運算的性質(zhì), 將構(gòu)成濾波算法的 5 個公式進行推導(dǎo)和簡化, 并用 FPU 直接實現(xiàn)推導(dǎo)、簡化后的濾波公式, 從而減少濾波算法所需的運算次數(shù), 提升硬件處理速度。

卡爾曼濾波器5個公式的數(shù)據(jù)傳遞情況如圖3所示, 根據(jù)濾波公式之間的數(shù)據(jù)引用情況, 可以將5個公式分成3個執(zhí)行狀態(tài)和時序。其中, CAL_1和 CAL_2 組成第 1 個執(zhí)行時序, CAL_3 組成第 2個執(zhí)行時序, CAL_4 和 CAL_5 組成第 3 個執(zhí)行時序。

系統(tǒng)各模塊的處理速度與 FPU 的運算速度緊密相關(guān), 在設(shè)計FPU時, 我們綜合考慮浮點數(shù)運算的速度和精度, 設(shè)計的FPU執(zhí)行一條加、減、乘、除運算需要的初始化時間是4個時鐘周期, 完成初始化之后, 每個時鐘周期即可完成一次加、減、乘、除運算。

由 FPU 構(gòu)建的上層濾波運算模塊 CAL_1, CAL_2, CAL_3, CAL_4和CAL_5初始化所需時間由各模塊運算最慢的元素決定, 具體計算各個公式的各個元素時, 暫不參加計算的數(shù)據(jù)采用模塊內(nèi)部的寄存器組暫時保存。表1是使用公式推導(dǎo)法和傳統(tǒng)矩陣運算法時, 組成卡爾曼濾波器的5個濾波公式模塊完成模塊初始化需要的時間對比。

表1 初始化時間對比

各個濾波模塊的初始化時間會影響系統(tǒng)的整體濾波速度。在系統(tǒng)時鐘頻率為 100 MHz的情況下, 使用傳統(tǒng)矩陣運算法設(shè)計完成的卡爾曼濾波器, 完成一次濾波運算需要 840 ns; 運用公式推導(dǎo)法設(shè)計完成的卡爾曼濾波器, 完成一次濾波僅需 400 ns?？梢钥闯? 使用本方法是使用傳統(tǒng)矩陣運算法速度的2.1倍。

2.3 FPGA平臺參數(shù)評估

圖 4 為利用Quartus Ⅱ軟件, 對系統(tǒng)進行分析和綜合后得到的在FPGA平臺下的參數(shù)評估結(jié)果, 顯示該系統(tǒng)的組合邏輯、時序邏輯等參數(shù)的資源占用情況。

3 應(yīng)用結(jié)果

圖 5 是卡爾曼濾波器的系統(tǒng)輸入值 measure_ input, 包含系統(tǒng)噪聲和測量噪聲。從圖中可以看出, 該輸入信號包含一定的噪聲干擾, 需要通過卡爾曼濾波器對其進行濾波處理。

圖 6 是經(jīng)過卡爾曼濾波算法濾波優(yōu)化之后的原變量輸出值x_now_0及其一階導(dǎo)數(shù)值x_now_1和二階導(dǎo)數(shù)值x_now_2。可以看出, x_now_0, x_now_1和x_now_2在經(jīng)過一定次數(shù)的濾波算法迭代后, 都得到很好的去噪效果。

圖 7 是預(yù)測誤差矩陣p_pred_00, p_pred_11和p_pred_22 的曲線圖, 可以看出, 誤差隨濾波算法迭代次數(shù)的增加而變化。

4 總結(jié)

本文根據(jù)濾波變量之間的關(guān)系將高維模型拆分為低維濾波模型, 再根據(jù)低維濾波模型的濾波參數(shù)設(shè)置情況, 將組成卡爾曼濾波器的5個濾波公式進行徹底推導(dǎo)和簡化, 然后設(shè)計浮點數(shù)運算單元FPU, 并利用“自底向上”的設(shè)計思路, 以FPU作為底層模塊對推導(dǎo)后的濾波公式加以實現(xiàn), 進而構(gòu)成整個濾波系統(tǒng)。采用此方法設(shè)計的卡爾曼濾波器在保證濾波精度的同時, 還擺脫了濾波平臺的依賴性, 增加了系統(tǒng)的可移植性和應(yīng)用范圍, 并且提升了濾波速度，可以更好地滿足實際工程應(yīng)用中對卡爾曼濾波器的高實時性要求。

參考文獻

[1]Kalman R E. A new approach to linear filtering and prediction problems. Transaction of the ASME —Journal of Basic Engineering, 1960, 82: 35–45

[2]Fonseca J V, Oliveira R C L, Abreu J A P, et al. Kalman filter embedded in FPGA to improve tracking performance in ballistic rockets // 2013 UKSim 15th International Conference on Computer Modelling and Simulation. Cambridge, 2013: 606–610

[3]楊丹. 卡爾曼濾波器設(shè)計及其應(yīng)用研究[D]. 湘潭: 湘潭大學(xué), 2014

[4]Wu Panlong, Zhang Lianzheng, Zhang Xinyu. The design of DSP/FPGA based maneuvering target tracking system. Wseas Transactions on Circuits and Systems, 2014, 13: 75–84

[5]趙大建, 趙偉, 張兆亮, 等. 基于FPGA的Kalman濾波器實現(xiàn)研究. 現(xiàn)代電子技術(shù), 2012, 35(6): 67–70

[6]ANSI/IEEE Std 754-1985, IEEE Standard for Binary Floating-Point Arithmetic[S]. Piscataway, NJ: IEEE Standards Board, 1985

[7]Welch G, Bishop G. An Introduction to the Kalman filter. University of North Carolina at Chapel Hill, 1995(7): 127–132

[8]Auger F, Hilairet M, Guerrero J M, et al. Industrial applications of the Kalman filter: a review. IEEE Transactions on Industrial Electronics, 2013, 60(12): 5458–5471

[9]Chui C K, Chen G. Kalman filtering with real-time applications. 4th ed. Berlin: Springer, 2008: 45–48

[10]王凱, 孫鋒. 異步FIFO的設(shè)計分析. 電子器件, 2014(3): 431–434

High-Speed Hardware Design of Kalman Filter Based on FPU with Formula Derivation Method

LIU Chao, YAN Wei?

School of Software and Microelectronics, Peking University, Beijing, 100871; ? Corresponding author, E-mail: yanwei@ss.pku.edu.cn

Based on the traditional hardware implementations of Kalman filter, the authors derive and simplify the filtering formulas according to the filtering model and matrix operations, and then design underlying FPU (float point unit) needed by the filtering formulas according to the “bottom-up” design thinking, to implement the complete Kalman filter system. The Kalman filter designed by this method not only gets rid of dependence on third-party platforms and increase the portability and application areas of filtering system, but the filtering speed improves significantly than traditional matrix operation method. For a constant acceleration filtering model, this paper provides detailed data comparison between formula derivation method and traditional matrix operation method, the Kalman filter designed by this method maintains the accuracy of the previous level and achieves the computing speed 2.1 times, compared with the traditional matrix operation method.

Kalman filter; target tracking; IEEE754; floating-point operation; real-time

10.13209/j.0479-8023.2015.135

TN492

2015-04-22;

2015-06-04; 網(wǎng)絡(luò)出版日期: 2016-04-07

江蘇省產(chǎn)學(xué)研聯(lián)合創(chuàng)新資金——前瞻性聯(lián)合研究項目(BY2013018)資助