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

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


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