基于DSP的改進(jìn)粒子濾波算法研究

摘 要:介紹了粒子濾波基本原理，針對(duì)粒子濾波計(jì)算量大和難以用硬件實(shí)現(xiàn)等缺點(diǎn)對(duì)粒子濾波算法進(jìn)行了改進(jìn)，使其平均計(jì)算周期縮短為原來的90%，應(yīng)用DSP實(shí)現(xiàn)了粒子濾波算法。改進(jìn)粒子濾波算法主要優(yōu)化了原粒子濾波算法中權(quán)值計(jì)算、重采樣和輸出步驟，使其計(jì)算速度和濾波精度有所提高。這種改進(jìn)粒子濾波算法在DSP系統(tǒng)中進(jìn)行仿真，結(jié)果證明它具有速度快，精度高的優(yōu)點(diǎn)。關(guān)鍵詞:粒子濾波; 硬件實(shí)現(xiàn); 權(quán)值計(jì)算; 數(shù)字信號(hào)處理器

中圖分類號(hào):TN911-34文獻(xiàn)標(biāo)識(shí)碼:A

文章編號(hào):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)為基礎(chǔ)的序貫Monte Carlo(sequential MC，SMC)方法，因此又稱為SMC濾波，1999年正式提出PF稱謂[3]，該名稱現(xiàn)已廣泛采用。由于PF算法在理論上對(duì)高維非線性、非高斯動(dòng)態(tài)系統(tǒng)的狀態(tài)遞推估計(jì)或概率推理等問題都不具敏感性，因此，在復(fù)雜問題的求解上它表現(xiàn)出突出的優(yōu)勢(shì)。但是到目前為止，在實(shí)時(shí)信號(hào)處理領(lǐng)域，粒子濾波算法幾乎沒有得到實(shí)際應(yīng)用，這主要是因?yàn)榱Ｗ訛V波算法本身較復(fù)雜，運(yùn)算量大，需要存儲(chǔ)的空間大。某些改進(jìn)粒子濾波算法雖然在一定程度上提高了粒子濾波算法的精度，卻使得粒子濾波算法更加復(fù)雜，實(shí)時(shí)性很差。

在此，首先介紹了標(biāo)準(zhǔn)粒子濾波算法，之后從硬件實(shí)現(xiàn)的角度出發(fā)，將粒子濾波權(quán)值計(jì)算中的權(quán)值歸一化部分合并到重采樣計(jì)算和輸出計(jì)算步驟中，并且改進(jìn)了權(quán)值計(jì)算方法，以非線性非高斯系統(tǒng)為例，驗(yàn)證了改進(jìn)算法的精度，結(jié)果說明，改進(jìn)算法更適合硬件實(shí)現(xiàn)，一定程度上在提高了算法運(yùn)算速度的同時(shí)，提高了算法的濾波精度。

1 粒子濾波算法及復(fù)雜度分析

1.1 粒子濾波算法

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

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

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

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

假定初始先驗(yàn)概率密度函數(shù)p(x0/z0)=p(x0)，已知觀測(cè)值z(mì)0:k={zi:i=0，1，2，…，n}，則后驗(yà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時(shí)刻的第m個(gè)粒子;δ(#8226;)為狄拉克函數(shù);w(m)k為k時(shí)刻第m個(gè)粒子的權(quán)值。

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

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

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

(1) 采樣:如果能夠直接從先驗(yàn)概率p(x)中產(chǎn)生粒子，則每個(gè)粒子初始化時(shí)的權(quán)值為1/M。當(dāng)無法從p(x)中產(chǎn)生粒子時(shí)，可以從重要性函數(shù)π(x)中采樣產(chǎn)生粒子x(m)k。

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

(2) 權(quán)值計(jì)算:此步驟主要由以下兩個(gè)部分組成。

① 權(quán)值更新:

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)

② 權(quán)值歸一化:

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

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

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

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

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

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

由式(9)可得，Neff≤M，且Neff越小，退化現(xiàn)象越嚴(yán)重。因此設(shè)定一個(gè)門限值Nth，當(dāng)Neff≤Nth時(shí)，就進(jìn)行重采樣。

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

圖1 重采樣示意圖

1.2 復(fù)雜度分析

粒子濾波算法的功能框圖如圖2所示。對(duì)于輸入一個(gè)觀測(cè)值z(mì)k，粒子濾波算法執(zhí)行采樣(粒子初始化)、權(quán)值更新、權(quán)值歸一化、重采樣及輸出計(jì)算幾個(gè)步驟。粒子濾波是一個(gè)反復(fù)迭代的方法，只有重采樣結(jié)束后才能進(jìn)行下一時(shí)刻的粒子更新。

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

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

2 硬件實(shí)現(xiàn)的改進(jìn)算法

粒子濾波算法的計(jì)算量比較大，不利于應(yīng)用于實(shí)時(shí)性較高的系統(tǒng)中。本文以提高粒子濾波算法的運(yùn)算速度和濾波精度為目的，采取減少除法運(yùn)算和改進(jìn)權(quán)值計(jì)算的方法。由第1.2節(jié)對(duì)粒子濾波算法復(fù)雜度分析可知，權(quán)值歸一化增加了算法復(fù)雜度，因?yàn)槌ㄟ\(yùn)算對(duì)硬件的開銷很大，所以盡可能減少除法運(yùn)算是提高粒子濾波算法運(yùn)算速度的關(guān)鍵。經(jīng)過反復(fù)研究粒子濾波算法發(fā)現(xiàn)，可以把權(quán)值歸一化步驟合并到權(quán)值計(jì)算、結(jié)果輸出和重采樣等幾個(gè)步驟中，從而減少大量的除法運(yùn)算和降低硬件的開銷。具體改進(jìn)方法如下:

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

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

(3) 粒子濾波算法中重采樣過程產(chǎn)生的均勻分布的隨機(jī)數(shù)是在區(qū)間[0，1/M)，改進(jìn)粒子濾波算法中重采樣過程產(chǎn)生的均勻分布的隨機(jī)數(shù)是在區(qū)間[0，Sk/M)。

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

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

本文中為了減少計(jì)算量，選取先驗(yàn)概率密度為重要性函數(shù):

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

權(quán)重更新為:

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

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

(2) 采樣:從先驗(yàn)概率密度p(xk/x(m)k-1)中采樣，即對(duì)各個(gè)粒子狀態(tài)進(jìn)行更新。

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

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

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

圖3 改進(jìn)粒子濾波算法功能框圖

3 仿真實(shí)驗(yàn)和結(jié)果分析

3.1 仿真實(shí)驗(yàn)方法

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

3.2 仿真模型和實(shí)驗(yàn)結(jié)果

使用本文提出的改進(jìn)FBPF算法，對(duì)非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)進(jìn)行了試驗(yàn)，采用的系統(tǒng)模型[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);觀測(cè)噪聲nk～N(0，1)。采用FBPF算法和改進(jìn)FBPF算法進(jìn)行實(shí)驗(yàn)，仿真的數(shù)據(jù)樣點(diǎn)數(shù)為60，粒子個(gè)數(shù)為50，有效樣本容量限Nth=20。

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

圖4 PF算法濾波結(jié)果

圖5 改進(jìn)PF算法濾波結(jié)果

表1 平均運(yùn)行時(shí)間和濾波結(jié)果比較

PF算法改進(jìn)PF算法

狀態(tài)原始值與濾波值方差0.219 10.128 7

狀態(tài)原始值與加噪值方差0.811 20.877 8

觀測(cè)原始值與濾波值方差0.194 80.068 3

觀測(cè)原始值與加噪值方差1.767 91.879 4

平均運(yùn)行時(shí)間 /s0.109 70.102 2

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

4 結(jié) 語

PF算法的復(fù)雜性和龐大計(jì)算量使其運(yùn)算速度緩慢，限制了其在實(shí)際中的應(yīng)用。本文介紹了PF算法的基本原理，通過改進(jìn)權(quán)值計(jì)算步驟和重采樣步驟，優(yōu)化算法結(jié)構(gòu)，提高了運(yùn)算速度。改進(jìn)PF算法所用的時(shí)間低于PF算法，能夠應(yīng)用于實(shí)時(shí)性要求較高的系統(tǒng)中，如車輛追蹤、智能機(jī)器人、視頻監(jiān)視等方面。

參考文獻(xiàn)

[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]李冰冰.基于粒子濾波的目標(biāo)跟蹤算法的應(yīng)用研究[D].長(zhǎng)春:東北師范大學(xué)，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]趙加祥，佟吉?jiǎng)偅瑖?yán)建功，等.DSP系統(tǒng)設(shè)計(jì)和BIOS編程及應(yīng)用實(shí)例[M].北京:機(jī)械工業(yè)出版社，2008.

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

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

[15]梅志紅，趙莉.基于CCS環(huán)境和MATLAB仿真的FIR數(shù)字濾波器實(shí)現(xiàn)[J].電氣電子教學(xué)學(xué)報(bào)，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.