專利名稱：基于fpga的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法
技術(shù)領域：
本發(fā)明屬于信號處理技術(shù)領域，雷達數(shù)據(jù)處理方法，可用于目標跟蹤，特別是一種 基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法。
背景技術(shù)：
現(xiàn)代雷達系統(tǒng)一般都由以下兩個部分組成，即雷達信號處理部分和雷達數(shù)據(jù)處 理部分。雷達信號處理器作為第一次處理，將處理后的信號送入雷達數(shù)據(jù)處理器作第二次 處理。目標跟蹤作為雷達數(shù)據(jù)處理的功能之一，已經(jīng)成為現(xiàn)代雷達跟蹤系統(tǒng)中一個重要組 成部分。在實際雷達目標跟蹤系統(tǒng)中，目標動態(tài)模型通常在笛卡爾坐標系中建模，而雷達 量測一般卻在極/球坐標系中得到。這樣雷達目標跟蹤就成為一個非線性估計問題。解決 這一問題的其中一種方法為EKF，即將雷達對目標運動狀態(tài)的非線性量測方程圍繞目標運 動狀態(tài)的一步預測值進行Taylor級數(shù)展開，對其取一階進行截斷，以得到雷達量測的預測 值及新息的協(xié)方差陣。但是這種近似可能會在目標運動狀態(tài)估計及相應誤差協(xié)方差陣的計 算上引入大的誤差，從而導致濾波器發(fā)散。解決這一問題的另一類常用方法就是轉(zhuǎn)換量測 卡爾曼濾波，即先將雷達量測通過坐標變換表示成笛卡爾坐標系中量測的偽線性形式，然 后估計轉(zhuǎn)換量測誤差的前兩階矩并基于卡爾曼濾波完成目標跟蹤。去偏轉(zhuǎn)換量測Kalman濾波器(DCMKF)在雷達目標跟蹤系統(tǒng)中有著極其廣泛的應 用，但是由于其算法包含大量的矩陣加、減、乘、求逆等運算，計算量較大。傳統(tǒng)的使用方法 是利用數(shù)字信號處理器(DSP)實現(xiàn)DCMKF，DSP芯片是基于軟件可編程的，依靠一條一條軟 件指令去串行執(zhí)行，因此不可避免的問題就是功耗大，處理能力受到主頻的限制等不足，因 此采用傳統(tǒng)的軟件方法實現(xiàn)DCMKF難以保證雷達目標跟蹤系統(tǒng)的實時性。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方 法，以充分利用FPGA內(nèi)部越來越多地內(nèi)嵌了乘法器以及支持大規(guī)模并行運算的特點，在保 證濾波精度的同時提高運算速度。實現(xiàn)本發(fā)明目的的技術(shù)解決方案為一種基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波 器的設計方法，首先設計基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波的系統(tǒng)，包括三角函數(shù)模 塊、一步預測模塊、預測誤差協(xié)方差模塊、增益矩陣模塊、狀態(tài)更新模塊、濾波誤差協(xié)方差模 塊和FIFO模塊，其中增益矩陣模塊包含平均真實協(xié)方差子模塊和增益矩陣子模塊；狀態(tài)更 新模塊包含坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊；所述的 三角函數(shù)模塊、一步預測模塊、預測誤差協(xié)方差模塊、濾波誤差協(xié)方差模塊、平均真實協(xié)方 差子模塊、增益矩陣子模塊、坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更 新子模塊分別調(diào)用了浮點加、減、乘、除運算模塊；三角函數(shù)模塊三角函數(shù)模塊的輸入端口為目標的俯仰角和方位角、時鐘信號、復位信號和輸入使能信號；輸出端口為俯仰角與方位角的三角函數(shù)值和下一模塊的握手信 號；在三角函數(shù)模塊中，俯仰角與方位角的三角函數(shù)值計算一次，然后在坐標轉(zhuǎn)換模塊、量 測轉(zhuǎn)換的平均真實偏差模塊和量測轉(zhuǎn)換的平均真實協(xié)方差的模塊需要時讀取這些數(shù)據(jù)，犧 牲少量的寄存器來換取寶貴的乘法單元；一步預測模塊輸入端口為上一時刻目標的狀態(tài)估計值、時鐘信號、復位信號和輸 入使能信號；輸出端口為一步提前預測值和下一模塊的握手信號；其中復位信號的作用是 每處理完一幀數(shù)據(jù)，對內(nèi)部計數(shù)器進行復位；輸入使能信號的作用是提示并允許接收上一時 刻目標狀態(tài)估計值；握手信號的作用是使能狀態(tài)更新模塊、新息子模塊接收一步預測值；預測誤差協(xié)方差模塊輸入端口為上一時刻的濾波誤差協(xié)方差值、時鐘信號、復位 信號和輸入使能信號；輸出端口為預測誤差協(xié)方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協(xié)方差矩陣，即DCMKF 的平均真實協(xié)方差，然后計算增益矩陣的值，對于整個增益矩陣模塊而言，輸入端口為目標 的俯仰角與方位角的三角函數(shù)值、徑向距離、預測誤差協(xié)方差值、時鐘信號、復位信號和上 述各值的輸入使能信號；輸出端口為增益矩陣的值和狀態(tài)更新模塊、濾波誤差協(xié)方差模塊 的握手信號；濾波更新模塊在計算濾波更新值時，需要經(jīng)過球坐標到笛卡爾坐標的坐標變換、 去偏修正、新息計算、濾波更新四個過程；對于整個濾波更新模塊而言，輸入端口有目標的 徑向距離、目標的俯仰角與方位角的三角函數(shù)值、狀態(tài)一步預測值、增益矩陣的值、時鐘信 號、復位信號和上述各值的輸入使能信號；輸出端口為狀態(tài)估計值和握手信號，其中該握手 信號為LPM_FIF01模塊的寫使能信號；濾波誤差協(xié)方差模塊輸入端口為預測誤差協(xié)方差值、增益矩陣的值、時鐘信號、 復位信號和兩個數(shù)據(jù)輸入使能信號；輸出端口為濾波誤差協(xié)方差值和握手信號，其中該握 手信號為LPM_FIF02模塊的寫使能信號；FIFO模塊調(diào)用兩個FIFO模塊，其中LPM_FIF01模塊用于暫存每一次濾波更新 值，LPM_FIF02模塊用于暫存濾波誤差協(xié)方差的值，以便在下一時刻調(diào)用。本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(1)采用結(jié)構(gòu)層次化設計方法，底層模塊運 用VHDL輸入，頂層采用原理圖輸入方式。這種設計風格可提高代碼的可讀性，易于模塊劃 分，且方便在設計過程中進行仿真。( 在用FPGA實現(xiàn)DCMKF之前，先對算法進行預處理， 將算法分解為簡單的標量運算，便于代碼的實現(xiàn)，并且避免了稀疏矩陣中大量的0元素參 與乘加計算，可以節(jié)約大量的FPGA內(nèi)部資源。(3)從總體上給出了 CMKF的并行結(jié)構(gòu)，由于 DCMKF算法的特性，DCMKF的各運算模塊的內(nèi)部可以并行運算，但是各個運算模塊之間并不 能實現(xiàn)完全意義上的并行。設計時，在每個模塊之間加上握手信號，用于使能下一個運算模 塊接收數(shù)據(jù)并運算，各模塊便能按照相應的時序有條不紊的運行。(4)選用QUARTUS II軟 件自帶的浮點加減乘除運算模塊，提高了設計的性能，縮短設計的時間，大大簡化了浮點數(shù) 據(jù)通路的實現(xiàn)。下面結(jié)合附圖對本發(fā)明作進一步詳細描述。


圖1為基于FPGA的去偏轉(zhuǎn)換量測Kalman濾波器的結(jié)構(gòu)層次示意圖。
圖2為基于FPGA的去偏轉(zhuǎn)換量測Kalman濾波器的算法流程圖。圖3為狀態(tài)一步預測模塊結(jié)構(gòu)框圖。圖4為預測誤差協(xié)方差模塊結(jié)構(gòu)框圖。圖5為平均真實偏差模塊結(jié)構(gòu)框圖。圖6為增益矩陣模塊結(jié)構(gòu)框圖。圖7為狀態(tài)更新模塊結(jié)構(gòu)框圖。圖8為協(xié)方差更新模塊結(jié)構(gòu)框圖。
具體實施例方式本發(fā)明基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，首先設計基于FPGA 的去偏轉(zhuǎn)換量測卡爾曼濾波的系統(tǒng)，包括三角函數(shù)模塊、一步預測模塊、預測誤差協(xié)方差模 塊、增益矩陣模塊、狀態(tài)更新模塊、濾波誤差協(xié)方差模塊和FIFO模塊，其中增益矩陣模塊包 含平均真實協(xié)方差子模塊和增益矩陣子模塊；狀態(tài)更新模塊包含坐標轉(zhuǎn)換子模塊、平均真 實偏差子模塊、新息子模塊和狀態(tài)更新子模塊；所述的三角函數(shù)模塊、一步預測模塊、預測 誤差協(xié)方差模塊、濾波誤差協(xié)方差模塊、平均真實協(xié)方差子模塊、增益矩陣子模塊、坐標轉(zhuǎn) 換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊分別調(diào)用了浮點加、減、乘、 除運算模塊；三角函數(shù)模塊三角函數(shù)模塊的輸入端口為目標的俯仰角和方位角、時鐘信號、復 位信號和輸入使能信號；輸出端口為俯仰角與方位角的三角函數(shù)值和下一模塊的握手信 號；在三角函數(shù)模塊中，俯仰角與方位角的三角函數(shù)值計算一次，然后在坐標轉(zhuǎn)換模塊、量 測轉(zhuǎn)換的平均真實偏差模塊和量測轉(zhuǎn)換的平均真實協(xié)方差的模塊需要時讀取這些數(shù)據(jù)，犧 牲少量的寄存器來換取寶貴的乘法單元；一步預測模塊輸入端口為上一時刻目標的狀態(tài)估計值、時鐘信號、復位信號和 輸入使能信號；輸出端口為一步提前預測值和下一模塊的握手信號；其中復位信號的作用 是每處理完一幀數(shù)據(jù)，對內(nèi)部計數(shù)器進行復位；輸入使能信號的作用是提示并允許接收上 一時刻目標狀態(tài)估計值；握手信號的作用是使能狀態(tài)更新模塊、新息子模塊接收一步預測 值；預測誤差協(xié)方差模塊輸入端口為上一時刻的濾波誤差協(xié)方差值、時鐘信號、復位 信號和輸入使能信號；輸出端口為預測誤差協(xié)方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協(xié)方差矩陣，即DCMKF 的平均真實協(xié)方差，然后計算增益矩陣的值，對于整個增益矩陣模塊而言，輸入端口為目標 的俯仰角與方位角的三角函數(shù)值、徑向距離、預測誤差協(xié)方差值、時鐘信號、復位信號和上 述各值的輸入使能信號；輸出端口為增益矩陣的值和狀態(tài)更新模塊、濾波誤差協(xié)方差模塊 的握手信號；濾波更新模塊在計算濾波更新值時，需要經(jīng)過球坐標到笛卡爾坐標的坐標變換、 去偏修正、新息計算、濾波更新四個過程；對于整個濾波更新模塊而言，輸入端口有目標的 徑向距離、目標的俯仰角與方位角的三角函數(shù)值、狀態(tài)一步預測值、增益矩陣的值、時鐘信 號、復位信號和上述各值的輸入使能信號；輸出端口為狀態(tài)估計值和握手信號，其中該握手 信號為LPM_FIF01模塊的寫使能信號；
濾波誤差協(xié)方差模塊輸入端口為預測誤差協(xié)方差值、增益矩陣的值、時鐘信號、 復位信號和兩個數(shù)據(jù)輸入使能信號；輸出端口為濾波誤差協(xié)方差值和握手信號，其中該握 手信號為LPM_FIF02模塊的寫使能信號；FIFO模塊調(diào)用兩個FIFO模塊，其中LPM_FIF01模塊用于暫存每一次濾波更新 值，LPM_FIF02模塊用于暫存濾波誤差協(xié)方差的值，以便在下一時刻調(diào)用。在用FPGA實現(xiàn)DCMKF之前，需要對算法進行預處理，將DCMKF算法由矩陣運算轉(zhuǎn) 化為簡單的標量力卩、減、乘、除運算。本發(fā)明采用結(jié)構(gòu)化設計思想，利用FPGA實現(xiàn)浮點DCMKF 的設計。在保證實時性的前提下，在模塊內(nèi)部對運算單元進行分時復用，模塊之間通過握手 信號協(xié)調(diào)工作，解決了采用數(shù)字信號處理器(DSP)軟件方法實現(xiàn)過程中存在的并行性和速 度問題，并保證了運算的精度。本發(fā)明基于FPGA的去偏轉(zhuǎn)換量測Kalman濾波并行實現(xiàn)的步驟如下由于DCMKF 算法的特性，DCMKF的各運算模塊的內(nèi)部可以并行運算，但是各個運算模塊之間并不能實現(xiàn) 完全意義上的并行，但有些模塊可以并行運算。其中平均真實偏差模塊，平均真實協(xié)方差模 塊，一步預測模塊，預測誤差協(xié)方差模塊，三角函數(shù)模塊可以并行運算。設計時，在每個模塊 之間加上握手信號，用于使能下一個運算模塊接收數(shù)據(jù)并運算，各模塊便能按照相應的時 序有條不紊的運行。因此，各個模塊之間不僅有相應的數(shù)據(jù)輸入輸出端口，還應該有握手信 號、時鐘信號和復位信號。(1)三角函數(shù)模塊，在求俯仰角和方位角三角函數(shù)值時，利用Taylor公式求出當 前時刻測量值的正弦值和余弦值；(2)平均真實偏差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的 噪聲方差求去轉(zhuǎn)換量測的平均真實偏差；(3)平均真實協(xié)方差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自 的噪聲方差求去轉(zhuǎn)換量測的平均真實協(xié)方差；(4) 一步預測模塊，將上一時刻的狀態(tài)估計值代入狀態(tài)預測方程，得到當前時刻的 狀態(tài)預測值；(5)預測誤差協(xié)方差模塊，利用上一時刻的濾波誤差協(xié)方差以及狀態(tài)噪聲協(xié)方差 代入預測誤差協(xié)方差方程，得到預測誤差協(xié)方差值；(6)增益模塊，利用預測誤差協(xié)方差值和量測轉(zhuǎn)換的平均真實協(xié)方差值代入增益 矩陣方程，得到增益；(7)新息模塊，利用去偏轉(zhuǎn)換量測后的量測值和狀態(tài)預測值，求取新息；(8)狀態(tài)更新值模塊，利用狀態(tài)預測值、增益以及新息，代入狀態(tài)更新方程，得到狀 態(tài)更新值；(9)濾波誤差協(xié)方差模塊，利用預測誤差協(xié)方差值和增益，代入濾波誤差協(xié)方差方 程，得到濾波誤差協(xié)方差值；(10)返回到步驟⑴進行下一時刻的計算。結(jié)合圖3，本發(fā)明的一步預測模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當系統(tǒng)接收 到當前時刻測量值且一步預測模塊接收完前一時刻狀態(tài)估計值之后，開始啟動數(shù)據(jù)分配模 塊，每個時鐘周期將相應的數(shù)據(jù)賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘 輸出一個狀態(tài)預測值；其中一步預測模塊共占用兩個浮點加法單元和兩個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊(LPM)中，浮點加、乘法器的周期參數(shù)分別設置為m、n 個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法運算時，需要將未參與運算的數(shù)據(jù)同樣設 置為η個時鐘的延時；在數(shù)據(jù)運算模塊的第二級浮點加法運算時，也需要將相應的上一級 運算結(jié)果設置為m個時鐘的延時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完九個 狀態(tài)值后，每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能運算模塊，這樣每 個數(shù)據(jù)在運算模塊中經(jīng)過n+m+m個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)， 其中 m > 7，η > 5。結(jié)合圖4，本發(fā)明的預測誤差協(xié)方差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當 系統(tǒng)接收到當前時刻測量值且預測誤差協(xié)方差模塊接收完前一時刻濾波誤差協(xié)方差值后， 開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將相應的數(shù)賦給對應的寄存器，經(jīng)過n+m+m+m+m個 時鐘周期之后，每個時鐘輸出一個預測誤差協(xié)方差值；其中預測誤差協(xié)方差模塊共占用九 個浮點加法單元和八個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊(LPM)中，浮 點加、乘法器的周期參數(shù)分別設置為m、η個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法 運算時，需要將未參與運算的數(shù)據(jù)同樣設置為η個時鐘的延時；在數(shù)據(jù)運算模塊的第三、 四級浮點加法運算時，也需要將相應的上一級運算結(jié)果設置為m個時鐘的延時；在輸入端 口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完前一時刻濾波誤差協(xié)方差值后，每個時鐘將相應 的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能運算模塊，這樣每個數(shù)據(jù)在運算模塊中經(jīng)過 n+m+m+m+m個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。結(jié)合圖5，本發(fā)明的平均真實偏差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當平均 真實偏差模塊接收完徑向距離和俯仰角與方位角的三角函數(shù)值之后，開始啟動數(shù)據(jù)分配模 塊，每個時鐘周期將相應的數(shù)賦給對應的寄存器，經(jīng)過η+η個時鐘周期之后，每個時鐘輸出 一個平均真實偏差值；其中平均真實偏差模塊共占用三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊(LPM)中，浮點乘法器的周期參數(shù)分別設置為η個時鐘周期；在輸入 端口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完徑向距離和俯仰角與方位角的三角函數(shù)值之 后，每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能數(shù)據(jù)運算模塊，這樣每個 數(shù)據(jù)在數(shù)據(jù)運算模塊中經(jīng)過η+η個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)， 其中 m > 7，η > 5。結(jié)合圖6，本發(fā)明的增益矩陣模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當增益矩 陣模塊接收完預測誤差協(xié)方差值之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將相應的數(shù) 賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出一個增益值；其中增益矩陣 模塊共占用兩個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模 塊(LPM)中，浮點加、乘法器的周期參數(shù)分別設置為m、n個時鐘周期；在數(shù)據(jù)運算模塊的第 二級浮點加法運算時，需要將相應的上一級運算結(jié)果設置為m個時鐘的延時；在輸入端口， 每個時鐘周期輸入一個數(shù)據(jù)，當接收完預測誤差協(xié)方差值和新息之后，每個時鐘將相應的 乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能運算模塊，這樣每個數(shù)據(jù)在數(shù)據(jù)運算模塊中經(jīng)過 n+m+m個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中7，η > 5。結(jié)合圖7本發(fā)明的狀態(tài)更新模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當狀態(tài)更新 模塊接收完狀態(tài)預測值和增益矩陣和新息之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將 相應的數(shù)賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出一個狀態(tài)估計值；其中狀態(tài)更新模塊共占用三個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的 參數(shù)可設置模塊(LPM)中，浮點加、乘法器的周期參數(shù)分別設置為m、η個時鐘周期；在數(shù) 據(jù)運算模塊的第一級浮點乘法運算時，需要將未參與運算的數(shù)據(jù)同樣設置為η個時鐘的延 時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完狀態(tài)預測值和增益矩陣和新息后， 每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能數(shù)據(jù)運算模塊，這樣每個數(shù)據(jù) 在數(shù)據(jù)運算模塊中經(jīng)過n+m+m個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其 中 m > 7，η > 5。結(jié)合圖8本發(fā)明的濾波誤差協(xié)方差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當濾 波誤差協(xié)方差模塊接收完預測誤差協(xié)方差值和增益矩陣值之后，開始啟動數(shù)據(jù)分配模塊， 每個時鐘周期將相應的數(shù)賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出 一個濾波誤差協(xié)方差值；其中濾波誤差協(xié)方差模塊共占用一個浮點加法單元、兩個浮點減 法單元和三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊(LPM)中，浮點加、減、 乘法器的周期參數(shù)分別設置為m、m、n個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法運算 時，需要將未參與運算的數(shù)據(jù)同樣設置為η個時鐘的延時；在輸入端口，每個時鐘周期輸入 一個數(shù)據(jù)，當接收完預測誤差協(xié)方差值和增益矩陣值后，每個時鐘將相應的乘數(shù)和被乘數(shù) 傳送到對應的寄存器并使能數(shù)據(jù)運算模塊，這樣每個數(shù)據(jù)在運算模塊中經(jīng)過n+m+m個時鐘 周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。實施例參照圖1，本發(fā)明基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器系統(tǒng)包括三角函數(shù)模 塊、一步預測模塊、預測誤差協(xié)方差模塊、增益矩陣模塊、狀態(tài)更新模塊、濾波誤差協(xié)方差模 塊和FIFO模塊。其中增益矩陣模塊包含平均真實協(xié)方差子模塊和增益矩陣子模塊；狀態(tài)更 新模塊包含坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊。其中在 各運算模塊分別調(diào)用了浮點加、減、乘、除運算模塊。本實施例采用雷達對近程目標進行跟蹤，其中雷達數(shù)據(jù)輸出周期T為0. 8192mSo 選擇Singer加速度模型作為目標的動態(tài)模型。系統(tǒng)狀態(tài)方程為Xk+1 = Φ Xk+TkWk(1)觀測方程為Zk = HkXk+Vk(2)其中又^⑷，凡，^九，‘‘九忑廣為系統(tǒng)的狀態(tài)向量，包括目標在乂軸彳軸、 Z軸方向上的坐標位置、速度和加速度；Φ為狀態(tài)轉(zhuǎn)移矩陣；rk為噪聲矩陣；Wk為系統(tǒng)的狀 態(tài)噪聲為系統(tǒng)量測值；Hk為量測矩陣；Vk為量測噪聲。
權(quán)利要求
1.一種基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特征在于首先設計基 于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波的系統(tǒng)，包括三角函數(shù)模塊、一步預測模塊、預測誤差 協(xié)方差模塊、增益矩陣模塊、狀態(tài)更新模塊、濾波誤差協(xié)方差模塊和FIFO模塊，其中增益矩 陣模塊包含平均真實協(xié)方差子模塊和增益矩陣子模塊；狀態(tài)更新模塊包含坐標轉(zhuǎn)換子模 塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊；所述的三角函數(shù)模塊、一步預測 模塊、預測誤差協(xié)方差模塊、濾波誤差協(xié)方差模塊、平均真實協(xié)方差子模塊、增益矩陣子模 塊、坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊分別調(diào)用了浮點 加、減、乘、除運算模塊；三角函數(shù)模塊三角函數(shù)模塊的輸入端口為目標的俯仰角和方位角、時鐘信號、復位信 號和輸入使能信號；輸出端口為俯仰角與方位角的三角函數(shù)值和下一模塊的握手信號；在 三角函數(shù)模塊中，俯仰角與方位角的三角函數(shù)值計算一次，然后在坐標轉(zhuǎn)換模塊、量測轉(zhuǎn)換 的平均真實偏差模塊和量測轉(zhuǎn)換的平均真實協(xié)方差的模塊需要時讀取這些數(shù)據(jù)，犧牲少量 的寄存器來換取寶貴的乘法單元；一步預測模塊輸入端口為上一時刻目標的狀態(tài)估計值、時鐘信號、復位信號和輸入使 能信號；輸出端口為一步提前預測值和下一模塊的握手信號；其中復位信號的作用是每處 理完一幀數(shù)據(jù)，對內(nèi)部計數(shù)器進行復位；輸入使能信號的作用是提示并允許接收上一時刻 目標狀態(tài)估計值；握手信號的作用是使能狀態(tài)更新模塊、新息子模塊接收一步預測值；預測誤差協(xié)方差模塊輸入端口為上一時刻的濾波誤差協(xié)方差值、時鐘信號、復位信號 和輸入使能信號；輸出端口為預測誤差協(xié)方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協(xié)方差矩陣，即DCMKF的平 均真實協(xié)方差，然后計算增益矩陣的值，對于整個增益矩陣模塊而言，輸入端口為目標的俯 仰角與方位角的三角函數(shù)值、徑向距離、預測誤差協(xié)方差值、時鐘信號、復位信號和上述各 值的輸入使能信號；輸出端口為增益矩陣的值和狀態(tài)更新模塊、濾波誤差協(xié)方差模塊的握 手信號；濾波更新模塊在計算濾波更新值時，需要經(jīng)過球坐標到笛卡爾坐標的坐標變換、去偏 修正、新息計算、濾波更新四個過程；對于整個濾波更新模塊而言，輸入端口有目標的徑向 距離、目標的俯仰角與方位角的三角函數(shù)值、狀態(tài)一步預測值、增益矩陣的值、時鐘信號、復 位信號和上述各值的輸入使能信號；輸出端口為狀態(tài)估計值和握手信號，其中該握手信號 為LPM_FIF01模塊的寫使能信號；濾波誤差協(xié)方差模塊輸入端口為預測誤差協(xié)方差值、增益矩陣的值、時鐘信號、復位 信號和兩個數(shù)據(jù)輸入使能信號；輸出端口為濾波誤差協(xié)方差值和握手信號，其中該握手信 號為LPM_FIF02模塊的寫使能信號；FIFO模塊調(diào)用兩個FIFO模塊，其中LPM_FIF01模塊用于暫存每一次濾波更新值， LPM_FIF02模塊用于暫存濾波誤差協(xié)方差的值，以便在下一時刻調(diào)用。
2.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特 征在于平均真實偏差模塊、平均真實協(xié)方差模塊、一步預測模塊、預測誤差協(xié)方差模塊和三 角函數(shù)模塊并行運算，設計時，在每個模塊之間加上握手信號，用于使能下一個運算模塊接 收數(shù)據(jù)并運算，各模塊便能按照相應的時序有條不紊的運行，即基于FPGA的去偏轉(zhuǎn)換量測 Kalman濾波并行實現(xiàn)的步驟如下(1)三角函數(shù)模塊，在求俯仰角和方位角三角函數(shù)值時，利用Taylor公式求出當前時 刻測量值的正弦值和余弦值；(2)平均真實偏差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的噪聲 方差求去轉(zhuǎn)換量測的平均真實偏差；(3)平均真實協(xié)方差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的噪 聲方差求去轉(zhuǎn)換量測的平均真實協(xié)方差；(4)一步預測模塊，將上一時刻的狀態(tài)估計值代入狀態(tài)預測方程，得到當前時刻的狀態(tài) 預測值；(5)預測誤差協(xié)方差模塊，利用上一時刻的濾波誤差協(xié)方差以及狀態(tài)噪聲協(xié)方差代入 預測誤差協(xié)方差方程，得到預測誤差協(xié)方差值；(6)增益模塊，利用預測誤差協(xié)方差值和量測轉(zhuǎn)換的平均真實協(xié)方差值代入增益矩陣 方程，得到增益；(7)新息模塊，利用去偏轉(zhuǎn)換量測后的量測值和狀態(tài)預測值，求取新息；(8)狀態(tài)更新值模塊，利用狀態(tài)預測值、增益以及新息，代入狀態(tài)更新方程，得到狀態(tài)更 新值；(9)濾波誤差協(xié)方差模塊，利用預測誤差協(xié)方差值和增益，代入濾波誤差協(xié)方差方程， 得到濾波誤差協(xié)方差值；(10)返回到步驟(1)進行下一時刻的計算。
3.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特 征在于一步預測模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當系統(tǒng)接收到當前時刻測量值且 一步預測模塊接收完前一時刻狀態(tài)估計值之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將 相應的數(shù)據(jù)賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出一個狀態(tài)預測 值；其中一步預測模塊共占用兩個浮點加法單元和兩個浮點乘法運算單元；在QUARTUS II 的參數(shù)可設置模塊中，浮點加、乘法器的周期參數(shù)分別設置為m、n個時鐘周期；在數(shù)據(jù)運算 模塊的第一級浮點乘法運算時，需要將未參與運算的數(shù)據(jù)同樣設置為η個時鐘的延時；在 數(shù)據(jù)運算模塊的第二級浮點加法運算時，也需要將相應的上一級運算結(jié)果設置為m個時鐘 的延時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完九個狀態(tài)值后，每個時鐘將相 應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能運算模塊，這樣每個數(shù)據(jù)在運算模塊中經(jīng)過 n+m+m個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。
4.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特 征在于預測誤差協(xié)方差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當系統(tǒng)接收到當前時刻測 量值且預測誤差協(xié)方差模塊接收完前一時刻濾波誤差協(xié)方差值后，開始啟動數(shù)據(jù)分配模 塊，每個時鐘周期將相應的數(shù)賦給對應的寄存器，經(jīng)過n+m+m+m+m個時鐘周期之后，每個時 鐘輸出一個預測誤差協(xié)方差值；其中預測誤差協(xié)方差模塊共占用九個浮點加法單元和八個 浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊中，浮點加、乘法器的周期參數(shù)分別 設置為m、η個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法運算時，需要將未參與運算的 數(shù)據(jù)同樣設置為η個時鐘的延時；在數(shù)據(jù)運算模塊的第三、四級浮點加法運算時，也需要將 相應的上一級運算結(jié)果設置為m個時鐘的延時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)， 當接收完前一時刻濾波誤差協(xié)方差值后，每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能運算模塊，這樣每個數(shù)據(jù)在運算模塊中經(jīng)過n+m+m+m+m個時鐘周期的運算延時 后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。
5.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特 征在于平均真實偏差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當平均真實偏差模塊接收完 徑向距離和俯仰角與方位角的三角函數(shù)值之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將 相應的數(shù)賦給對應的寄存器，經(jīng)過η+η個時鐘周期之后，每個時鐘輸出一個平均真實偏差 值；其中平均真實偏差模塊共占用三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模 塊中，浮點乘法器的周期參數(shù)分別設置為η個時鐘周期；在輸入端口，每個時鐘周期輸入一 個數(shù)據(jù)，當接收完徑向距離和俯仰角與方位角的三角函數(shù)值之后，每個時鐘將相應的乘數(shù) 和被乘數(shù)傳送到對應的寄存器并使能數(shù)據(jù)運算模塊，這樣每個數(shù)據(jù)在數(shù)據(jù)運算模塊中經(jīng)過 η+η個時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。
6.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特 征在于增益矩陣模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當增益矩陣模塊接收完預測誤差 協(xié)方差值之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將相應的數(shù)賦給對應的寄存器，經(jīng)過 n+m+m個時鐘周期之后，每個時鐘輸出一個增益值；其中增益矩陣模塊共占用兩個浮點加 法單元和三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊中，浮點加、乘法器的 周期參數(shù)分別設置為m、η個時鐘周期；在數(shù)據(jù)運算模塊的第二級浮點加法運算時，需要將 相應的上一級運算結(jié)果設置為m個時鐘的延時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)， 當接收完預測誤差協(xié)方差值和新息之后，每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄 存器并使能運算模塊，這樣每個數(shù)據(jù)在數(shù)據(jù)運算模塊中經(jīng)過n+m+m個時鐘周期的運算延時 后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。
7.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特征 在于狀態(tài)更新模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當狀態(tài)更新模塊接收完狀態(tài)預測值 和增益矩陣和新息之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將相應的數(shù)賦給對應的寄 存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出一個狀態(tài)估計值；其中狀態(tài)更新模塊共占 用三個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的參數(shù)可設置模塊中，浮點 加、乘法器的周期參數(shù)分別設置為m、η個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法運 算時，需要將未參與運算的數(shù)據(jù)同樣設置為η個時鐘的延時；在輸入端口，每個時鐘周期輸 入一個數(shù)據(jù)，當接收完狀態(tài)預測值和增益矩陣和新息后，每個時鐘將相應的乘數(shù)和被乘數(shù) 傳送到對應的寄存器并使能數(shù)據(jù)運算模塊，這樣每個數(shù)據(jù)在數(shù)據(jù)運算模塊中經(jīng)過n+m+m個 時鐘周期的運算延時后，每個時鐘周期輸出一個數(shù)據(jù)，其中m > 7，η > 5。
8.根據(jù)權(quán)利要求1所述的基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，其特征 在于濾波誤差協(xié)方差模塊分為數(shù)據(jù)分配模塊和數(shù)據(jù)運算模塊，當濾波誤差協(xié)方差模塊接收 完預測誤差協(xié)方差值和增益矩陣值之后，開始啟動數(shù)據(jù)分配模塊，每個時鐘周期將相應的 數(shù)賦給對應的寄存器，經(jīng)過n+m+m個時鐘周期之后，每個時鐘輸出一個濾波誤差協(xié)方差值； 其中濾波誤差協(xié)方差模塊共占用一個浮點加法單元、兩個浮點減法單元和三個浮點乘法運 算單元；在QUARTUS II的參數(shù)可設置模塊中，浮點加、減、乘法器的周期參數(shù)分別設置為m、 m、η個時鐘周期；在數(shù)據(jù)運算模塊的第一級浮點乘法運算時，需要將未參與運算的數(shù)據(jù)同 樣設置為η個時鐘的延時；在輸入端口，每個時鐘周期輸入一個數(shù)據(jù)，當接收完預測誤差協(xié)方差值和增益矩陣值后，每個時鐘將相應的乘數(shù)和被乘數(shù)傳送到對應的寄存器并使能數(shù)據(jù) 運算模塊，這樣每個數(shù)據(jù)在運算模塊中經(jīng)過n+m+m個時鐘周期的運算延時后，每個時鐘周 期輸出一個數(shù)據(jù)，其中m彡7，η彡5。
全文摘要
本發(fā)明公開了一種基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波器的設計方法，首先設計基于FPGA的去偏轉(zhuǎn)換量測卡爾曼濾波的系統(tǒng)，增益矩陣模塊包含平均真實協(xié)方差子模塊和增益矩陣子模塊；狀態(tài)更新模塊包含坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊；所述的三角函數(shù)模塊、一步預測模塊、預測誤差協(xié)方差模塊、濾波誤差協(xié)方差模塊、平均真實協(xié)方差子模塊、增益矩陣子模塊、坐標轉(zhuǎn)換子模塊、平均真實偏差子模塊、新息子模塊和狀態(tài)更新子模塊分別調(diào)用了浮點加、減、乘、除運算模塊。本發(fā)明采用結(jié)構(gòu)層次化設計，底層模塊運用VHDL輸入，頂層采用原理圖輸入方式，可提高代碼的可讀性，易于模塊劃分，且方便在設計過程中進行仿真。
文檔編號G01S13/66GK102064799SQ20101062272
公開日2011年5月18日 申請日期2010年12月31日 優(yōu)先權(quán)日2010年12月31日
發(fā)明者吳盤龍, 張捷, 朱建良, 杜國平, 王向民, 王寶寶, 王筱莉, 薄煜明, 鄒衛(wèi)軍, 陳帥 申請人:南京理工大學