基於fpga的去偏轉換量測卡爾曼濾波器的設計方法
2023-04-30 15:26:26
專利名稱：基於fpga的去偏轉換量測卡爾曼濾波器的設計方法
技術領域：
本發明屬於信號處理技術領域，雷達數據處理方法，可用於目標跟蹤，特別是一種 基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法。
背景技術：
現代雷達系統一般都由以下兩個部分組成，即雷達信號處理部分和雷達數據處 理部分。雷達信號處理器作為第一次處理，將處理後的信號送入雷達數據處理器作第二次 處理。目標跟蹤作為雷達數據處理的功能之一，已經成為現代雷達跟蹤系統中一個重要組 成部分。在實際雷達目標跟蹤系統中，目標動態模型通常在笛卡爾坐標系中建模，而雷達 量測一般卻在極/球坐標系中得到。這樣雷達目標跟蹤就成為一個非線性估計問題。解決 這一問題的其中一種方法為EKF，即將雷達對目標運動狀態的非線性量測方程圍繞目標運 動狀態的一步預測值進行Taylor級數展開，對其取一階進行截斷，以得到雷達量測的預測 值及新息的協方差陣。但是這種近似可能會在目標運動狀態估計及相應誤差協方差陣的計 算上引入大的誤差，從而導致濾波器發散。解決這一問題的另一類常用方法就是轉換量測 卡爾曼濾波，即先將雷達量測通過坐標變換表示成笛卡爾坐標系中量測的偽線性形式，然 後估計轉換量測誤差的前兩階矩並基於卡爾曼濾波完成目標跟蹤。去偏轉換量測Kalman濾波器(DCMKF)在雷達目標跟蹤系統中有著極其廣泛的應 用，但是由於其算法包含大量的矩陣加、減、乘、求逆等運算，計算量較大。傳統的使用方法 是利用數位訊號處理器(DSP)實現DCMKF，DSP晶片是基於軟體可編程的，依靠一條一條軟 件指令去串行執行，因此不可避免的問題就是功耗大，處理能力受到主頻的限制等不足，因 此採用傳統的軟體方法實現DCMKF難以保證雷達目標跟蹤系統的實時性。
發明內容
本發明的目的在於提供一種基於FPGA的去偏轉換量測卡爾曼濾波器的設計方 法，以充分利用FPGA內部越來越多地內嵌了乘法器以及支持大規模並行運算的特點，在保 證濾波精度的同時提高運算速度。實現本發明目的的技術解決方案為一種基於FPGA的去偏轉換量測卡爾曼濾波 器的設計方法，首先設計基於FPGA的去偏轉換量測卡爾曼濾波的系統，包括三角函數模 塊、一步預測模塊、預測誤差協方差模塊、增益矩陣模塊、狀態更新模塊、濾波誤差協方差模 塊和FIFO模塊，其中增益矩陣模塊包含平均真實協方差子模塊和增益矩陣子模塊；狀態更 新模塊包含坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊；所述的 三角函數模塊、一步預測模塊、預測誤差協方差模塊、濾波誤差協方差模塊、平均真實協方 差子模塊、增益矩陣子模塊、坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更 新子模塊分別調用了浮點加、減、乘、除運算模塊；三角函數模塊三角函數模塊的輸入埠為目標的俯仰角和方位角、時鐘信號、復位信號和輸入使能信號；輸出埠為俯仰角與方位角的三角函數值和下一模塊的握手信 號；在三角函數模塊中，俯仰角與方位角的三角函數值計算一次，然後在坐標轉換模塊、量 測轉換的平均真實偏差模塊和量測轉換的平均真實協方差的模塊需要時讀取這些數據，犧 牲少量的寄存器來換取寶貴的乘法單元；一步預測模塊輸入埠為上一時刻目標的狀態估計值、時鐘信號、復位信號和輸 入使能信號；輸出埠為一步提前預測值和下一模塊的握手信號；其中復位信號的作用是 每處理完一幀數據，對內部計數器進行復位；輸入使能信號的作用是提示並允許接收上一時 刻目標狀態估計值；握手信號的作用是使能狀態更新模塊、新息子模塊接收一步預測值；預測誤差協方差模塊輸入埠為上一時刻的濾波誤差協方差值、時鐘信號、復位 信號和輸入使能信號；輸出埠為預測誤差協方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協方差矩陣，即DCMKF 的平均真實協方差，然後計算增益矩陣的值，對於整個增益矩陣模塊而言，輸入埠為目標 的俯仰角與方位角的三角函數值、徑向距離、預測誤差協方差值、時鐘信號、復位信號和上 述各值的輸入使能信號；輸出埠為增益矩陣的值和狀態更新模塊、濾波誤差協方差模塊 的握手信號；濾波更新模塊在計算濾波更新值時，需要經過球坐標到笛卡爾坐標的坐標變換、 去偏修正、新息計算、濾波更新四個過程；對於整個濾波更新模塊而言，輸入埠有目標的 徑向距離、目標的俯仰角與方位角的三角函數值、狀態一步預測值、增益矩陣的值、時鐘信 號、復位信號和上述各值的輸入使能信號；輸出埠為狀態估計值和握手信號，其中該握手 信號為LPM_FIF01模塊的寫使能信號；濾波誤差協方差模塊輸入埠為預測誤差協方差值、增益矩陣的值、時鐘信號、 復位信號和兩個數據輸入使能信號；輸出埠為濾波誤差協方差值和握手信號，其中該握 手信號為LPM_FIF02模塊的寫使能信號；FIFO模塊調用兩個FIFO模塊，其中LPM_FIF01模塊用於暫存每一次濾波更新 值，LPM_FIF02模塊用於暫存濾波誤差協方差的值，以便在下一時刻調用。本發明與現有技術相比，其顯著優點(1)採用結構層次化設計方法，底層模塊運 用VHDL輸入，頂層採用原理圖輸入方式。這種設計風格可提高代碼的可讀性，易於模塊劃 分，且方便在設計過程中進行仿真。( 在用FPGA實現DCMKF之前，先對算法進行預處理， 將算法分解為簡單的標量運算，便於代碼的實現，並且避免了稀疏矩陣中大量的0元素參 與乘加計算，可以節約大量的FPGA內部資源。(3)從總體上給出了 CMKF的並行結構，由於 DCMKF算法的特性，DCMKF的各運算模塊的內部可以並行運算，但是各個運算模塊之間並不 能實現完全意義上的並行。設計時，在每個模塊之間加上握手信號，用於使能下一個運算模 塊接收數據並運算，各模塊便能按照相應的時序有條不紊的運行。(4)選用QUARTUS II軟 件自帶的浮點加減乘除運算模塊，提高了設計的性能，縮短設計的時間，大大簡化了浮點數 據通路的實現。下面結合附圖對本發明作進一步詳細描述。
圖1為基於FPGA的去偏轉換量測Kalman濾波器的結構層次示意圖。
圖2為基於FPGA的去偏轉換量測Kalman濾波器的算法流程圖。圖3為狀態一步預測模塊結構框圖。圖4為預測誤差協方差模塊結構框圖。圖5為平均真實偏差模塊結構框圖。圖6為增益矩陣模塊結構框圖。圖7為狀態更新模塊結構框圖。圖8為協方差更新模塊結構框圖。
具體實施例方式本發明基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，首先設計基於FPGA 的去偏轉換量測卡爾曼濾波的系統，包括三角函數模塊、一步預測模塊、預測誤差協方差模 塊、增益矩陣模塊、狀態更新模塊、濾波誤差協方差模塊和FIFO模塊，其中增益矩陣模塊包 含平均真實協方差子模塊和增益矩陣子模塊；狀態更新模塊包含坐標轉換子模塊、平均真 實偏差子模塊、新息子模塊和狀態更新子模塊；所述的三角函數模塊、一步預測模塊、預測 誤差協方差模塊、濾波誤差協方差模塊、平均真實協方差子模塊、增益矩陣子模塊、坐標轉 換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊分別調用了浮點加、減、乘、 除運算模塊；三角函數模塊三角函數模塊的輸入埠為目標的俯仰角和方位角、時鐘信號、復 位信號和輸入使能信號；輸出埠為俯仰角與方位角的三角函數值和下一模塊的握手信 號；在三角函數模塊中，俯仰角與方位角的三角函數值計算一次，然後在坐標轉換模塊、量 測轉換的平均真實偏差模塊和量測轉換的平均真實協方差的模塊需要時讀取這些數據，犧 牲少量的寄存器來換取寶貴的乘法單元；一步預測模塊輸入埠為上一時刻目標的狀態估計值、時鐘信號、復位信號和 輸入使能信號；輸出埠為一步提前預測值和下一模塊的握手信號；其中復位信號的作用 是每處理完一幀數據，對內部計數器進行復位；輸入使能信號的作用是提示並允許接收上 一時刻目標狀態估計值；握手信號的作用是使能狀態更新模塊、新息子模塊接收一步預測 值；預測誤差協方差模塊輸入埠為上一時刻的濾波誤差協方差值、時鐘信號、復位 信號和輸入使能信號；輸出埠為預測誤差協方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協方差矩陣，即DCMKF 的平均真實協方差，然後計算增益矩陣的值，對於整個增益矩陣模塊而言，輸入埠為目標 的俯仰角與方位角的三角函數值、徑向距離、預測誤差協方差值、時鐘信號、復位信號和上 述各值的輸入使能信號；輸出埠為增益矩陣的值和狀態更新模塊、濾波誤差協方差模塊 的握手信號；濾波更新模塊在計算濾波更新值時，需要經過球坐標到笛卡爾坐標的坐標變換、 去偏修正、新息計算、濾波更新四個過程；對於整個濾波更新模塊而言，輸入埠有目標的 徑向距離、目標的俯仰角與方位角的三角函數值、狀態一步預測值、增益矩陣的值、時鐘信 號、復位信號和上述各值的輸入使能信號；輸出埠為狀態估計值和握手信號，其中該握手 信號為LPM_FIF01模塊的寫使能信號；
濾波誤差協方差模塊輸入埠為預測誤差協方差值、增益矩陣的值、時鐘信號、 復位信號和兩個數據輸入使能信號；輸出埠為濾波誤差協方差值和握手信號，其中該握 手信號為LPM_FIF02模塊的寫使能信號；FIFO模塊調用兩個FIFO模塊，其中LPM_FIF01模塊用於暫存每一次濾波更新 值，LPM_FIF02模塊用於暫存濾波誤差協方差的值，以便在下一時刻調用。在用FPGA實現DCMKF之前，需要對算法進行預處理，將DCMKF算法由矩陣運算轉 化為簡單的標量力卩、減、乘、除運算。本發明採用結構化設計思想，利用FPGA實現浮點DCMKF 的設計。在保證實時性的前提下，在模塊內部對運算單元進行分時復用，模塊之間通過握手 信號協調工作，解決了採用數位訊號處理器(DSP)軟體方法實現過程中存在的並行性和速 度問題，並保證了運算的精度。本發明基於FPGA的去偏轉換量測Kalman濾波並行實現的步驟如下由於DCMKF 算法的特性，DCMKF的各運算模塊的內部可以並行運算，但是各個運算模塊之間並不能實現 完全意義上的並行，但有些模塊可以並行運算。其中平均真實偏差模塊，平均真實協方差模 塊，一步預測模塊，預測誤差協方差模塊，三角函數模塊可以並行運算。設計時，在每個模塊 之間加上握手信號，用於使能下一個運算模塊接收數據並運算，各模塊便能按照相應的時 序有條不紊的運行。因此，各個模塊之間不僅有相應的數據輸入輸出埠，還應該有握手信 號、時鐘信號和復位信號。(1)三角函數模塊，在求俯仰角和方位角三角函數值時，利用Taylor公式求出當 前時刻測量值的正弦值和餘弦值；(2)平均真實偏差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的 噪聲方差求去轉換量測的平均真實偏差；(3)平均真實協方差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自 的噪聲方差求去轉換量測的平均真實協方差；(4) 一步預測模塊，將上一時刻的狀態估計值代入狀態預測方程，得到當前時刻的 狀態預測值；(5)預測誤差協方差模塊，利用上一時刻的濾波誤差協方差以及狀態噪聲協方差 代入預測誤差協方差方程，得到預測誤差協方差值；(6)增益模塊，利用預測誤差協方差值和量測轉換的平均真實協方差值代入增益 矩陣方程，得到增益；(7)新息模塊，利用去偏轉換量測後的量測值和狀態預測值，求取新息；(8)狀態更新值模塊，利用狀態預測值、增益以及新息，代入狀態更新方程，得到狀 態更新值；(9)濾波誤差協方差模塊，利用預測誤差協方差值和增益，代入濾波誤差協方差方 程，得到濾波誤差協方差值；(10)返回到步驟⑴進行下一時刻的計算。結合圖3，本發明的一步預測模塊分為數據分配模塊和數據運算模塊，當系統接收 到當前時刻測量值且一步預測模塊接收完前一時刻狀態估計值之後，開始啟動數據分配模 塊，每個時鐘周期將相應的數據賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘 輸出一個狀態預測值；其中一步預測模塊共佔用兩個浮點加法單元和兩個浮點乘法運算單元；在QUARTUS II的參數可設置模塊(LPM)中，浮點加、乘法器的周期參數分別設置為m、n 個時鐘周期；在數據運算模塊的第一級浮點乘法運算時，需要將未參與運算的數據同樣設 置為η個時鐘的延時；在數據運算模塊的第二級浮點加法運算時，也需要將相應的上一級 運算結果設置為m個時鐘的延時；在輸入埠，每個時鐘周期輸入一個數據，當接收完九個 狀態值後，每個時鐘將相應的乘數和被乘數傳送到對應的寄存器並使能運算模塊，這樣每 個數據在運算模塊中經過n+m+m個時鐘周期的運算延時後，每個時鐘周期輸出一個數據， 其中 m > 7，η > 5。結合圖4，本發明的預測誤差協方差模塊分為數據分配模塊和數據運算模塊，當 系統接收到當前時刻測量值且預測誤差協方差模塊接收完前一時刻濾波誤差協方差值後， 開始啟動數據分配模塊，每個時鐘周期將相應的數賦給對應的寄存器，經過n+m+m+m+m個 時鐘周期之後，每個時鐘輸出一個預測誤差協方差值；其中預測誤差協方差模塊共佔用九 個浮點加法單元和八個浮點乘法運算單元；在QUARTUS II的參數可設置模塊(LPM)中，浮 點加、乘法器的周期參數分別設置為m、η個時鐘周期；在數據運算模塊的第一級浮點乘法 運算時，需要將未參與運算的數據同樣設置為η個時鐘的延時；在數據運算模塊的第三、 四級浮點加法運算時，也需要將相應的上一級運算結果設置為m個時鐘的延時；在輸入端 口，每個時鐘周期輸入一個數據，當接收完前一時刻濾波誤差協方差值後，每個時鐘將相應 的乘數和被乘數傳送到對應的寄存器並使能運算模塊，這樣每個數據在運算模塊中經過 n+m+m+m+m個時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。結合圖5，本發明的平均真實偏差模塊分為數據分配模塊和數據運算模塊，當平均 真實偏差模塊接收完徑向距離和俯仰角與方位角的三角函數值之後，開始啟動數據分配模 塊，每個時鐘周期將相應的數賦給對應的寄存器，經過η+η個時鐘周期之後，每個時鐘輸出 一個平均真實偏差值；其中平均真實偏差模塊共佔用三個浮點乘法運算單元；在QUARTUS II的參數可設置模塊(LPM)中，浮點乘法器的周期參數分別設置為η個時鐘周期；在輸入 埠，每個時鐘周期輸入一個數據，當接收完徑向距離和俯仰角與方位角的三角函數值之 後，每個時鐘將相應的乘數和被乘數傳送到對應的寄存器並使能數據運算模塊，這樣每個 數據在數據運算模塊中經過η+η個時鐘周期的運算延時後，每個時鐘周期輸出一個數據， 其中 m > 7，η > 5。結合圖6，本發明的增益矩陣模塊分為數據分配模塊和數據運算模塊，當增益矩 陣模塊接收完預測誤差協方差值之後，開始啟動數據分配模塊，每個時鐘周期將相應的數 賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘輸出一個增益值；其中增益矩陣 模塊共佔用兩個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的參數可設置模 塊(LPM)中，浮點加、乘法器的周期參數分別設置為m、n個時鐘周期；在數據運算模塊的第 二級浮點加法運算時，需要將相應的上一級運算結果設置為m個時鐘的延時；在輸入埠， 每個時鐘周期輸入一個數據，當接收完預測誤差協方差值和新息之後，每個時鐘將相應的 乘數和被乘數傳送到對應的寄存器並使能運算模塊，這樣每個數據在數據運算模塊中經過 n+m+m個時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其中7，η > 5。結合圖7本發明的狀態更新模塊分為數據分配模塊和數據運算模塊，當狀態更新 模塊接收完狀態預測值和增益矩陣和新息之後，開始啟動數據分配模塊，每個時鐘周期將 相應的數賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘輸出一個狀態估計值；其中狀態更新模塊共佔用三個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的 參數可設置模塊(LPM)中，浮點加、乘法器的周期參數分別設置為m、η個時鐘周期；在數 據運算模塊的第一級浮點乘法運算時，需要將未參與運算的數據同樣設置為η個時鐘的延 時；在輸入埠，每個時鐘周期輸入一個數據，當接收完狀態預測值和增益矩陣和新息後， 每個時鐘將相應的乘數和被乘數傳送到對應的寄存器並使能數據運算模塊，這樣每個數據 在數據運算模塊中經過n+m+m個時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其 中 m > 7，η > 5。結合圖8本發明的濾波誤差協方差模塊分為數據分配模塊和數據運算模塊，當濾 波誤差協方差模塊接收完預測誤差協方差值和增益矩陣值之後，開始啟動數據分配模塊， 每個時鐘周期將相應的數賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘輸出 一個濾波誤差協方差值；其中濾波誤差協方差模塊共佔用一個浮點加法單元、兩個浮點減 法單元和三個浮點乘法運算單元；在QUARTUS II的參數可設置模塊(LPM)中，浮點加、減、 乘法器的周期參數分別設置為m、m、n個時鐘周期；在數據運算模塊的第一級浮點乘法運算 時，需要將未參與運算的數據同樣設置為η個時鐘的延時；在輸入埠，每個時鐘周期輸入 一個數據，當接收完預測誤差協方差值和增益矩陣值後，每個時鐘將相應的乘數和被乘數 傳送到對應的寄存器並使能數據運算模塊，這樣每個數據在運算模塊中經過n+m+m個時鐘 周期的運算延時後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。實施例參照圖1，本發明基於FPGA的去偏轉換量測卡爾曼濾波器系統包括三角函數模 塊、一步預測模塊、預測誤差協方差模塊、增益矩陣模塊、狀態更新模塊、濾波誤差協方差模 塊和FIFO模塊。其中增益矩陣模塊包含平均真實協方差子模塊和增益矩陣子模塊；狀態更 新模塊包含坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊。其中在 各運算模塊分別調用了浮點加、減、乘、除運算模塊。本實施例採用雷達對近程目標進行跟蹤，其中雷達數據輸出周期T為0. 8192mSo 選擇Singer加速度模型作為目標的動態模型。系統狀態方程為Xk+1 = Φ Xk+TkWk(1)觀測方程為Zk = HkXk+Vk(2)其中又^⑷，凡，^九，『『九忑廣為系統的狀態向量，包括目標在乂軸彳軸、 Z軸方向上的坐標位置、速度和加速度；Φ為狀態轉移矩陣；rk為噪聲矩陣；Wk為系統的狀 態噪聲為系統量測值；Hk為量測矩陣；Vk為量測噪聲。
權利要求
1.一種基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特徵在於首先設計基 於FPGA的去偏轉換量測卡爾曼濾波的系統，包括三角函數模塊、一步預測模塊、預測誤差 協方差模塊、增益矩陣模塊、狀態更新模塊、濾波誤差協方差模塊和FIFO模塊，其中增益矩 陣模塊包含平均真實協方差子模塊和增益矩陣子模塊；狀態更新模塊包含坐標轉換子模 塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊；所述的三角函數模塊、一步預測 模塊、預測誤差協方差模塊、濾波誤差協方差模塊、平均真實協方差子模塊、增益矩陣子模 塊、坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊分別調用了浮點 加、減、乘、除運算模塊；三角函數模塊三角函數模塊的輸入埠為目標的俯仰角和方位角、時鐘信號、復位信 號和輸入使能信號；輸出埠為俯仰角與方位角的三角函數值和下一模塊的握手信號；在 三角函數模塊中，俯仰角與方位角的三角函數值計算一次，然後在坐標轉換模塊、量測轉換 的平均真實偏差模塊和量測轉換的平均真實協方差的模塊需要時讀取這些數據，犧牲少量 的寄存器來換取寶貴的乘法單元；一步預測模塊輸入埠為上一時刻目標的狀態估計值、時鐘信號、復位信號和輸入使 能信號；輸出埠為一步提前預測值和下一模塊的握手信號；其中復位信號的作用是每處 理完一幀數據，對內部計數器進行復位；輸入使能信號的作用是提示並允許接收上一時刻 目標狀態估計值；握手信號的作用是使能狀態更新模塊、新息子模塊接收一步預測值；預測誤差協方差模塊輸入埠為上一時刻的濾波誤差協方差值、時鐘信號、復位信號 和輸入使能信號；輸出埠為預測誤差協方差值和下一模塊的握手信號；增益矩陣模塊在計算增益矩陣時，需要先求出量測噪聲的協方差矩陣，即DCMKF的平 均真實協方差，然後計算增益矩陣的值，對於整個增益矩陣模塊而言，輸入埠為目標的俯 仰角與方位角的三角函數值、徑向距離、預測誤差協方差值、時鐘信號、復位信號和上述各 值的輸入使能信號；輸出埠為增益矩陣的值和狀態更新模塊、濾波誤差協方差模塊的握 手信號；濾波更新模塊在計算濾波更新值時，需要經過球坐標到笛卡爾坐標的坐標變換、去偏 修正、新息計算、濾波更新四個過程；對於整個濾波更新模塊而言，輸入埠有目標的徑向 距離、目標的俯仰角與方位角的三角函數值、狀態一步預測值、增益矩陣的值、時鐘信號、復 位信號和上述各值的輸入使能信號；輸出埠為狀態估計值和握手信號，其中該握手信號 為LPM_FIF01模塊的寫使能信號；濾波誤差協方差模塊輸入埠為預測誤差協方差值、增益矩陣的值、時鐘信號、復位 信號和兩個數據輸入使能信號；輸出埠為濾波誤差協方差值和握手信號，其中該握手信 號為LPM_FIF02模塊的寫使能信號；FIFO模塊調用兩個FIFO模塊，其中LPM_FIF01模塊用於暫存每一次濾波更新值， LPM_FIF02模塊用於暫存濾波誤差協方差的值，以便在下一時刻調用。
2.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特 徵在於平均真實偏差模塊、平均真實協方差模塊、一步預測模塊、預測誤差協方差模塊和三 角函數模塊並行運算，設計時，在每個模塊之間加上握手信號，用於使能下一個運算模塊接 收數據並運算，各模塊便能按照相應的時序有條不紊的運行，即基於FPGA的去偏轉換量測 Kalman濾波並行實現的步驟如下(1)三角函數模塊，在求俯仰角和方位角三角函數值時，利用Taylor公式求出當前時 刻測量值的正弦值和餘弦值；(2)平均真實偏差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的噪聲 方差求去轉換量測的平均真實偏差；(3)平均真實協方差模塊，利用雷達測量到的徑向距離、俯仰角和方位角以及各自的噪 聲方差求去轉換量測的平均真實協方差；(4)一步預測模塊，將上一時刻的狀態估計值代入狀態預測方程，得到當前時刻的狀態 預測值；(5)預測誤差協方差模塊，利用上一時刻的濾波誤差協方差以及狀態噪聲協方差代入 預測誤差協方差方程，得到預測誤差協方差值；(6)增益模塊，利用預測誤差協方差值和量測轉換的平均真實協方差值代入增益矩陣 方程，得到增益；(7)新息模塊，利用去偏轉換量測後的量測值和狀態預測值，求取新息；(8)狀態更新值模塊，利用狀態預測值、增益以及新息，代入狀態更新方程，得到狀態更 新值；(9)濾波誤差協方差模塊，利用預測誤差協方差值和增益，代入濾波誤差協方差方程， 得到濾波誤差協方差值；(10)返回到步驟(1)進行下一時刻的計算。
3.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特 徵在於一步預測模塊分為數據分配模塊和數據運算模塊，當系統接收到當前時刻測量值且 一步預測模塊接收完前一時刻狀態估計值之後，開始啟動數據分配模塊，每個時鐘周期將 相應的數據賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘輸出一個狀態預測 值；其中一步預測模塊共佔用兩個浮點加法單元和兩個浮點乘法運算單元；在QUARTUS II 的參數可設置模塊中，浮點加、乘法器的周期參數分別設置為m、n個時鐘周期；在數據運算 模塊的第一級浮點乘法運算時，需要將未參與運算的數據同樣設置為η個時鐘的延時；在 數據運算模塊的第二級浮點加法運算時，也需要將相應的上一級運算結果設置為m個時鐘 的延時；在輸入埠，每個時鐘周期輸入一個數據，當接收完九個狀態值後，每個時鐘將相 應的乘數和被乘數傳送到對應的寄存器並使能運算模塊，這樣每個數據在運算模塊中經過 n+m+m個時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。
4.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特 徵在於預測誤差協方差模塊分為數據分配模塊和數據運算模塊，當系統接收到當前時刻測 量值且預測誤差協方差模塊接收完前一時刻濾波誤差協方差值後，開始啟動數據分配模 塊，每個時鐘周期將相應的數賦給對應的寄存器，經過n+m+m+m+m個時鐘周期之後，每個時 鍾輸出一個預測誤差協方差值；其中預測誤差協方差模塊共佔用九個浮點加法單元和八個 浮點乘法運算單元；在QUARTUS II的參數可設置模塊中，浮點加、乘法器的周期參數分別 設置為m、η個時鐘周期；在數據運算模塊的第一級浮點乘法運算時，需要將未參與運算的 數據同樣設置為η個時鐘的延時；在數據運算模塊的第三、四級浮點加法運算時，也需要將 相應的上一級運算結果設置為m個時鐘的延時；在輸入埠，每個時鐘周期輸入一個數據， 當接收完前一時刻濾波誤差協方差值後，每個時鐘將相應的乘數和被乘數傳送到對應的寄存器並使能運算模塊，這樣每個數據在運算模塊中經過n+m+m+m+m個時鐘周期的運算延時 後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。
5.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特 徵在於平均真實偏差模塊分為數據分配模塊和數據運算模塊，當平均真實偏差模塊接收完 徑向距離和俯仰角與方位角的三角函數值之後，開始啟動數據分配模塊，每個時鐘周期將 相應的數賦給對應的寄存器，經過η+η個時鐘周期之後，每個時鐘輸出一個平均真實偏差 值；其中平均真實偏差模塊共佔用三個浮點乘法運算單元；在QUARTUS II的參數可設置模 塊中，浮點乘法器的周期參數分別設置為η個時鐘周期；在輸入埠，每個時鐘周期輸入一 個數據，當接收完徑向距離和俯仰角與方位角的三角函數值之後，每個時鐘將相應的乘數 和被乘數傳送到對應的寄存器並使能數據運算模塊，這樣每個數據在數據運算模塊中經過 η+η個時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。
6.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特 徵在於增益矩陣模塊分為數據分配模塊和數據運算模塊，當增益矩陣模塊接收完預測誤差 協方差值之後，開始啟動數據分配模塊，每個時鐘周期將相應的數賦給對應的寄存器，經過 n+m+m個時鐘周期之後，每個時鐘輸出一個增益值；其中增益矩陣模塊共佔用兩個浮點加 法單元和三個浮點乘法運算單元；在QUARTUS II的參數可設置模塊中，浮點加、乘法器的 周期參數分別設置為m、η個時鐘周期；在數據運算模塊的第二級浮點加法運算時，需要將 相應的上一級運算結果設置為m個時鐘的延時；在輸入埠，每個時鐘周期輸入一個數據， 當接收完預測誤差協方差值和新息之後，每個時鐘將相應的乘數和被乘數傳送到對應的寄 存器並使能運算模塊，這樣每個數據在數據運算模塊中經過n+m+m個時鐘周期的運算延時 後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。
7.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特徵 在於狀態更新模塊分為數據分配模塊和數據運算模塊，當狀態更新模塊接收完狀態預測值 和增益矩陣和新息之後，開始啟動數據分配模塊，每個時鐘周期將相應的數賦給對應的寄 存器，經過n+m+m個時鐘周期之後，每個時鐘輸出一個狀態估計值；其中狀態更新模塊共佔 用三個浮點加法單元和三個浮點乘法運算單元；在QUARTUS II的參數可設置模塊中，浮點 加、乘法器的周期參數分別設置為m、η個時鐘周期；在數據運算模塊的第一級浮點乘法運 算時，需要將未參與運算的數據同樣設置為η個時鐘的延時；在輸入埠，每個時鐘周期輸 入一個數據，當接收完狀態預測值和增益矩陣和新息後，每個時鐘將相應的乘數和被乘數 傳送到對應的寄存器並使能數據運算模塊，這樣每個數據在數據運算模塊中經過n+m+m個 時鐘周期的運算延時後，每個時鐘周期輸出一個數據，其中m > 7，η > 5。
8.根據權利要求1所述的基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，其特徵 在於濾波誤差協方差模塊分為數據分配模塊和數據運算模塊，當濾波誤差協方差模塊接收 完預測誤差協方差值和增益矩陣值之後，開始啟動數據分配模塊，每個時鐘周期將相應的 數賦給對應的寄存器，經過n+m+m個時鐘周期之後，每個時鐘輸出一個濾波誤差協方差值； 其中濾波誤差協方差模塊共佔用一個浮點加法單元、兩個浮點減法單元和三個浮點乘法運 算單元；在QUARTUS II的參數可設置模塊中，浮點加、減、乘法器的周期參數分別設置為m、 m、η個時鐘周期；在數據運算模塊的第一級浮點乘法運算時，需要將未參與運算的數據同 樣設置為η個時鐘的延時；在輸入埠，每個時鐘周期輸入一個數據，當接收完預測誤差協方差值和增益矩陣值後，每個時鐘將相應的乘數和被乘數傳送到對應的寄存器並使能數據 運算模塊，這樣每個數據在運算模塊中經過n+m+m個時鐘周期的運算延時後，每個時鐘周 期輸出一個數據，其中m彡7，η彡5。
全文摘要
本發明公開了一種基於FPGA的去偏轉換量測卡爾曼濾波器的設計方法，首先設計基於FPGA的去偏轉換量測卡爾曼濾波的系統，增益矩陣模塊包含平均真實協方差子模塊和增益矩陣子模塊；狀態更新模塊包含坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊；所述的三角函數模塊、一步預測模塊、預測誤差協方差模塊、濾波誤差協方差模塊、平均真實協方差子模塊、增益矩陣子模塊、坐標轉換子模塊、平均真實偏差子模塊、新息子模塊和狀態更新子模塊分別調用了浮點加、減、乘、除運算模塊。本發明採用結構層次化設計，底層模塊運用VHDL輸入，頂層採用原理圖輸入方式，可提高代碼的可讀性，易於模塊劃分，且方便在設計過程中進行仿真。
文檔編號G01S13/66GK102064799SQ20101062272
公開日2011年5月18日 申請日期2010年12月31日 優先權日2010年12月31日
發明者吳盤龍, 張捷, 朱建良, 杜國平, 王向民, 王寶寶, 王筱莉, 薄煜明, 鄒衛軍, 陳帥 申請人:南京理工大學