專利名稱：基于fpga的無跡卡爾曼濾波系統(tǒng)及并行實現(xiàn)方法
技術領域：
本發(fā)明屬于信號處理技術領域，涉及非線性濾波方法，可用于目標跟蹤和參數(shù)估計。

背景技術：
濾波理論是在對系統(tǒng)可觀測信號進行測量的基礎上，根據(jù)一定的濾波準則，對系統(tǒng)的狀態(tài)進行估計的理論和方法。根據(jù)貝葉斯理論，最優(yōu)估計只是一個理想化的解，一般情況下，無法得到它的解析形式?？柭?Kalman)濾波是目前最經(jīng)典、應用最廣泛的線性濾波器，當系統(tǒng)為線性高斯分布時，它可以得到遞歸貝葉斯估計的最小均方誤差解。然而，實際的系統(tǒng)往往是非線性、非高斯的，無法采用卡爾曼濾波求解，為此提出了大量非線性濾波方法，主要可分為粒子濾波和高斯濾波兩大類。
粒子濾波是一種貝葉斯框架下基于采樣的非線性濾波方法，它使用一些帶權值的樣本粒子來近似模擬先驗和后驗概率分布。這里的樣本粒子是隨機采樣得到的，當樣本數(shù)目足夠大時，粒子濾波趨于最優(yōu)貝葉斯估計，可以得到很好的濾波精度。但是，粒子濾波計算量太大，實時性差，難以在工程中應用。
高斯濾波以擴展卡爾曼濾波(Extended Kalman Filter，EKF)和無跡卡爾曼濾波(Unscented Kalman Filter，UKF)為代表。前者是一種弱非線性濾波算法，它將非線性函數(shù)通過一定的規(guī)則(如泰勒級數(shù)展開等)進行線性化處理，在一定程度上提高了非線性目標的跟蹤性能，具有很好的實時性，目前已在軍用和民用領域得到廣泛應用。然而，當系統(tǒng)呈現(xiàn)強非線性時，一階線性化近似會帶來較大的誤差，可能導致濾波器不穩(wěn)定甚至發(fā)散。
UKF(Unscented Kalman Filter)是一種基于UT(Unscented Transform)變換的新濾波算法。該算法摒棄了對非線性函數(shù)進行線性化的傳統(tǒng)做法，轉而從統(tǒng)計學的角度尋找解決問題的思路。與粒子濾波的隨機采樣方式不同，UKF選取的Sigma樣本點很少，且是按照一定規(guī)則確定性選取的，實時性顯著優(yōu)于粒子濾波。與EKF算法相比，UKF算法對于高斯輸入的非線性函數(shù)的近似可以精確到三階，對于非高斯輸入的非線性函數(shù)的近似，至少可以精確到二階，精度上明顯優(yōu)于EKF。因此，UKF可以在濾波精度和實時性之間做到良好的折衷，具有廣泛的應用前景。
雖然與粒子濾波相比，UKF的實時性已有了明顯提高，但UKF中仍涉及到諸如矩陣求逆和Cholesky分解等復雜運算，當系統(tǒng)狀態(tài)維數(shù)和觀測維數(shù)較大時，運算速度明顯降低，硬件實現(xiàn)難度遠大于EKF。
FPGA具有大容量的Block RAM資源可用于存儲大量數(shù)據(jù)和實現(xiàn)查找表等功能；大量的DSP48E資源可以實現(xiàn)高效的浮點數(shù)加法、減法和乘法等運算；豐富的邏輯資源可以實現(xiàn)大規(guī)模并行運算。隨著超大規(guī)模集成電路技術的發(fā)展，F(xiàn)PGA的性能也在不斷提高，充分利用FPGA的這一特點會大大提高算法的運算速度。


發(fā)明內容
本發(fā)明的目的是針對上述問題，提出一種基于FPGA的無跡卡爾曼濾波系統(tǒng)及并行實現(xiàn)方法，以充分利用FPGA易于實現(xiàn)大規(guī)模并行運算的特點，在保證濾波精度的同時提高運算速度，降低硬件實現(xiàn)復雜度。
為實現(xiàn)上述目的，本發(fā)明基于FPGA的無跡卡爾曼濾波系統(tǒng)，包括 協(xié)方差矩陣Cholesky分解模塊A，用于對分塊對角協(xié)方差矩陣的對角線上的各子矩陣分別進行Cholesky分解，得到下三角矩陣，將下三角矩陣的L個行向量，分成K組，分別輸入到Sigma點產生模塊，其中L＝2m+n，m為狀態(tài)量的維數(shù)，n為觀測量的維數(shù)，K≥1； Sigma點產生模塊B，用于接收上一時刻的狀態(tài)估計值，利用A模塊得到的結果產生K組Sigma點，分別輸入到時間更新模塊，其中第一組有2L/K+1個采樣點，其余各組有2L/K個采樣點； 時間更新模塊C，用于把接收到的Sigma點代入系統(tǒng)模型的狀態(tài)方程，得到狀態(tài)預測值，輸入到觀測預測模塊； 觀測預測模塊D，用于把狀態(tài)預測值代入觀測模型的觀測方程，得到觀測預測值，并將觀測預測值和狀態(tài)預測值輸入到部分均值和協(xié)方差矩陣計算模塊； 部分均值和協(xié)方差矩陣計算模塊E，用于對觀測預測值和狀態(tài)預測值加權求和，分別計算部分狀態(tài)預測協(xié)方差矩陣、部分觀測預測協(xié)方差矩陣和部分互協(xié)方差矩陣，并將其計算結果輸入到總體均值計算模塊和總體協(xié)方差矩陣計算模塊； 總體均值計算模塊F，用于接收部分均值和協(xié)方差矩陣計算模塊的結果，分別計算狀態(tài)預測均值和觀測預測均值，并將其計算結果輸入到總體協(xié)方差矩陣計算模塊和狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊； 總體協(xié)方差矩陣計算模塊G，用于接收部分均值和協(xié)方差矩陣計算模塊和總體均值計算模塊的結果，分別計算狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和互協(xié)方差矩陣，并將觀測預測協(xié)方差矩陣輸入到觀測預測協(xié)方差矩陣求逆模塊，將互協(xié)方差矩陣輸入到增益計算模塊，將狀態(tài)預測協(xié)方差矩陣和觀測預測協(xié)方差矩陣輸入到狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊； 觀測預測協(xié)方差矩陣求逆模塊H，用于對觀測預測協(xié)方差矩陣采用奇異值分解方法求逆，并將求逆結果輸入到增益計算模塊； 增益計算模塊I，用于接收互協(xié)方差矩陣和觀測預測協(xié)方差矩陣的逆矩陣，計算增益，并將增益輸入到狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊； 狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊J，用于接收增益、狀態(tài)預測均值、狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和當前時刻的觀測數(shù)據(jù)，計算狀態(tài)估計值和狀態(tài)協(xié)方差矩陣，并將狀態(tài)估計值作為當前時刻的最終結果輸出，將狀態(tài)協(xié)方差矩陣擴維后輸入到A模塊。
為實現(xiàn)上述目的，本發(fā)明基于FPGA的無跡卡爾曼濾波并行實現(xiàn)方法，包括如下步驟 (1)協(xié)方差矩陣Cholesky分解步驟，對分塊對角協(xié)方差矩陣的對角線上的各子矩陣分別進行Cholesky分解，得到下三角矩陣，將下三角矩陣的L個行向量，分成K組，其中L＝2m+n，m為狀態(tài)量的維數(shù)，n為觀測量的維數(shù)，K≥1； (2)Sigma點產生步驟，接收上一時刻的狀態(tài)估計值，利用Cholesky分解得到的L個向量產生K組Sigma點； (3)時間更新步驟，將Sigma點代入系統(tǒng)模型的狀態(tài)方程，得到狀態(tài)預測值； (4)觀測預測步驟，將狀態(tài)預測值代入觀測模型的觀測方程，得到觀測預測值； (5)部分均值和協(xié)方差矩陣計算步驟，對觀測預測值和狀態(tài)預測值進行加權求和，分別計算出部分狀態(tài)預測協(xié)方差矩陣、部分觀測預測協(xié)方差矩陣和部分互協(xié)方差矩陣； (6)總體均值計算步驟，計算狀態(tài)預測均值和觀測預測均值； (7)總體協(xié)方差矩陣計算步驟，分別計算狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和互協(xié)方差矩陣； (8)觀測預測協(xié)方差矩陣求逆步驟，對觀測預測協(xié)方差矩陣采用奇異值分解方法求逆； (9)增益計算步驟，將互協(xié)方差矩陣和觀測預測協(xié)方差矩陣的逆矩陣相乘得到增益； (10)狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計步驟，利用增益、狀態(tài)預測均值、狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和當前時刻的觀測數(shù)據(jù)，計算狀態(tài)估計值和狀態(tài)協(xié)方差矩陣，并將狀態(tài)估計值作為當前時刻的最終結果輸出，對狀態(tài)協(xié)方差矩陣擴維，返回到步驟(1)進行下一時刻的計算。
本發(fā)明具有如下優(yōu)點 本發(fā)明的濾波系統(tǒng)由于從總體上給出了UKF的并行結構，提高了濾波的速度； 本發(fā)明的濾波方法由于將分塊對角矩陣的Cholesky分解轉化為對對角線上各子矩陣的Cholesky分解，降低了進行Cholesky分解的矩陣維數(shù)，易于硬件實現(xiàn)； 本發(fā)明由于給出了Cholesky分解矩陣各元素在FPGA中的并行運算方法，提高了濾波的速度。
本發(fā)明由于采用奇異值分解實現(xiàn)矩陣求逆，并將求逆矩陣分為多個處理2×2維子矩陣的并行運算單元在FPGA中同時進行運算，降低了硬件實現(xiàn)復雜度。



圖1是本發(fā)明的無跡卡爾曼濾波系統(tǒng)結構圖； 圖2是本發(fā)明的無機卡爾曼濾波方法流程圖； 圖3是現(xiàn)有M×M維矩陣奇異值分解各子矩陣之間進行元素交換圖。

具體實施例方式 參照圖1，本發(fā)明基于FPGA的無跡卡爾曼濾波系統(tǒng)包括協(xié)方差矩陣Cholesky分解模塊A、Sigma點產生模塊B、時間更新模塊C、觀測預測模塊D、部分均值和協(xié)方差矩陣計算模塊E、總體均值計算模塊F、總體協(xié)方差矩陣計算模塊G、觀測預測協(xié)方差矩陣求逆模塊H、增益計算模塊I和狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊J。其中，模塊B包含K個Sigma點產生子模塊Bi，i＝1，2，...，K，B1，B2，...，BK采用K個并行運算單元結構，模塊C包含K個時間更新子模塊Ci，i＝1，2，...，K，C1，C2，...，CK采用K個并行運算單元結構，模塊D包含K個觀測預測子模塊Di，i＝1，2，..，K，D1，D2，...，DK采用K個并行運算單元結構，模塊E包含K個部分均值和協(xié)方差矩陣計算子模塊Ei，i＝1，2，...，K，E1，E2，...，EK采用K個并行運算單元結構。模塊A實現(xiàn)協(xié)方差矩陣Cholesky分解，并將分解得到的下三角矩陣的行向量分成K組，分別送入模塊B的K個子模塊，模塊B的K個子模塊Bi分別產生一組Sigma點，送入模塊Ci，模塊Ci分別對該組Sigma點進行時間更新產生一組狀態(tài)預測值送入模塊Di，模塊Di將該組狀態(tài)預測值代入觀測方程產生一組觀測預測值并將狀態(tài)預測值和觀測預測值一起送入模塊Ei，模塊Ei對該組觀測預測值和狀態(tài)預測值加權求和，分別計算部分狀態(tài)預測協(xié)方差矩陣、部分觀測預測協(xié)方差矩陣和部分互協(xié)方差矩陣。將E1，E2，...，EK的計算結果送入模塊F和模塊G，模塊F采用E1，E2，...，EK輸入的K組計算結果實現(xiàn)總體均值的計算，并將計算結果送入模塊G，模塊G實現(xiàn)總體協(xié)方差矩陣計算，并將計算結果送入模塊H，模塊H對觀測預測協(xié)方差矩陣求逆，并將計算結果送入模塊I，模塊I計算增益，并將計算結果送入模塊J，模塊J計算狀態(tài)估計值和狀態(tài)協(xié)方差矩陣，并將狀態(tài)估計值輸出，將狀態(tài)協(xié)方差矩陣擴維后送入模塊A。
本系統(tǒng)的工作原理如下 協(xié)方差矩陣Cholesky分解模塊A采用Cholesky分解方法將協(xié)方差矩陣(L+λ)Px分解為一個上三角矩陣Δ和下三角矩陣ΔT的乘積，即(L+λ)Px＝ΔΔT。由于Px是由狀態(tài)協(xié)方差矩陣Pk-1、狀態(tài)噪聲協(xié)方差矩陣Q和觀測噪聲協(xié)方差矩陣R擴維后得到的，所以(L+λ)Px矩陣是分塊對角矩陣，對分塊對角協(xié)方差矩陣的對角線上的各子矩陣Pk-1、Q和R分別進行Cholesky分解，得到下三角矩陣ΔT，其中L＝2m+n，m為狀態(tài)量的維數(shù)，n為觀測量的維數(shù)，λ＝α2(L+k)-L，α為確定采樣點的散布程度，k為影響采樣點分布的四階矩； Sigma點產生子模塊Bi，i＝1，2，...，K，接收上一時刻的狀態(tài)估計值

并利用模塊A的第i組結果{ΔjT}(ΔjT表示ΔT的第j個行向量，j＝1+L(i-1)/K，...，Li/K)，按照1)式分別產生L個Sigma點χj和L個Sigma點χj+L，其中子模塊B0產生2L/K+1個Sigma點，包含其余子模塊產生2L/K個Sigma點； j＝1+L(i-1)/K，...，Li/K i＝1，...，K1) 時間更新子模塊Ci，i＝1，2，...，K，將來自Bi的一組Sigma點代入系統(tǒng)模型的狀態(tài)方程，得到狀態(tài)預測值{χk|k-1，ix(j)}，對于模塊C0，j＝0，1，...，2L/K，對于其余子模塊，j＝1，...，2L/K； 觀測預測子模塊Di，i＝1，2，...，K，把來自Ci的一組狀態(tài)預測值代入觀測模型的觀測方程，得到觀測預測值{yk|k-1，i(j)}，對于模塊D0，j＝0，1，...，2L/K，對于其余子模塊，j＝1，...，2L/K； 部分均值和協(xié)方差矩陣計算子模塊Ei，分別計算狀態(tài)預測樣本點的加權和

觀測預測樣本點的加權和

狀態(tài)預測協(xié)方差矩陣

觀測預測協(xié)方差矩陣

以及互協(xié)方差矩陣

計算公式如下 其中，對于模塊E0，j＝0，1，...，2L/K，對于其余子模塊，j＝1，...，2L/K，i＝1，2，...，K； 總體均值計算模塊F，按式7)和式8)計算狀態(tài)預測均值

和觀測預測均值
其中，i＝1，2，...，K； 總體協(xié)方差矩陣計算模塊G，對K個模塊Ei計算得到的部分狀態(tài)預測協(xié)方差矩陣

部分觀測預測協(xié)方差矩陣

以及部分互協(xié)方差矩陣

分別求和，得到狀態(tài)預測協(xié)方差矩陣

觀測預測協(xié)方差矩陣

和互協(xié)方差矩陣

然后按式9)～式11)更新各個協(xié)方差矩陣


其中，i＝1，2，...，K，α，β為權值中的參數(shù)，α＞0，β≥0，χk|k-1，0x(0)為模塊C0計算出的第一個狀態(tài)預測值，yk|k-1，0(0)為模塊D0計算出的第一個觀測預測值； 觀測預測協(xié)方差矩陣求逆模塊H，對觀測預測協(xié)方差矩陣

進行奇異值分解，得到正交矩陣U和正交矩陣V以及對角矩陣∑，對∑求逆得到∑-1，則

的逆矩陣

的計算公式如下； 增益計算模塊I，將互協(xié)方差矩陣

和觀測預測協(xié)方差矩陣的逆矩陣

相乘，得到增益K，計算公式如下； 狀態(tài)協(xié)方差矩陣估計模塊J，接收增益K、狀態(tài)預測均值

觀測預測均值

狀態(tài)預測協(xié)方差矩陣

觀測預測協(xié)方差矩陣

和當前時刻的觀測數(shù)據(jù)yk，計算狀態(tài)估計值

和狀態(tài)協(xié)方差矩陣Pk，并將

作為當前時刻的最終結果輸出，將Pk擴維后輸入到協(xié)方差矩陣Cholesky分解模塊A，計算公式如下。
參照圖2，本發(fā)明的基于FPGA的無跡卡爾曼濾波方法，包括以下步驟 步驟1，對分塊對角協(xié)方差矩陣的對角線上的各子矩陣分別進行Cholesky分解。
1.1)計算矩陣第i列的第一個非零元素，i≥1，并根據(jù)第一個非零元素同時計算該列的其它非零元素； 1.2)計算矩陣第i＝i+1列的第一個非零元素，并根據(jù)第一個非零元素同時計算該列的其它非零元素，i≤N，N為子矩陣維數(shù)。
步驟2，接收上一時刻，即k-1時刻的狀態(tài)估計值

利用步驟1Cholesky分解得到的L個向量產生K組Sigma點其中χk-1x、χk-1w和χk-1v分別為χk-1中對應于狀態(tài)量、過程噪聲和觀測噪聲的分量。
步驟3，將步驟2得到的K組Sigma點χk-1中對應于狀態(tài)量的χk-1x分量和對應于過程噪聲的χk-1w分量代入到如式16)所示的系統(tǒng)模型的狀態(tài)方程中，得到K組狀態(tài)預測值 步驟4，將步驟3得到的K組狀態(tài)預測值χk|k-1x和K組Sigma點χk-1中對應于觀測噪聲的χk-1v分量代入到如式17)所示的觀測模型的觀測方程中，得到觀測預測值 步驟5，對步驟4得到的K組觀測預測值yk|k-1和步驟3得到的狀態(tài)預測值χk|k-1x進行加權求和，得到K組狀態(tài)預測樣本點的加權和

和K組觀測預測樣本點的加權和

按式4)～6)分別計算部分狀態(tài)預測協(xié)方差矩陣

部分觀測預測協(xié)方差矩陣

以及部分互協(xié)方差矩陣

其中i＝1，2，...，K。
步驟6，將步驟5得到的K組狀態(tài)預測樣本點的加權和

求和得到狀態(tài)預測均值

將步驟5得到的K組觀測預測樣本點的加權和

求和得到觀測預測均值

其中i＝1，2，...，K。
步驟7，計算總體協(xié)方差矩陣。
7.1)將步驟5得到的K組部分狀態(tài)預測協(xié)方差矩陣

K組部分觀測預測協(xié)方差矩陣

以及K組部分互協(xié)方差矩陣

分別求和，得到狀態(tài)預測協(xié)方差矩陣

觀測預測協(xié)方差矩陣

和互協(xié)方差矩陣

其中i＝1，2，...，K； 7.2)按式9)～式11)更新各個協(xié)方差矩陣。
步驟8，對步驟7得到的觀測預測協(xié)方差矩陣

求逆。
8.1)若步驟7.2)得到的更新后的觀測預測協(xié)方差矩陣

維數(shù)為奇數(shù)，則對該矩陣補一行和一列零元素使其維數(shù)為偶數(shù)，否則不變； 8.2)將

劃分為M/2×M/2個由2×2維的子矩陣組成的分塊矩陣，M為觀測預測協(xié)方差矩陣維數(shù)，a2i-1，2j-1、a2i-1，2j、a2i，2j-1和a2i，2j為觀測預測協(xié)方差矩陣對應位置上的元素，為了表述方便將Pij表示為

其中αij＝a2i-1，2j-1，βij＝a2i-1，2j，γij＝a2i，2j-1，δij＝a2i，2j， 8.3)將待求的正交矩陣U劃分為M/2×M/2個由2×2維的子矩陣

組成的分塊矩陣，其中，當i≠j時，Uij的初始值為μij＝νij＝σij＝τij＝0，當i＝j時，Uij的初始值為μii＝τii＝1，σii＝νii＝0，將待求的正交矩陣V劃分為M/2×M/2個由2×2維的子矩陣組成的分塊矩陣，其中，當i≠j時，Vij的初始值為ηij＝ωij＝ρij＝εij＝0，當i＝j時，Vij的初始值為ηii＝εii＝1，ωii＝ρii＝0； 8.4)按如下公式計算φ+θ和φ-θ，并計算cosθ、sinθ、cosφ和sinφ 其中，αii、βii、γii和δii分別是子矩陣對應位置上的元素， 8.5)將對角線上的子矩陣按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，αii、βii、γii和δii分別是子矩陣更新前對應位置上的元素，αii′和δii′分別是按式20)更新后子矩陣對應位置上的元素， 8.6)將非對角線上的子矩陣Pij(i≠j)按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，αij、βij、γij和δij分別是子矩陣更新前對應位置上的元素，αij′、βij′、γij′和δij′是按式21)更新后子矩陣對應位置上的元素，且i≠j； 8.7)將Uij按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，μij、νij、σij和τij是更新前對應位置上的元素，μij′、νij′、σij′和τij′是按式22)更新后對應位置上的元素， 8.8)將Vij按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，ηij、ωij、ρij和εij是更新前對應位置上的元素，ηij′、ωij′、ρij′和εij′是按式23)更新后對應位置上的元素， 8.9)將步驟8.5)～8.8)更新后的矩陣

U和V的子矩陣內部進行元素交換，交換規(guī)則如下 當i＝1，j＝1時，
當i＝1，i≠1時，
當i≠1，j＝1時，
其它情況
其中，α、β、γ和δ是交換前子矩陣

對應位置上的元素，α′、β′、γ′和δ′是交換后子矩陣

對應位置上的元素，i，j是子矩陣的下標， 8.10)將矩陣

U和V各子矩陣之間進行元素交換，交換規(guī)則如圖3所示，并將交換后的矩陣

U和V代入步驟8.4)； 8.11)將步驟8.4)～8.9)重復S(M-1)次，最后得到S(M-1)次更新后的正交矩陣U和V，并由S(M-1)次更新后的

得到對角矩陣∑，其中，S＝log2M； 8.12)將對角矩陣∑的對角線上的非零元素求倒數(shù)，得到∑-1，則

的逆矩陣為V∑-1UT。
步驟9，將步驟7得到的互協(xié)方差矩陣

和步驟8得到的觀測預測協(xié)方差矩陣的逆矩陣

相乘，得到增益K。
步驟10，將步驟9得到的增益K、步驟6得到的狀態(tài)預測均值

觀測預測均值

和當前時刻(k時刻)的觀測數(shù)據(jù)yk代入14)式得到狀態(tài)估計值

將步驟7得到的狀態(tài)預測協(xié)方差矩陣

觀測預測協(xié)方差矩陣

和步驟9得到的增益K代入15)式得到狀態(tài)協(xié)方差矩陣估計值Pk，并將狀態(tài)估計值

作為當前時刻的最終結果輸出，對狀態(tài)協(xié)方差矩陣Pk擴維，返回到步驟1進行下一時刻的計算。
本發(fā)明的效果可通過如下仿真實驗說明 1.仿真條件 本實驗采用二維平面內靜止布置的三個被動傳感器測量同一平面內的單個運動目標，三個被動傳感器成正三角形分布，各傳感器位置坐標分別為(20，20)、(30，20)和(25，28.66)，單位為km，每個傳感器的采樣周期T為10ms。目標在坐標軸上的初始位置為x0＝0km，y0＝23km，目標以速度作勻速直線運動。系統(tǒng)模型的狀態(tài)方程為 xk＝Fk，k-1xk-1+wk-1 28) 其中，為k時刻的目標狀態(tài)，wk-1為高斯分布的狀態(tài)噪聲，F(xiàn)為目標狀態(tài)轉移矩陣。


T為采樣周期29) 觀測模型的觀測方程為 zik＝Hi[xk]+vik30) 其中，zik是第i(i＝1，2，3)個被動觀測站的方位觀測，yi和xi分別為觀測站在直角坐標中兩個方向上的位置，vik是觀測噪聲，假設vik為零均值白噪聲，方差為σi，觀測噪聲協(xié)方差矩陣為
在Xilinx公司的FPGA芯片XC4VFX140上用本發(fā)明提出的無跡卡爾曼濾波系統(tǒng)實現(xiàn)對該運動目標的跟蹤，將UKF算法在FPGA中采用本發(fā)明提出的并行結構進行處理。其中FPGA芯片的速度級別為-11，主時鐘頻率為100MHz，運算過程中涉及的乘法器、加法器、減法器、除法器和開方運算器均采用Xilinx提供的浮點數(shù)IP核實現(xiàn)，反正切、正弦和余弦的計算均采用Xilinx提供的CORDIC算法IP核實現(xiàn)。
2.仿真結果 表1給出了FPGA的運算時間與并行單元個數(shù)之間的關系。從表1中可以看出并行單元個數(shù)越多，運算時間越短，因而本發(fā)明的基于FPGA的無跡卡爾曼濾波系統(tǒng)及并行實現(xiàn)方法具有良好的實時性。
表1不同并行單元個數(shù)對應的運算時間
權利要求
1.一種基于FPGA的無跡卡爾曼濾波系統(tǒng)，包括
協(xié)方差矩陣Cholesky分解模塊A，用于對分塊對角協(xié)方差矩陣的對角線上的各子矩陣分別進行Cholesky分解，得到下三角矩陣，將下三角矩陣的L個行向量，分成K組，分別輸入到Sigma點產生模塊，其中L＝2m+n，m為狀態(tài)量的維數(shù)，n為觀測量的維數(shù)，K≥1；
Sigma點產生模塊B，用于接收上一時刻的狀態(tài)估計值，利用A模塊得到的結果產生K組Sigma點，分別輸入到時間更新模塊，其中第一組有2L/K+1個采樣點，其余各組有2L/K個采樣點；
時間更新模塊C，用于把接收到的Sigma點代入系統(tǒng)模型的狀態(tài)方程，得到狀態(tài)預測值，輸入到觀測預測模塊；
觀測預測模塊D，用于把狀態(tài)預測值代入觀測模型的觀測方程，得到觀測預測值，并將觀測預測值和狀態(tài)預測值輸入到部分均值和協(xié)方差矩陣計算模塊；
部分均值和協(xié)方差矩陣計算模塊E，用于對觀測預測值和狀態(tài)預測值加權求和，分別計算部分狀態(tài)預測協(xié)方差矩陣、部分觀測預測協(xié)方差矩陣和部分互協(xié)方差矩陣，并將其計算結果輸入到總體均值計算模塊和總體協(xié)方差矩陣計算模塊；
總體均值計算模塊F，用于接收部分均值和協(xié)方差矩陣計算模塊的結果，分別計算狀態(tài)預測均值和觀測預測均值，并將其計算結果輸入到總體協(xié)方差矩陣計算模塊和狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊；
總體協(xié)方差矩陣計算模塊G，用于接收部分均值和協(xié)方差矩陣計算模塊和總體均值計算模塊的結果，分別計算狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和互協(xié)方差矩陣，并將觀測預測協(xié)方差矩陣輸入到觀測預測協(xié)方差矩陣求逆模塊，將互協(xié)方差矩陣輸入到增益計算模塊，將狀態(tài)預測協(xié)方差矩陣和觀測預測協(xié)方差矩陣輸入到狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊；
觀測預測協(xié)方差矩陣求逆模塊H，用于對觀測預測協(xié)方差矩陣采用奇異值分解方法求逆，并將求逆結果輸入到增益計算模塊；
增益計算模塊I，用于接收互協(xié)方差矩陣和觀測預測協(xié)方差矩陣的逆矩陣，計算增益，并將增益輸入到狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊；
狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊J，用于接收增益、狀態(tài)預測均值、狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和當前時刻的觀測數(shù)據(jù)，計算狀態(tài)估計值和狀態(tài)協(xié)方差矩陣，并將狀態(tài)估計值作為當前時刻的最終結果輸出，將狀態(tài)協(xié)方差矩陣擴維后輸入到A模塊。
2.根據(jù)權利要求1所述的基于FPGA的無跡卡爾曼濾波系統(tǒng)，其中模塊B包含K個Sigma點產生子模塊Bi，i＝1，2，...，K，B1，B2，...，BK采用K個并行運算單元結構。
3.根據(jù)權利要求1所述的基于FPGA的無跡卡爾曼濾波系統(tǒng)，其中模塊C包含K個時間更新子模塊Ci，i＝1，2，...，K，C1，C2，...，CK采用K個并行運算單元結構。
4.根據(jù)權利要求1所述的基于FPGA的無跡卡爾曼濾波系統(tǒng)，其中模塊D包含K個觀測預測子模塊Di，i＝1，2，...，K，D1，D2，...，DK采用K個并行運算單元結構。
5.根據(jù)權利要求1所述的基于FPGA的無跡卡爾曼濾波系統(tǒng)，其中模塊E包含K個部分均值和協(xié)方差矩陣計算子模塊Ei，i＝1，2，...，K，E1，E2，...，EK采用K個并行運算單元結構。
6.一種基于FPGA的無跡卡爾曼濾波的并行實現(xiàn)方法，包括
(1)協(xié)方差矩陣Cholesky分解步驟，對分塊對角協(xié)方差矩陣的對角線上的各子矩陣分別進行Cholesky分解，得到下三角矩陣，將下三角矩陣的L個行向量，分成K組，其中L＝2m+n，m為狀態(tài)量的維數(shù)，n為觀測量的維數(shù)，K≥1；
(2)Sigma點產生步驟，接收上一時刻的狀態(tài)估計值，利用Cholesky分解得到的L個向量產生K組Sigma點；
(3)時間更新步驟，將Sigma點代入系統(tǒng)模型的狀態(tài)方程，得到狀態(tài)預測值；
(4)觀測預測步驟，將狀態(tài)預測值代入觀測模型的觀測方程，得到觀測預測值；
(5)部分均值和協(xié)方差矩陣計算步驟，對觀測預測值和狀態(tài)預測值進行加權求和，分別計算出部分狀態(tài)預測協(xié)方差矩陣、部分觀測預測協(xié)方差矩陣和部分互協(xié)方差矩陣；
(6)總體均值計算步驟，計算狀態(tài)預測均值和觀測預測均值；
(7)總體協(xié)方差矩陣計算步驟，分別計算狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和互協(xié)方差矩陣；
(8)觀測預測協(xié)方差矩陣求逆步驟，對觀測預測協(xié)方差矩陣采用奇異值分解方法求逆；
(9)增益計算步驟，將互協(xié)方差矩陣和觀測預測協(xié)方差矩陣的逆矩陣相乘得到增益；
(10)狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計步驟，利用增益、狀態(tài)預測均值、狀態(tài)預測協(xié)方差矩陣、觀測預測協(xié)方差矩陣和當前時刻的觀測數(shù)據(jù)，計算狀態(tài)估計值和狀態(tài)協(xié)方差矩陣，并將狀態(tài)估計值作為當前時刻的最終結果輸出，對狀態(tài)協(xié)方差矩陣擴維，返回到步驟(1)進行下一時刻的計算。
7.根據(jù)權利要求6所述的無跡卡爾曼濾波的并行實現(xiàn)方法，其中步驟(1)所述的子矩陣Cholesky分解，先計算矩陣第i列的第一個非零元素，i≥1，并根據(jù)第一個非零元素同時計算該列的其它非零元素；再計算矩陣第i＝i+1列的第一個非零元素，并根據(jù)第一個非零元素同時計算該列的其它非零元素，i≤N，N為子矩陣維數(shù)。
8.根據(jù)權利要求6所述的無跡卡爾曼濾波的并行實現(xiàn)方法，其中步驟(8)所述的對觀測預測協(xié)方差矩陣采用奇異值分解方法求逆，按如下步驟進行
(8a)若觀測預測協(xié)方差矩陣維數(shù)為奇數(shù)，則對該矩陣補一行和一列零元素使其維數(shù)為偶數(shù)，否則不變；
(8b)將觀測預測協(xié)方差矩陣
劃分為M/2×M/2個由2×2維的子矩陣組成的分塊矩陣，M為觀測預測協(xié)方差矩陣維數(shù)，a2i-1，2j-1、a2i-1，2j、a2i，2j-1和a2i，2j為觀測預測協(xié)方差矩陣對應位置上的元素，為了表述方便將Pij表示為
其中αij＝a2i-1，2j-1，βij＝a2i-1，2j，γij＝a2i，2j-1，δij＝a2i，2j，
(8c)將待求的正交矩陣U劃分為M/2×M/2個由2×2維的子矩陣組成的分塊矩陣，其中，當i≠j時，Uij的初始值為μij＝νij＝σij＝τij＝0，當i＝j時，Uij的初始值為μii＝τii＝1，σii＝νii＝0，將待求的正交矩陣V劃分為M/2×M/2個由2×2維的子矩陣組成的分塊矩陣，其中，當i≠j時，Vij的初始值為ηij＝ωij＝ρij＝εij＝0，當i＝j時，Vij的初始值為ηii＝εii＝1，ωii＝ρii＝0；
(8d)按如下公式計算φ+θ和φ-θ，并計算cosθ、sinθ、cosφ和sinφ
其中，αii、βii、γii和δii分別是子矩陣對應位置上的元素，
(8e)將對角線上的子矩陣Pii按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，αii、βii、γii和δii分別是子矩陣更新前對應位置上的元素，αii′和δii′分別是按上式更新后子矩陣對應位置上的元素，
(8f)將非對角線上的子矩陣Pij(i≠j)按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，αij、βij、γij和δij分別是子矩陣更新前對應位置上的元素，αij′、βij′、γij′和δij′是按上式更新后子矩陣對應位置上的元素，且i≠j；
(8g)將Uij按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，μij、νij、σij和τij是更新前對應位置上的元素，μij′、νij′、σij′和τij′是按上式更新后對應位置上的元素，
(8h)將Vij按如下公式進行更新
其中，ciL表示cosθ，siL表示sinθ，ciR表示cosφ，siR表示sinφ，ηij、ωij、ρij和εij是更新前對應位置上的元素，ηij′、ωij′、ρij′和εij′是按式23)更新后對應位置上的元素，
(8i)將步驟(8e)～(8h)更新后的矩陣矩陣
U和V的子矩陣內部進行元素交換；
(8j)將矩陣
U和V各子矩陣之間進行元素交換，交換后的矩陣
U和V代入步驟(8d)；
(8k)將步驟(8d)～(8j)重復S(M-1)次，最后得到S(M-1)次更新后的正交矩陣U和V，并由S(M-1)次更新后的
得到對角矩陣∑，其中，S＝log2M；
(8l)將對角矩陣∑的對角線上的非零元素求倒數(shù)，得到∑-1，則
的逆矩陣為V∑-1UT。
全文摘要
本發(fā)明公開了一種基于FPGA的無跡卡爾曼濾波系統(tǒng)，主要解決現(xiàn)有無跡卡爾曼濾波硬件實現(xiàn)難度大和實時性差的問題。該系統(tǒng)包括協(xié)方差矩陣Cholesky分解模塊A，Sigma點產生模塊B、時間更新模塊C、觀測預測模塊D、部分均值和協(xié)方差矩陣計算模塊E、總體均值計算模塊F、總體協(xié)方差矩陣計算模塊G、觀測預測協(xié)方差矩陣求逆模塊H、增益計算模塊I和狀態(tài)量估計和狀態(tài)協(xié)方差矩陣估計模塊J。模塊A產生K組行向量到模塊B，模塊B、C、D和E是串連關系，分別包含K個采用并行運算單元結構的子模塊；模塊F、G接收模塊E的K組結果并處理，處理結果依次經(jīng)過模塊H、I和J得到當前結果。本發(fā)明具有濾波速度快和易于硬件實現(xiàn)的優(yōu)點，可用于目標跟蹤和參數(shù)估計。
文檔編號H03H21/00GK101777887SQ201010013568
公開日2010年7月14日 申請日期2010年1月8日 優(yōu)先權日2010年1月8日
發(fā)明者姬紅兵, 李倩, 王瑋, 閆家銘 申請人:西安電子科技大學