基于DSP的改進粒子濾波算法研究

摘 要:介紹了粒子濾波基本原理，針對粒子濾波計算量大和難以用硬件實現等缺點對粒子濾波算法進行了改進，使其平均計算周期縮短為原來的90%，應用DSP實現了粒子濾波算法。改進粒子濾波算法主要優化了原粒子濾波算法中權值計算、重采樣和輸出步驟，使其計算速度和濾波精度有所提高。這種改進粒子濾波算法在DSP系統中進行仿真，結果證明它具有速度快，精度高的優點。關鍵詞:粒子濾波; 硬件實現; 權值計算; 數字信號處理器

中圖分類號:TN911-34文獻標識碼:A

文章編號:1004-373X(2010)18-0009-04

Improved Particle Filter Algorithm Based on DSP

FENG Jun-xiang， ZHANG Jian， BO Chao

(School of Electronics and Information， Jiangsu University of Science and Technology， Zhenjiang 212003， China)

Abstract: Particle filter is based on the Monte Carlo and recursive Bayesian estimation which has special advantages in dealing with the nonlinear and non-Gaussian problems. However， both the enormous computations and low speed restrict its implementation in real-time system. At first， the basic theory of particle filter is introduced. Then， the particle filter algorithm is improved for solving the disadvantages of enormous computations and hard implementation of hardware， which reduced the average cycle to 90%. At last， particle filter algorithm is fulfilled through DSP. Compared with original particle filter algorithm， the improved algorithm optimizes the steps of computation of weights， resample and output，which improve the calculator speed and filter precision. The improved particle filter algorithm testifies its advantages of fast speed and high accuracy through carrying out simulation in DSP system.Keywords: particle filter; hardwareimplementation; computation of weights; DSP

0 引 言

粒子濾波(particle filtering，PF)[1-2]或Monte Carlo粒子濾波(MCPF)是以重要性采樣(importance sampling，IS)和序貫重要性采樣(sequential IS，SIS)為基礎的序貫Monte Carlo(sequential MC，SMC)方法，因此又稱為SMC濾波，1999年正式提出PF稱謂[3]，該名稱現已廣泛采用。由于PF算法在理論上對高維非線性、非高斯動態系統的狀態遞推估計或概率推理等問題都不具敏感性，因此，在復雜問題的求解上它表現出突出的優勢。但是到目前為止，在實時信號處理領域，粒子濾波算法幾乎沒有得到實際應用，這主要是因為粒子濾波算法本身較復雜，運算量大，需要存儲的空間大。某些改進粒子濾波算法雖然在一定程度上提高了粒子濾波算法的精度，卻使得粒子濾波算法更加復雜，實時性很差。

在此，首先介紹了標準粒子濾波算法，之后從硬件實現的角度出發，將粒子濾波權值計算中的權值歸一化部分合并到重采樣計算和輸出計算步驟中，并且改進了權值計算方法，以非線性非高斯系統為例，驗證了改進算法的精度，結果說明，改進算法更適合硬件實現，一定程度上在提高了算法運算速度的同時，提高了算法的濾波精度。

1 粒子濾波算法及復雜度分析

1.1 粒子濾波算法

粒子濾波算法是一種基于貝葉斯原理用粒子概率密度表示的序貫蒙特卡羅模擬方法[4-5]。對于離散時間估計問題，可用下面的狀態方程(1)和測量方程(2)進行描述。

xk=f(xk-1，vk-1)，k∈N (1)

zk=h(xk，nk)，k∈N(2)

式中:k為離散時間k時刻;xk∈Rdx為動態系統在k時刻的狀態變量;zk∈Rdz為動態系統在k時刻的觀測向量;v∈Rdv和n∈Rdn分別為系統噪聲和觀測噪聲，它們是相互獨立的隨機噪聲;方程f:Rdx→Rdx和h:Rdx→Rdz分別為有界的線性或非線性映射。狀態方程模型用來描述狀態隨時間演變的過程，測量方程模型用來描述狀態與觀測值之間的關系。

假定初始先驗概率密度函數p(x0/z0)=p(x0)，已知觀測值z0:k={zi:i=0，1，2，…，n}，則后驗概率分布可表示為[6]:

p(x1:k/z1:k)≈∑Mm=1w(m)1:kδ(x1:k-xm1:k)(3)

式中:{x(m)1:k，w(m)1:k}Mm=1是確定的，x(m)1:k={x(m)1，x(m)2，…，x(m)k};x(m)k為k時刻的第m個粒子;δ(#8226;)為狄拉克函數;w(m)k為k時刻第m個粒子的權值。

假如待估值為E[h(x1:k)]，其中h(#8226;)是以x1:k為自變量的函數，則估計值可由下式計算得出[7]:

E[h(x1:k)]=∑Mm=1w(m)1:kh(xm1:k)(4)

粒子濾波算法主要包括以下3個步驟:采樣、權值計算、重采樣。粒子濾波的前兩個步驟稱為序貫重要性采樣(SIS)，三個步驟合稱為序貫重要性重采樣(SIR)。SIS算法存在的一個基本問題就是退化現象，即經過幾步迭代遞推后，許多粒子的權重變得非常小，只有少數幾個粒子具有較大權值，大量的計算浪費在計算小權值粒子上。文獻[8]指出權重的方差隨著時間而增大，因此退化現象無法避免，也稱之為序貫蒙特卡羅方法進步的絆腳石。為了避免退化現象，Gordon等人提出了重采樣方法，其基本思想是對后驗概率密度再采樣NS次，產生新的支撐點集{x*(m)k/*m=1，2，…，NS}保留或復制較大的粒子，剔除權值較小的粒子[9]。

(1) 采樣:如果能夠直接從先驗概率p(x)中產生粒子，則每個粒子初始化時的權值為1/M。當無法從p(x)中產生粒子時，可以從重要性函數π(x)中采樣產生粒子x(m)k。

x(m)k≈π(xk/x(m)k-1，z1:k) (5)

(2) 權值計算:此步驟主要由以下兩個部分組成。

① 權值更新:

w*(m)k = w(m)k-1p(zk /x(m)k)p(x(m)k/x(m)k-1)π(x(m)k /x(m)k-1，z1:k ) (6)

② 權值歸一化:

w(m)k=w*(m)k∑Mi=1w*(i)k (7)

(3) 輸出:輸出結果xk=∑Mm=1x(m)kw(m)k

(4) 重采樣:引入重采樣以后，當觀察到濾波器發生明顯的退化時再進行重采樣步驟，因此選擇適當的時機進行重采樣尤其重要，通常選用門限法來判斷是否重采樣[10]。門限法采用有效采樣尺度Neff對退化現象進行衡量，其中有效采樣尺度Neff的定義如下:

Neff=N1+Var(w(m)k)(8)

式中:Var(w(m)k)為w(m)k的方差，但此表達式很難確切計算，因此在實際計算中應用Neff的近似估計值:

Neff=1∑Ni=1(w(m)k)2(9)

由式(9)可得，Neff≤M，且Neff越小，退化現象越嚴重。因此設定一個門限值Nth，當Neff≤Nth時，就進行重采樣。

圖1為重采樣示意圖，k-1時刻的先驗概率密度由若干權值為N-1的粒子近似表示(圖中僅畫出17個粒子)，經過k-1時刻系統觀測后(第一步)，重新計算粒子的權值wik-1，與實際情況符合較好的粒子(即圖中波峰處的粒子)被賦予較大的權值(即圖中面積較大的粒子);偏離實際情況的粒子(即圖中波谷處的粒子)被賦予較小的權值(即圖中面積較小的粒子)。經過重采樣過程(第二步)，權值較大的粒子衍生出較多的后代粒子;權值較小的粒子相應的后代粒子較少，并且后代粒子權重被重新設置為N-1。經過系統轉移過程(第三步)，預測每個粒子在k時刻的狀態。k時刻系統觀測過程(第四步)與k-1時刻系統觀測過程相同，最終的目標狀態表示為所有粒子的加權和。

圖1 重采樣示意圖

1.2 復雜度分析

粒子濾波算法的功能框圖如圖2所示。對于輸入一個觀測值zk，粒子濾波算法執行采樣(粒子初始化)、權值更新、權值歸一化、重采樣及輸出計算幾個步驟。粒子濾波是一個反復迭代的方法，只有重采樣結束后才能進行下一時刻的粒子更新。

算法本身的復雜度和所用空間模型的復雜度共同決定了實際應用粒子濾波算法的復雜度，對于每一個觀測值需要執行的操作步驟比較多，同時又需要處理大量的粒子。粒子濾波算法一般用于解決非線性問題，因此計算的函數都是非線性函數，大多數非線性運算都集中在權值計算階段。采樣階段的非線性函數主要是隨機數生成和狀態方程兩個部分，至于具體的非線性函數由實際情況決定。

圖2 粒子濾波算法的功能框圖

2 硬件實現的改進算法

粒子濾波算法的計算量比較大，不利于應用于實時性較高的系統中。本文以提高粒子濾波算法的運算速度和濾波精度為目的，采取減少除法運算和改進權值計算的方法。由第1.2節對粒子濾波算法復雜度分析可知，權值歸一化增加了算法復雜度，因為除法運算對硬件的開銷很大，所以盡可能減少除法運算是提高粒子濾波算法運算速度的關鍵。經過反復研究粒子濾波算法發現，可以把權值歸一化步驟合并到權值計算、結果輸出和重采樣等幾個步驟中，從而減少大量的除法運算和降低硬件的開銷。具體改進方法如下:

(1) 粒子濾波算法中輸出結果是每個粒子值分別與對應歸一化的粒子權重相乘，改進粒子濾波算法的輸出結果改為每個粒子值分別與對應的未歸一化粒子權重相乘，然后除以權重之和，即把輸出結果由xk=∑Mm=1x(m)kw(m)k改為xk=∑Mm=1x(m)kw*(m)k/Sk。

(2) 粒子濾波算法中粒子退化檢測過程中有效樣本容量是M個歸一化權值平方和的倒數，改進粒子濾波算法中粒子退化檢測過程的有效樣本容量是M個未歸一化權值平方和的倒數與權值和的平方相乘，即把粒子濾波算法中粒子退化檢測過程的有效樣本容量Neff=1/∑Mm=1(w(m)k)2改為Neff=S2k/∑Mm=1(w(m)k)2。

(3) 粒子濾波算法中重采樣過程產生的均勻分布的隨機數是在區間[0，1/M)，改進粒子濾波算法中重采樣過程產生的均勻分布的隨機數是在區間[0，Sk/M)。

(4) 權值計算函數12πσe-(z-z′)22σ2中12πσ和2σ2為重復計算的常量，預先計算其值，設置為中間變量K和G，可以把幾個乘除法運算省去，同時將權值計算中(z-z′)2改為|z-z′|，減少了1次乘法運算。最后權值計算函數為Ke-|z-z′|G。

通過以上的改進粒子濾波算法省去了權值歸一化中M次除法運算，大大減少了運算量，同時也降低了粒子在歸一化時權值和不能整除每個權值而引入的計算誤差。改進粒子濾波算法的功能框圖如圖3所示。

本文中為了減少計算量，選取先驗概率密度為重要性函數:

π(xk/x(m)k-1，zk)=p(xk/x(m)k-1)(10)

權重更新為:

wk∝w(m)k-1*p(zk/x(m)k) (11)

(1) 初始化:初始化粒子{x(m)0}Mm=1從先驗概率p(x0/z0)=p(x0)中采樣抽取，M為粒子數目，每個粒子初始化時的權值為1/M。

(2) 采樣:從先驗概率密度p(xk/x(m)k-1)中采樣，即對各個粒子狀態進行更新。

(3) 權值計算:計算非歸一化權重w(m)k=w(m)k-1p(zk/x(m)k)，計算權重的同時對這一時刻的權重累加求和，非歸一化的權值-粒子集Xk={x(m)k，w(m)k}Mi=1和權重累加和Sk=∑Mm=1w(m)k。

(4) 輸出:輸出結果xk=(∑Mm=1x(m)kw(m)k)/Sk

(5) 重采樣:若Neff≤Nth，則轉入重采樣步驟，否則進入重新采樣過程。重新采樣過程是復制權重大的粒子，并用其覆蓋權重小的粒子，然后返回采樣過程。

圖3 改進粒子濾波算法功能框圖

3 仿真實驗和結果分析

3.1 仿真實驗方法

本文選用的核心處理芯片是TMS320VC5509A系列，DSP(以后簡稱C5509A)。C5509A具有處理速度快，庫函數中含有高效率的乘除法、指數運算、三角函數和隨機數生成函數的特點，是美國德州儀器公司生產的TMS320系列DSP芯片中定點數字信號處理器(DSP)[11]。它的結構是專門針對實時信號處理而設計的，具有指令靈活，可操作性強，速度快以及支持并行運算和C語言等特點，是一種性價比較高的DSP。該仿真，采用Matlab與CCS的聯合仿真[12]，應用實時數據交換(RTDX)程序設計方法[13-15]，RTDX是TI公司推出的一種非常優秀的實時數據傳輸技術，為DSP系統的軟件調試提供了一種全新的方法。它利用DSP的內部仿真邏輯和JTAG接口實現主機與目標機之間的數據交換。不占用DSP的系統總線和串口等I/O資源，數據傳送完全可以在應用程序的后臺運行。通過集成在Matlab 7.1中的CCSLink工具，把C5509A及其集成開發環境CCS 2.21 連接在一起。在Matlab環境下即可完成對CCS和DSP目標板的操作，包括與目標板之間的數據交換，檢測處理器的狀態，控制DSP程序的運行等，同時不占用DSP的有用資源。因此能夠精確地計算出每一次粒子循環所用的時間。該實驗中DSP的時鐘頻率是120 MHz。

3.2 仿真模型和實驗結果

使用本文提出的改進FBPF算法，對非線性、非高斯系統的狀態估計進行了試驗，采用的系統模型[16]如下:

xk=1+sin(0.4πt)+0.5xk-1+vk-1

yk=0.2x2k+nk，t≤30

0.5xk-2+nk， t>30

式中:過程噪聲vk-1～Gamma(3，2);觀測噪聲nk～N(0，1)。采用FBPF算法和改進FBPF算法進行實驗，仿真的數據樣點數為60，粒子個數為50，有效樣本容量限Nth=20。

圖4、圖5分別為PF算法和改進PF算法的濾波結果。從圖中可以看到，改進的PF算法中粒子收斂速度比PF算法快。詳細的比較參數如表1所示。

圖4 PF算法濾波結果

圖5 改進PF算法濾波結果

表1 平均運行時間和濾波結果比較

PF算法改進PF算法

狀態原始值與濾波值方差0.219 10.128 7

狀態原始值與加噪值方差0.811 20.877 8

觀測原始值與濾波值方差0.194 80.068 3

觀測原始值與加噪值方差1.767 91.879 4

平均運行時間 /s0.109 70.102 2

改進PF算法和PF算法的方差都是小范圍內波動，但總體上改進PF算法的精度優于PF算法。進行改進PF算法編程時，盡量減少中間變量和乘除法的運算，以便減少硬件開銷，使改進PF算法的程序更適合在硬件上運行。由于改進PF算法省去了權值歸一化步驟和優化了權值計算步驟，所以改進PF算法的平均運行時間總是低于PF算法，并且濾波精度高于PF算法。

4 結 語

PF算法的復雜性和龐大計算量使其運算速度緩慢，限制了其在實際中的應用。本文介紹了PF算法的基本原理，通過改進權值計算步驟和重采樣步驟，優化算法結構，提高了運算速度。改進PF算法所用的時間低于PF算法，能夠應用于實時性要求較高的系統中，如車輛追蹤、智能機器人、視頻監視等方面。

參考文獻

[1]ARULAMPALARN M S， MASKELL S， GORDON N， et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J].IEEE Trans. Signal Processing，2002，50(2):174-188.

[2]DOUCET J A， GODSILL S， ANDRIEU C. On sequential Monte Carlo methods for Bayesian filtering[J]. Statistics and Computing，2000，10:197-208.

[3]CARPENTER J， CLIFFORD P， FEARNHEAD P. Improved particle filter for nonlinear problems[J]. IEEE Proc. Radar，Sonar Navig.， 1999:146(1):1-7.

[4]DOUCET A， FREITAS de， GORDON N， et al. Sequential Monte Carlo methods in practice [M]. New York: Springer Verlag， 2001.

[5]BERZUMI C， BEST N G， GILKS W R， et a1. Dynamic conditional independence models and Markov chain Monte Carlo methods[J]. Amer. Statist Assoc.， 1997， 92: 1403-1412.

[6]GORDON N J， SALMOND D J， SMITH A F M. A novel approach to non-linear and non-Gaussian Bayesian state estimation[C]//Proc. Inst. Elect. Eng. F. [S.l.]: IEEE， 1993， 140: 107-113.

[7]FEARNHEAD P. Sequential Monte Carlo methods in filter theory [D]. Oxford， UK: Merton College， Univ. Oxford，1998.

[8]GODSILL Simon， CLAPP Tim. Improvement strategies for Monte Carlo particle filter [D].Cambridge: Cambridge University.1998.

[9]李冰冰.基于粒子濾波的目標跟蹤算法的應用研究[D].長春:東北師范大學，2007.

[10]LIU J S， CHEN R. Sequential Monte Carlo methods for dynamic systems[J]. Journal of American Statistician. 1998，83:1032-1044.

[11]TI.TMS320VC5509A fixed-point digital signal processor data manual[M]. Texas: TI Inc.， 2003:18-56.

[12]趙加祥，佟吉剛，嚴建功，等.DSP系統設計和BIOS編程及應用實例[M].北京:機械工業出版社，2008.

[13]劉偉，劉洋，焦淑紅.基于Matlab 7.0軟件的實時數據交換的實現[J].國外電子元器件，2006(3):12-15.

[14]陳曙光，袁德明.基于MATLAB與CCS的聯合算法仿真[J].電子工程師，2005，31(11):45-47.

[15]梅志紅，趙莉.基于CCS環境和MATLAB仿真的FIR數字濾波器實現[J].電氣電子教學學報，2005，27(3):44-47.

[16]MERWE R Van der. A doucet the unscented particle filter: advances in neural Information processing systems [M]. [S.l.]: MIT， 2000.