專利名稱：一種基于現(xiàn)場可編程門陣列的卡爾曼濾波器的制作方法
技術領域：
本實用新型涉及一種濾波器，具體為一種基于現(xiàn)場可編程門陣列(FPGA, Field Programmable Gate Array)的卡爾曼濾波器i殳計。采用此種硬件集成電 路，在已知歷史量測的條件下，對目標的位置估計、速度等狀態(tài)的估計值與測 量值之間誤差的方差進行最小化，并通過更新k時刻的狀態(tài)正確估計目標在k+l 時刻的位置和速度等狀態(tài)矢量。本實用新型屬于數(shù)字集成電路設計和數(shù)字信號 處理技術領域。
(二 )背景4支術
隨著微電子技術的發(fā)展和高速、超高速集成電路芯片的問世，使計算機技 術不管是在容量上還是在速度上均有了非常大的提高。許多過去難以實時計算 的問題也逐步迎刃而解。其中包含卡爾曼濾波算法的廣泛應用，就是一個明顯的 例子。
卡爾曼濾波算法是一種最優(yōu)化自回歸數(shù)據(jù)處理算法。在機動目標跟蹤中具 有良好的性能，對于解決很大部分的問題，卡爾曼濾波算法是最佳估計并能進 行遞推計算。它的廣泛應用已經(jīng)超過30年，包括機器人導航，控制，傳感器數(shù) 據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近年來更被應用于計算 機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。
卡爾曼濾波，是一種線性最小方差估計，特點是考慮了系統(tǒng)的模型誤差和 測量噪聲的統(tǒng)計特性?？紤]帶L個傳感器的多傳感器系統(tǒng)，目標的運動模型和測 量模型分別為
x(A; + l) = Ox(A:) + GV(A:) (1) + i=l, 2,…L (2)
其中難)=雄)少(O KO W)雄)〗"",為n維目標狀態(tài)變量；,e iT;, 為第i個傳感器的m維觀測向量，w(A:)e及"為n維過程噪聲向量，v'("eiT'是m維測 量噪聲向量，二者均是零均值的相互獨立白噪聲隨機向量序列，①為"x"維狀態(tài) 轉移矩陣，G為噪聲分布矩陣，//;為mx"維測量系tt陣。W④是零均值高斯過程白噪聲向量，代表的物理意義是角加速度，有
i￡[ ] = 2, (3)
v'(A:)是均值為零且相互獨立的高斯序列，物理意義是測量誤差，有 i五[w(^h/(力]-i (A:)c^ (4)
則卡爾曼濾波基本方程如下
預測方程 i(A: + l/" = (D(A:)i(^:) (5)
預測協(xié)方差陣尸(^ + 1/" = 0^)尸(^^(^ + 2(" (6)
殘差協(xié)方差陣+1) = + +1 / "/T (A: +1) + +1) (7)
爾曼濾波增益陣《("l卜P("l/^/、 + l)S-乂A: + l) (8)
濾波協(xié)方差更新尸"+1) = [7 -《"+ + +1 / (9) 狀態(tài)更新+1) = f (A +1 / " +《(A + +1) — //(A: ++1 / 0)
以上即為集中式卡爾曼濾波的基本方程。只要給定濾波初值f(o)和P(o)，根 據(jù)k時刻的觀測A就可遞推計算得此時刻的狀態(tài)估計值i(W 。
在一個濾波周期內(nèi)，從卡爾曼濾波在使用系統(tǒng)信息和觀測信息的先后次序
來看，卡爾曼濾波具有兩個明顯的信息更新過程時間更新過程和觀測更新過 程。式(8)說明了根據(jù)k時刻的狀態(tài)估計預測k+l時刻狀態(tài)的方法，式(6)對這種 預測的質量優(yōu)劣做了定量描述。這兩式的計算中，僅使用了與系統(tǒng)的動態(tài)特性 有關的信息，如狀態(tài)一步轉移矩陣、噪聲輸入陣、過程噪聲方差陣。從時間的 推移過程來看，這兩式將時間從k時刻推進至k+l時刻，描述了卡爾曼濾波的時 間更新過程。其余各式用來計算對時間更新值的修正量，該修正量由時間更新
的質量優(yōu)劣(4")、觀測信息的質量優(yōu)劣(&)、觀測與狀態(tài)的關系(A)以及具 體的觀測信息^所確定，這一過程描述了卡爾曼濾波的觀測更新過程。將卡爾 曼濾波方程改寫為框圖即可清晰看出的看出觀測和狀態(tài)的更新過程，如圖l所 示。 由圖可得，卡爾曼濾波具有兩個計算回路:增益計算回路和濾波計算回 路。增益計算回路是獨立計算的，濾波計算回路依賴于增益計算回路?？柭?濾波的兩個回^各相互耳關系。
在應用中，卡爾曼濾波器通常由軟件處理或者DSP處理器來實現(xiàn)。使用DSP處 理器具有設計簡單靈活，可直接采用C語言矩陣運算的優(yōu)點，但由于卡爾曼濾波算法在運算中有大量的矩陣加減乘除運算且DSP處理器采用程序順序執(zhí)行的CPU
架構，在要求較高的場合，不能滿足系統(tǒng)高速、實時的需要。而基于FPGA實現(xiàn)的
卡爾曼濾波器，采用的是硬件并行算法，肯巨很好的解決速度和實時性的問題，
并且其具有靈活的可配置特性和優(yōu)良的抗干擾能力，使得FPGA構成的數(shù)字信號 處理系統(tǒng)非常易于修改、測試及硬件升級。本專利以基于現(xiàn)場可編程邏輯門陣 列FPGA器件為硬件平臺，進行算法設計?；谀K化設計思想，設計了FPGA控 制模塊和卡爾曼濾波子模塊，實現(xiàn)了FPGA硬件電路的卡爾曼濾波器結構設計和 遞推的目標狀態(tài)最佳估計。

發(fā)明內(nèi)容
1、 目的本實用新型的目的是提供一種基于FPGA的卡爾曼濾波器，它克 服了現(xiàn)有技術的不足，在實際使用中具有靈活的可配置特性和優(yōu)良的抗干擾能 力，使得FPGA構成的數(shù)字信號處理系統(tǒng)非常易于修改、測試及硬件升級。
2、 技術方案本實用新型是一種基于FPGA的卡爾曼濾波器，它是一個硬 件集成電路，該電路主要由六個模塊組成，它們是模塊fcl,用來完成最佳濾 波值計算；模塊fc2,計算一步狀態(tài)預測值；模塊fc3，實現(xiàn)濾波增益值的計算； 模塊fc4，完成預測誤差方差的計算；模塊fc5，完成濾波誤差方差的計算；狀 態(tài)控制模塊statemachine，控制整個增益計算回路和濾波計算回路的時序和狀 態(tài)跳轉。
所述最佳濾波值計算模塊fcl如圖3所示。經(jīng)過方程分解
x
+1)=辟+1 / fc) +碌+1), +1) — //(A: + l)雄+1 / :
X2」
(11)
其中；q + A:辦-;x/)， ；x:2 =x/2十A:2(y-x/》
模塊fcl用來完成最佳濾波值計算，輸入計算信號為系統(tǒng)上位機采樣值，
即探測目標的坐標信息、遞歸計算的卡爾曼濾波增益和一步狀態(tài)預測值，所有
輸入均為32位sign型變量。其具體算法如公式(11)。在有符號數(shù)計算中，存 在兩個32位有符號數(shù)相乘結果截位的問題。具體的截位算法為首先將相乘的 結果賦值給一個64寄存器變量，然后判斷符號位，如果是正數(shù)且高32位中沒 有1,則直接截取符號位和低31位組成一個新的32位數(shù)；如果高32位中存在 1,則低31位全部置1加上符號位組成新的32位數(shù)。當符號位為1即所得結果 為負數(shù)時，截取方法類似。
所述一步狀態(tài)預測值計算模塊fc2如圖4所示。經(jīng)過方程分解
fCA:/":!),"")^:")^/, x/2〗[ (12)
模塊fc2計算一步狀態(tài)預測值，其具體算法見上述公式。式中T為上位機 一個采樣周期的時間，對應為卡爾曼濾波的一個計算周期。以系統(tǒng)時鐘為ns級 計算， 一個計算周期約占50-100 (決定于遞歸次數(shù)，和卡爾曼濾波增益的初值設置相關)個系統(tǒng)時鐘，則 一個計算周期在ms量級，對應的系統(tǒng)頻率為MHz以 上，完全滿足采樣要求。因此，此處T設置成系統(tǒng)內(nèi)部的一個計數(shù)器，每完成 一次計算(以計算時間最大值為一個計算周期)自加一個時間單位(計算周期)。 T對應于電路中的time-count端口 。
所述濾波增益值計算模塊fc3如圖5所示。經(jīng)過方程分解
<formula>formula see original document page 6</formula>
模塊fc3實現(xiàn)濾波增益值的計算。其實現(xiàn)過程中需要應用除法器。32位有 符號除法器是利用Xilinx ISE自帶的IP core來生成，使整個設計變得簡單易 行。ISE自帶的除法器為流水結構，在不考慮占用資源的前提下，可在每個時鐘 周期輸出計算值。但考慮到芯片資源有限，可適當調整為四個時鐘周期完成一 次除法計算，在利用coregenerator生成除法器時選擇適合的計算周期即可。
所述預測誤差方差計算模塊fc4和濾波誤差方差計算模塊fc5如圖6所示。 經(jīng)過方程分解<formula>formula see original document page 6</formula>
模塊fc4和fc5分別完成預測誤差方差和濾波誤差方差的計算中的截取問 題，和前面4又勤目同。
所述狀態(tài)控制模塊s tatemachine如圖7所示。狀態(tài)控制模塊 (statemachine)主要實現(xiàn)對整個卡爾曼的時序控制和內(nèi)部信號T的生成。根 據(jù)卡爾曼濾波狀態(tài)圖，共需要六個計算信號對應六個狀態(tài)，每個狀態(tài)的跳轉都 由s-l-coniput到s —6—comput信號控制執(zhí)行，在新的狀態(tài)信號來臨之前，所有 狀態(tài)均在等待。當完成一個系統(tǒng)周期的計算之后，statemachine模塊向上位機 發(fā)出REQUEST信號，接收下一個采樣值循環(huán)計算。
狀態(tài)控制模塊的狀態(tài)分配如圖8所示
上述六個^f莫塊的連接方式是按照端口直接的線連接。信號在各模塊中由狀 態(tài)控制模塊控制順序處理，不需要加入其他先入先出模塊(FIFO)等。其信號走向是如下
St印l:在READY狀態(tài)，給所有參量賦初值，同時初始化所有模塊，在valid 脈沖上升沿讀取上位機采樣信號y，送進模塊fc4等待下一個狀態(tài)的觸發(fā)信號；
St印2:當狀態(tài)s-l的s_l_compute觸發(fā)信號上升沿來臨，進入s —1狀態(tài)， 計算模塊fc4和fc2;
St印3:當s_2_compute的信號上升沿來臨，完成狀態(tài)s —2的才莫塊fc5的計
算；
Step4:當s-3-compute的信號來臨，對比P—11和P_21新值和寄存器寄存 的兩舊值，當它們的差為一個設定小量的值或以下時，則滿足判斷條件，跳轉 至狀態(tài)S-4，否則浪〖轉至狀態(tài)s —5,重新遞歸計算新的P_ll和P—21^4,直到遞 歸到一個穩(wěn)定的值；
Step5:由s一6—compute信號控制，根據(jù)穩(wěn)定的kl、 k2值計算一次xl和x2 并輸出，同時跳轉到狀態(tài)s —7;
Step6:計算xl、 x2完成后，發(fā)出REQUEST信號，重新讀取上位機新的采樣 信號y，跳轉至s-l狀態(tài)。
在本專利中，卡爾曼濾波器的輸入信號為紅外、紫外或毫米波雷達傳感器
所得的目標位置信息。在一維情況下，目標的動力學方程經(jīng)離散化以后可以表 示為jc(A: + 1) = (Dx(A:) + Gw(A:)
系統(tǒng)的量測方程+ U = + D義W +1) + 「(" 1) (11) 其中，
Vq r、
x、 ^分別代表傳感器探測到的目標位置和速度信息。T為濾波周期。
由于卡爾曼濾波的基本方程是時間域內(nèi)的遞推形式，其計算過程是一個不
斷的"預測一修正，，過程，在求解時不要求存儲大量的數(shù)據(jù)，并且一旦觀測到
了新的數(shù)據(jù)，隨時可以算得新的濾波值，因此這種濾波方程非常便于實時處理。 依據(jù)卡爾曼濾波方程的矩陣分解將卡爾曼的濾波回路和增益計算回路表示
成五個基本的子系統(tǒng)，分別記作fcl、 fc2、 fc3、 fc4和fc5和狀態(tài)控制模塊
statemachine。系統(tǒng)的整體框圖如圖2所示。
3、優(yōu)點及功效
(1) 本實用新型所涉及的硬件卡爾曼濾波器具有實時信息處理能力，運算速度 在MHz量級。
(2) 本實用新型所涉及的卡爾曼濾波器采用了基于FPGA的結構設計，其環(huán)境噪聲及系統(tǒng)噪聲模型可以調節(jié)，適于各種卡爾曼濾波算法的方案。 (3)本實用新型所涉及的卡爾曼濾波器由于采用了模塊化設計，每一部分都可
以單獨拿出作為預測方程的IP核使用。 (4 )太$ J l浙刑所沐刃的士 ^暴詭誠緊的碰林l《由.3夂苴漆成孰.簡並.
易于實現(xiàn)。
(5)本實用新型所涉及的卡爾曼濾波器，應用范圍廣，由于其配置靈活、資源 消耗小，可以在航空航天，空間探測器、車載導航系統(tǒng)、電力電機檢測系統(tǒng)等 設備上使用。


圖1:卡爾曼濾波方框示意圖
圖2:卡爾曼濾波電路總體結構方框示意圖
圖3:最佳濾波值模塊外部接口方框示意圖
圖4: 一步狀態(tài)預測值模塊外部接口方框示意圖
圖5:濾波增益值模塊外部接口方框示意圖
圖6:預測誤差方差和濾波誤差方差模塊外部接口方框示意圖
圖7:狀態(tài)控制模塊外部接口方框示意圖
圖8:卡爾曼濾波電路狀態(tài)分配示意圖
圖中的符號說明如下
s-l， s_2， s-3, S-4, s-5， s —6, s_7 為卡爾曼濾波整個控制模塊的7 個狀態(tài)；VALID 為輸入有效信號；REQUEST 為輸入請求信號。
具體實施方式
見圖l、圖2、圖3、圖4、圖5、圖6、圖7、圖8所示，具體實施如下
一種基于FPGA的卡爾曼濾波器，它是一個硬件集成電路，該電路主要由六 個模塊組成，它們是模塊fcl,用來完成最佳濾波值計算；模塊fc2，計算一 步狀態(tài)預測值；模塊fc3,實現(xiàn)濾波增益值的計算；模塊fc4，完成預測誤差方 差的計算；it塊fc5，完成濾波誤差方差的計算；狀態(tài)控制纟莫塊，控制整個增益 計算回路和濾波計算回路的時序和狀態(tài)跳轉。
所述最佳濾波值計算模塊fcl如圖3所示。經(jīng)過方程分解
<formula>formula see original document page 8</formula>模塊fcl用來完成最佳濾波值計算，輸入計算信號為系統(tǒng)上位機采樣值， 即探測目標的坐標信息、遞歸計算的卡爾曼濾波增益和一步狀態(tài)預測值，所有
輸入均為32位sign型變量。其具體算法如公式(11)。在有符號數(shù)計算中，存 在兩個32位有符號數(shù)相乘結果截位的問題。具體的截位算法為首先將相乘的 結果賦值給一個64寄存器變量，然后判斷符號位，如果是正數(shù)且高32位中沒 有1,則直接截取符號位和低31位組成一個新的32位數(shù)；如果高32位中存在 1，則低31位全部置1加上符號位組成新的32位數(shù)。當符號位為1即所得結果 為負數(shù)時，截取方法類似。
所述一步狀態(tài)預測值計算模塊fc2如圖4所示。經(jīng)過方程分解
》(yt"-l)二(D(Hl)》(A:-l)+A ;c/2]: (〗2)
其中= Xi + !Dc2 x/2 = x2
模塊fc2計算一步狀態(tài)預測值，其具體算法見上述公式。式中T為上位機 一個采樣周期的時間，對應為卡爾曼濾波的一個計算周期。以系統(tǒng)時鐘為ns級 計算， 一個計算周期約占50-100 (決定于遞歸次數(shù)，和卡爾曼濾波增益的初值 設置相關)個系統(tǒng)時鐘，則一個計算周期在ms量級，對應的系統(tǒng)頻率為MHz以 上，完全滿足采樣要求。因此，此處T設置成系統(tǒng)內(nèi)部的一個計數(shù)器，每完成 一次計算(以計算時間最大值為一個計算周期)自加一個時間單位(計算周期)。 T對應于電3各中的time — count端口 。
所述濾波增益值計算模塊fc3如圖5所示。經(jīng)過方程分解
、
W (13) 其中/^ +0'1,《2— /p^
模塊fc3實現(xiàn)濾波增益值的計算。其實現(xiàn)過程中需要應用除法器。32位有 符號除法器是利用Xilinx ISE自帶的IP core來生成，使整個設計變得簡單易 行。ISE自帶的除法器為流水結構，在不考慮占用資源的前提下，可在每個時鐘 周期輸出計算值。但考慮到芯片資源有限，可適當調整為四個時鐘周期完成一 次除法計算，在利用coregenerator生成除法器時選擇適合的計算周期即可。
所述預測誤差方差計算模塊fc4和濾波誤差方差計算模塊fc5如圖6所示。 經(jīng)過方程分解
/" 1)=哮/ A: - l萍-l)0)r (yt / ;t -1) +歸- (t -1)
=V" 、
、/^1 /7〖22」 (14 )
其中."ll+、+n^2+取2) +石 /^,A2+取2+^
J73 r2
6 = Al +取2 + ^ ^12 =尸22 +麗)=
(15)
其中
Pl2 = /^12 -&i/^12 "22 =^22—
模塊fc4和fc5分別完成預測誤差方差和濾波誤差方差的計算中的截取問
題，和前面名又述相同。
戶斤述4大態(tài)4空制才莫塊statemachine ^口 7戶斤示。4夫態(tài)4空制才莫塊(s tatemachine ) 主要實現(xiàn)對整個卡爾曼的時序控制和內(nèi)部信號T的生成。根據(jù)卡爾曼濾波狀態(tài) 圖，共需要六個計算信號對應六個狀態(tài)，每個狀態(tài)的跳轉都由s-l-comput到 S-6_comput信號控制執(zhí)行，在新的狀態(tài)信號來臨之前，所有狀態(tài)均在等待。當 完成一個系統(tǒng)周期的計算之后，statemachine模塊向上位機發(fā)出REQUEST信號， 接收下一個采樣值循環(huán)計算。狀態(tài)控制模塊的狀態(tài)分配如圖8所示。
這六個模塊的連接方式是按照端口直接的線連接。信號在各模塊中由狀態(tài) 控制模塊控制順序處理，不需要加入其他先入先出模塊(FIFO)等。
其信號走向是如下
St印l:在READY狀態(tài)，給所有參量賦初值，同時初始化所有模塊，在valid 脈沖上升沿讀取上位機采樣信號y,送進模塊fc4等待下一個狀態(tài)的觸發(fā)信號；
Step2:當狀態(tài)s-l的s-l-compute觸發(fā)信號上升沿來臨，進入s — l狀態(tài)， 計算模塊fc4和fc2;
St印3:當s —2-compute的信號上升沿來臨，完成狀態(tài)s —2的模塊fc5的計
算；
Step4:當s-3-compute的信號來臨，對比P-ll和P-21新值和寄存器寄存 的兩舊值，當它們的差為一個設定小量的值或以下時，則滿足判斷條件，跳轉 至狀態(tài)s-4，否則跳轉至狀態(tài)S-5，重新遞歸計算新的P-11和P-21值，直到遞 歸到一個穩(wěn)定的值；
Step5:由s-6 — compute信號控制，根據(jù)穩(wěn)定的kl、 k2值計算一次xl和x2 并輸出，同時跳轉到狀態(tài)s — 7;
Step6:計算xl、 x2完成后，發(fā)出REQUEST信號，重新讀取上位機新的采樣 信號y,跳轉至s —l狀態(tài)。
權利要求1、一種基于現(xiàn)場可編程門陣列的卡爾曼濾波器，它是一個硬件集成電路，其特征在于該電路主要由六個模塊組成，它們是模塊fc1，用來完成最佳濾波值計算；模塊fc2，計算一步狀態(tài)預測值；模塊fc3，實現(xiàn)濾波增益值的計算；模塊fc4，完成預測誤差方差的計算；模塊fc5，完成濾波誤差方差的計算；狀態(tài)控制模塊，控制整個增益計算回路和濾波計算回路的時序和狀態(tài)跳轉；這六個模塊的連接方式是按照端口直接的線連接，信號在各模塊中由狀態(tài)控制模塊控制順序處理，不需要加入其他先入先出模塊即FIFO；其信號走向是如下Step1在READY狀態(tài)，給所有參量賦初值，同時初始化所有模塊，在valid脈沖上升沿讀取上位機采樣信號y，送進模塊fc4等待下一個狀態(tài)的觸發(fā)信號；Step2當狀態(tài)s_1的s_1_compute觸發(fā)信號上升沿來臨，進入s_1狀態(tài)，計算模塊fc4和fc2；Step3當s_2_compute的信號上升沿來臨，完成狀態(tài)s_2的模塊fc5的計算；Step4當s_3_compute的信號來臨，對比P_11和P_21新值和寄存器寄存的兩舊值，當它們的差為一個設定小量的值或以下時，則滿足判斷條件，跳轉至狀態(tài)s_4，否則跳轉至狀態(tài)s_5，重新遞歸計算新的P_11和P_21值，直到遞歸到一個穩(wěn)定的值；Step5由s_6_compute信號控制，根據(jù)穩(wěn)定的k1、k2值計算一次x1和x2并輸出，同時跳轉到狀態(tài)s_7；Step6計算x1、x2完成后，發(fā)出REQUEST信號，重新讀取上位機新的采樣信號y，跳轉至s_1狀態(tài)。
專利摘要本實用新型一種基于現(xiàn)場可編程門陣列的卡爾曼濾波器，它是一個硬件集成電路，該電路主要由六個模塊組成，它們是模塊fc1，用來完成最佳濾波值計算；模塊fc2，計算一步狀態(tài)預測值；模塊fc3，實現(xiàn)濾波增益值的計算；模塊fc4，完成預測誤差方差的計算；模塊fc5，完成濾波誤差方差的計算；狀態(tài)控制模塊，控制整個增益計算回路和濾波計算回路的時序和狀態(tài)跳轉。這六個模塊的連接方式是按照端口直接的線連接。信號在各模塊中由狀態(tài)控制模塊控制順序處理，不需要加入其他先入先出模塊(FIFO)等?？柭鼮V波器的硬件集成電路算法成熟，結構簡單，易于實現(xiàn)。由于其配置靈活、資源消耗小，可以在航空航天，空間探測器、車載導航系統(tǒng)、電力電機檢測系統(tǒng)等設備上廣泛使用。
文檔編號H03H21/00GK201365232SQ20092010508
公開日2009年12月16日 申請日期2009年1月15日 優(yōu)先權日2009年1月15日
發(fā)明者翔 王, 睿 郭 申請人:北京航空航天大學