專利名稱：改進的自舉裂變粒子濾波方法及其dsp硬件實現(xiàn)方法
技術領域：
本發(fā)明涉及一種應用DSP硬件系統(tǒng)實現(xiàn)粒子濾波及仿真的方法，屬于非線性系統(tǒng)濾波和電子技術領域。
背景技術：
粒子濾波是一種基于蒙特卡羅方法和遞推貝葉斯估計的統(tǒng)計濾波方法，適用于任何能用狀態(tài)空間模型以及傳統(tǒng)的卡爾曼濾波表示的非高斯背景的非線性隨機系統(tǒng)。但是粒子濾波存在粒子退化、粒子多樣性喪失、粒子數(shù)與計算復雜度同比增長等問題。為此人們提出了許多改進方法，其中裂變自舉粒子濾波(FBPF)算法就是在粒子濾波算法的基礎上改進重采樣過程，引入了權值銳化檢測和“權值排序(Sorting)-裂變繁殖(Fission)-權值歸一(Normalizing) ”過程(簡稱SFN預處理過程)。裂變過程實質(zhì)上是一種隨機采樣方法，它的采樣計劃不是始終保持不變的，而是根據(jù)實驗結果不斷進行調(diào)整，設計出新的采樣計劃，從而克服了粒子匱乏問題。
在實際應用中，F(xiàn)BPF算法的復雜度主要由狀態(tài)空間模型的復雜度和算法本身的復雜度共同決定。如果一個算法的應用狀態(tài)空間模型已經(jīng)建立，其復雜度也就隨之確定，因此要降低一個算法的復雜度最終還要從簡化算法入手。FBPF算法的計算中非線性函數(shù)較多， 而大多數(shù)非線性函數(shù)計算又集中于權值計算步驟中。在FBPF算法中，一個輸入觀測值一般要經(jīng)過Ns次復雜的函數(shù)計算，權值計算時要進行Ns次指數(shù)運算，權值歸一化時要進行Ns次除法運算，輸出結果時要進行Ns次乘法運算，因此，粒子生成、權值計算、權值歸一化、結果輸出分別需要Ns次循環(huán)計算，如果粒子數(shù)Ns很大時計算量將大得驚人。因此，F(xiàn)BPF算法計算量比較大，實時性差。本發(fā)明以提高FBPF算法的運算速度和濾波精度為目的，對FBPF算法進行了一些改進，并發(fā)展與算法匹配的硬件結構來實現(xiàn)速度的提高。
同時，為了提高粒子濾波算法的運算速度和魯棒性，研究粒子濾波的硬件實現(xiàn)方法尤為關鍵。粒子濾波硬件實現(xiàn)的基本思想是將粒子濾波劃分為初始采樣、重采樣、狀態(tài)更新等不同過程，利用流水線實現(xiàn)分時并行處理。但實用化粒子濾波算法器尚未研制成功， 設計一個運算速度快、性能可靠、占用資源少的粒子濾波算法硬件模塊是本發(fā)明的主要內(nèi)容。發(fā)明內(nèi)容
本發(fā)明針對現(xiàn)有技術存在的缺陷提出了一種改進的裂變自舉粒子濾波算法，并給出了一種基于DSP的硬件實現(xiàn)及仿真方法。
本發(fā)明改進的自舉裂變粒子濾波方法，包括如下步驟
步驟1 :k = 0時初始化，采樣粒子X0 ~P⑷，< =HNs, i = 1,…，Ns ;k = 1， 2，…時做以下循環(huán)迭代，其中，^表示0時刻系統(tǒng)的狀態(tài)向量，P(Q)表示系統(tǒng)的狀態(tài)先驗條件概率，括號中的U表示某個變量，而P(Xo)表示初始狀態(tài)向量&的先驗概率，4表示0 時刻狀態(tài)向量的第i個粒子，其權值為< ；k，Ns表示自然數(shù)；
步驟2 :i = 1，…，Ns，采樣粒子 < p{xk lx'kl)，計算非歸一化權重 i*/^/·^)，計算權重的同時對這一時刻的權重累加求和，得到非歸一化的權值-粒子Ns集^ f ,和權重累加和& = ；其中, 表示k時刻系統(tǒng)的狀態(tài)向量，yk表示 k時刻系統(tǒng)的輸出，<是1^時刻狀態(tài)向量的第i個粒子X丨的權值；Ns
步驟3 輸出結果，& = * <) / & ；/=1
步驟4 權值銳化檢測，估計粒子集&的有效樣本容量換^=S2Jfj(W1k)2 ；/=1
步驟5 若說咁之Nth，則進行SFN預處理過程，得到新的權值-粒子集 Λ =丨圮,<丨2，并對新裂變粒子的丨置為1/NS，然后返回步驟2，否則直接轉入下一步；
步驟6 重采樣按粒子權值從大到小進行排列，將大權值的粒子進行裂變，然后覆蓋后面小權值的十個粒子，然后返回步驟2。
改進的自舉裂變粒子濾波方法的DSP硬件實現(xiàn)方法，包括如下步驟
第一步根據(jù)上述的改進的自舉裂變粒子濾波方法，在DSP中編寫程序，首先設置好系統(tǒng)模型和參數(shù)，包括數(shù)據(jù)樣點數(shù)、粒子個數(shù)、有效樣本容量限值、權值裂變的方差；
第二步對編寫好的程序進行編譯，應用CCS設置工程文件I^oject，將編譯好的C 文件、匯編文件、頭文件、LIB文件以及存儲器配置(CMD)文件添加到I^roject中，并根據(jù)目標板的實際存儲器和要求來完成該工程的編譯鏈接；
第三步應用MATLAB集成的RTDX接口將DSP中的數(shù)據(jù)通過RTDX通道傳給MATLAB 進行保存、分析和繪第四步將傳輸?shù)組ATLAB中的數(shù)據(jù)以圖形的形式輸出，輸出的圖形包括改進的自舉裂變粒子濾波方法中每個樣點粒子分布和有效樣本容量Nrff的關系圖，通過該圖分析粒子權值的銳化情況，從而分析粒子的多樣性情況；同時還可以輸出算法的濾波結果圖，計算出算法的平均濾波時間和濾波方差，從而分析算法的濾波精度和實時性。
本發(fā)明具體改進方法包括如下幾點
1. FBPF算法中輸出結果是每個粒子值分別與對應歸一化的粒子權重相乘，改進 FBPF算法輸出結果改為每個粒子值分別與對應的未歸一化的粒子權重相乘然后除以權重之和，即把輸出結果由^ =改為。其中 表示k時刻系統(tǒng)7=17=1的狀態(tài)向量，<是k時刻狀態(tài)向量的第i個粒子X丨的權值，Sk表示粒子權重之和。
2. FBPF算法中權值銳化檢測過程的有效樣本容量是Ns個歸一化權值平方和的倒數(shù)，改進FBPF算法中權值銳化檢測過程的有效樣本容量是Ns個未歸一化權值平方和的倒數(shù)與權值和的平方相乘，即把FBPF算法中的權值銳化檢測過程的有效樣本容量I =1/^( 2改為 H/f>:)2。 /=1 /=1
3. FBPF算法中重采樣過程產(chǎn)生的均勻分布的隨機數(shù)是在區(qū)間
本發(fā)明通過以上的改進省去了權值歸一化中的Ns次除法運算，大大減少了運算量。同時為了提高權值計算的精確性，將原算法中的權值計算公式^k=PiykIO·，片凡/<)，九表示k時刻系統(tǒng)的輸出。同時，經(jīng)大量實驗發(fā)現(xiàn)，權值銳化門限If 越大權值銳化越嚴重，權值越小粒子退化越嚴重，因此每一次濾波都采用重采樣步驟來消除粒子退化現(xiàn)象，并且當^eff > ^vft時進行SFN預處理過程。
為了應用DSP硬件實現(xiàn)改進的FBPF算法，選擇瑞泰創(chuàng)新公司生產(chǎn)的 ICETEK-5509-A開發(fā)板，此開發(fā)板的DSP芯片是美國TI公司的TMS320VC5509ADSP(簡稱 C5509A)，此芯片是美國TI公司生產(chǎn)的高性能、高性價比的16位定點DSP芯片，其結構是專門針對實時信號處理而設計的。
TI公司提供了一套強大的專用于進行TMS320系列DSP軟件設計的集成軟件開發(fā)環(huán)境，即Code Composer Studio (CCS) CCS支持實時數(shù)據(jù)交換(RTDX)技術，它利用DSP的內(nèi)部仿真邏輯和JTAG接口實現(xiàn)主機與目標機之間的數(shù)據(jù)交換，而不占用DSP的資源。同時 MATLAB集成了 RTDX接口，本發(fā)明在DSP中編寫了改進FBPF算法的程序，并將數(shù)據(jù)通過RTDX 通道傳給MATLAB進行保存、分析和繪圖，通過采用MATLAB和CCS聯(lián)合仿真的方法，分析粒子濾波算法的性能。


圖1改進的FBPF算法功能框圖。
具體實施方式
下面結合附圖對本發(fā)明的實施例做詳細的說明。
FBPF算法計算量比較大，不利于應用于實時性較高的系統(tǒng)中，本發(fā)明以提高FBPF 算法的運算速度和濾波精度為目的，采取減少除法運算和改進權值計算的方法。通過對 FBPF算法的復雜度分析可知，F(xiàn)BPF算法的復雜度主要由狀態(tài)空間模型的復雜度和算法本身的復雜度共同決定。要降低一個算法的復雜度最終還要從簡化算法入手；FBPF算法中的大多數(shù)非線性函數(shù)計算集中于權值計算步驟中，權值歸一化使得算法復雜度提高。另一方面，因為除法運算對硬件的開銷很大，所以盡可能的減少除法運算是提高FBPF算法運算速度的關鍵。經(jīng)過對FBPF算法反復研究發(fā)現(xiàn)，可以將權值歸一化步驟合并到結果輸出、權值銳化檢測、重采樣等幾個步驟中，從而減少大量的除法運算，降低算法的硬件開銷。
改進的FBPF算法如圖1所示，算法具體步驟如下
步驟1 :k = 0時初始化，采樣粒子X0 ~P⑷，< =HNs, i = 1,…，Ns ;k = 1， 2，…時做以下循環(huán)迭代。其中，^表示ο時刻系統(tǒng)的狀態(tài)向量，P(Q)表示系統(tǒng)的狀態(tài)先驗條件概率，χ丨表示0時刻狀態(tài)向量的第i個樣本(粒子)，其權值為<。
步驟2 :i = 1，…，Ns，采樣粒子 < p(Xk !x'ki)，計算非歸一化權重
權利要求
1.一種改進的自舉裂變粒子濾波方法，其特征在于包括如下步驟步驟1 :k = 0時初始化，采樣粒子名~/7(而)，<=1/乂“ = 1，…，Ns;k=l，2，…時做以下循環(huán)迭代，其中，^表示0時刻系統(tǒng)的狀態(tài)向量，P(Q)表示系統(tǒng)的狀態(tài)先驗條件概率，括號中的U表示某個變量，而P(Xc1)表示初始狀態(tài)向量&的先驗概率，4表示0時刻狀態(tài)向量的第i個粒子，其權值為< ；k，Ns表示大于1的自然數(shù)；步驟2 :i = 1，…，Ns，采樣粒子X丨 片而/·^“)，計算非歸一化權重，/^/幻，計算權重的同時對這一時刻的權重累加求和，得到非歸一化的權值-粒子集WkW1kY^Ns和權重累加和& = Σ< ；其中，^表示k時刻系統(tǒng)的狀態(tài)向量，Yk表示k時刻系統(tǒng)的輸出，7=1<是k時刻狀態(tài)向量的第i個粒子X丨的權值；步驟3:輸出結果，4 =(|χ/=1步驟4 權值銳化檢測，估計粒子集&的有效樣本容量換^=S2Jfj(W1k)2 ；/=1步驟5:若SiVft，則進行SFN預處理過程，得到新的權值-粒子集J^={K}t， 并對新裂變粒子的1置為1/NS，然后返回步驟2，否則直接轉入下一步；步驟6 重采樣按粒子權值從大到小進行排列，將大權值的粒子進行裂變，然后覆蓋后面小權值的十個粒子，然后返回步驟2。
2.一種如權利要求1所述的改進的自舉裂變粒子濾波方法的DSP硬件實現(xiàn)方法，其特征在于包括如下步驟第一步根據(jù)上述的改進的自舉裂變粒子濾波方法，在DSP中編寫程序，首先設置好系統(tǒng)模型和參數(shù)，包括數(shù)據(jù)樣點數(shù)、粒子個數(shù)、有效樣本容量限值、權值裂變的方差；第二步對編寫好的程序進行編譯，應用CCS設置工程文件ftx) ject，將編譯好的C文件、匯編文件、頭文件、LIB文件以及存儲器配置(CMD)文件添加到I^roject中，并根據(jù)目標板的實際存儲器和要求來完成該工程的編譯鏈接；第三步應用MATLAB集成的RTDX接口將DSP中的數(shù)據(jù)通過RTDX通道傳給MATLAB進行保存、分析和繪圖，采用MATLAB和CCS聯(lián)合仿真的方法，分析粒子濾波算法的性能；第四步將傳輸?shù)組ATLAB中的數(shù)據(jù)以圖形的形式輸出，輸出的圖形包括改進的自舉裂變粒子濾波方法中每個樣點粒子分布和有效樣本容量^ff的關系圖，通過該圖分析粒子權值的銳化情況，從而分析粒子的多樣性情況；同時還可以輸出算法的濾波結果圖，計算出算法的平均濾波時間和濾波方差，從而分析算法的濾波精度和實時性。
全文摘要
本發(fā)明一種改進的自舉裂變粒子濾波方法及其仿真方法，將權值歸一化步驟合并到結果輸出、權值銳化檢測、重采樣等幾個步驟中，從而減少了大量的除法運算，降低了硬件開銷。根據(jù)粒子濾波算法運算量大的特點，選擇了TI公司的TMS320VC5509A數(shù)字信號處理器(DSP)來實現(xiàn)粒子濾波算法，通過集成在MATLAB 7.1中的CCSLink工具將C5509及其集成開發(fā)環(huán)境CCS 2.21連接在一起，在DSP中編寫了改進FBPF算法的程序，并將數(shù)據(jù)通過RTDX通道傳給MATLAB進行保存、分析和繪圖，通過采用MATLAB和CCS聯(lián)合仿真的方法，分析粒子濾波算法的性能。
文檔編號G06F17/50GK102508947SQ20111030809
公開日2012年6月20日 申請日期2011年10月11日 優(yōu)先權日2011年10月11日
發(fā)明者伍雪冬, 張冰, 朱志宇, 李陽, 楊官校, 王建華, 王敏, 薄超, 趙強 申請人:江蘇科技大學