粒子濾波

基本思想

所謂粒子濾波就是指：通過尋找一組在狀態空間中傳播的隨機樣本來近似的表示概率密度函數，用樣本均值代替積分運算，進而獲得系統狀態的最小方差估計的過程，這些樣本被形象的稱為“粒子”，故而叫粒子濾波。采用數學語言描述如下: 對於平穩的隨機過程, 假定k - 1 時刻系統的后驗概率密度為p ( xk-1 zk-1 ) , 依據一定原則選取n 個隨機樣本點, k 時刻獲得測量信息后, 經過狀態和時間更新過程, n 個粒子的后驗概率密度可近似為p ( xk zk ) 。隨着粒子數目的增加, 粒子的概率密度函數逐漸逼近狀態的概率密度函數, 粒子濾波估計即達到了最優貝葉斯估計的效果。

粒子濾波算法擺脫了解決非線性濾波問題時隨機量必須滿足高斯分布的制約條件, 並在一定程度上解決了粒子數樣本匱乏問題, 因此近年來該算法在許多領域得到成功應用。

貝葉斯濾波

動態系統的狀態空間模型可描述為

其中f（）,h（）分別為狀態轉移方程與觀測方程，xk為系統狀態，yk為觀測值，uk為過程噪聲，vk為觀測噪聲。為了描述方便，用Xk=x0:k={x0,x1,…,xk}與Yk=y0:k={y0,y1,…,yk}分別表示0到k時刻所有的狀態與觀測值。在處理目標跟蹤問題時，通常假設目標的狀態轉移過程服從一階馬爾可夫模型，即當前時刻的狀態xk只與上一時刻的狀態xk-1有關。另外一個假設為觀測值相互獨立，即觀測值yk只與k時刻的狀態xk有關。

貝葉斯濾波為非線性系統的狀態估計問題提供了一種基於概率分布形式的解決方案。貝葉斯濾波將狀態估計視為一個概率推理過程，即將目標狀態的估計問題轉換為利用貝葉斯公式求解后驗概率密度p(Xk|Yk)或濾波概率密度p(xk|Yk)，進而獲得目標狀態的最優估計。貝葉斯濾波包含預測和更新兩個階段，預測過程利用系統模型預測狀態的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到后驗概率密度。

假設已知k-1時刻的概率密度函數為p(xk-1|Yk-1)，貝葉斯濾波的具體過程如下：

(1) 預測過程，由p(xk-1|Yk-1)得到p(xk|Yk-1)：

p(xk,xk-1|Yk-1)=p(xk|xk-1,Yk-1)p(xk-1|Yk-1)       

當給定xk-1時，狀態xk與Yk-1相互獨立，因此

p(xk,xk-1|Yk-1)=p(xk|xk-1)p(xk-1|Yk-1)              

上式兩端對xk-1積分，可得Chapman-Komolgorov方程

(2) 更新過程，由p(xk|Yk-1)得到p(xk|Yk)：

獲取k時刻的測量yk后，利用貝葉斯公式對先驗概率密度進行更新，得到后驗概率

假設yk只由xk決定，即

p(yk|xk,Yk-1)=p(yk|xk)                      

因此

                     

其中，p(yk|Yk-1)為歸一化常數

               

貝葉斯濾波以遞推的形式給出后驗(或濾波)概率密度函數的最優解。目標狀態的最優估計值可由后驗(或濾波)概率密度函數進行計算。通常根據極大后驗(MAP)准則或最小均方誤差(MMSE)准則，將具有極大后驗概率密度的狀態或條件均值作為系統狀態的估計值，即

                                                            

貝葉斯濾波需要進行積分運算，除了一些特殊的系統模型（如線性高斯系統，有限狀態的離散系統）之外，對於一般的非線性、非高斯系統，貝葉斯濾波很難得到后驗概率的封閉解析式。因此，現有的非線性濾波器多采用近似的計算方法解決積分問題，以此來獲取估計的次優解。在系統的非線性模型可由在當前狀態展開的線性模型有限近似的前提下，基於一階或二階Taylor級數展開的擴展Kalman濾波得到廣泛應用。在一般情況下，逼近概率密度函數比逼近非線性函數容易實現。據此，Julier與Uhlmann提出一種Unscented Kalman濾波器，通過選定的sigma點來精確估計隨機變量經非線性變換后的均值和方差，從而更好的近似狀態的概率密度函數，其理論估計精度優於擴展Kalman濾波。獲取次優解的另外一中方案便是基於蒙特卡洛模擬的粒子濾波器。

蒙特卡洛近似思想

蒙特卡洛積分是一項用隨機釆樣來估算積分值的技術，簡單說就是以某事件出現的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率如P(x)的地方，一概對變量x采樣，以大量采樣及其相應的權值來近似表示P(x)。

然而，在計算機問世以前，隨機試驗卻受到一定限制，這是因為要使計算結果的准確度足夠高，需要進行的試驗次數相當大。因此人們都認為隨機試驗要用人工進行冗長的計算，這實際上是不可能的。

隨着電子計算機的出現，利用電子計算機可以實現大量的隨機抽樣試驗，使得用隨機試驗方法解決實際問題才有了能。

它的基本思想是：為了求解數學、物理、工程技術以及生產管理等方面的問題，首先建立一個概率模型或隨機過程，使它的參數等於問題的解，然后通過對模型或過程的觀察或抽樣試驗來計算所求參數的統計特征，最后給出所求解的近似

值，而解的精確度可用估計值的標准誤差來表示。 

粒子濾波算法的核心思想便是利用一系列隨機樣本的加權和表示后驗概率密度，通過求和來近似積分操作。

重要性采樣 (Importance Sampling)

這是一種蒙特卡洛方法(Monte Carlo)。在有限的采樣次數內，盡量讓采樣點覆蓋對積分貢獻很大的點。該方法目的並不是用來產生一個樣本的，而是求一個函數的定積分的，只是因為該定積分的求法是通過對另一個叫容易采集分布的隨機采用得到的。

序貫重要性采樣(Sequential Importance Sainpling,SIS)

在粒子濾波算法中重采樣的基本思想是拋棄那些對估計系統后驗概率密度權值較小的粒子,通過復制權值較大的粒子來代替它們,進行重釆樣有多種方法,常見的有多項式重釆樣、系統重采樣、分層抽樣、殘差抽樣等,目前最為常用的重采樣方法就是序貫重要性釆樣。

在基於重要性重采樣的蒙特卡洛模擬方法中,估計后驗濾波概率需要利用所有的觀測數據,每次新的觀測數據來到都需要重新計算整個狀態序列的重要性權值。序貫重要性釆樣作為粒子濾波的基礎,它將統計學的序貫分析方法應用到蒙特卡洛方法中,其主要思想就是遞歸的計算重采樣粒子,通過保留以前所有采樣時刻的粒子狀態以保持采樣粒子的繼承性,從而實現后驗濾波概率密度的遞歸估計。

基於序貫蒙特卡洛模擬方法的粒子濾波算法存在采樣粒子權值退化問題以及計算量過大的問題,在實際計算中,經過數次迭代,只有少數粒子的權值較大,其余粒子的權值較小甚至可以忽略不計,粒子權值的方差隨着時間的推移而增大,狀態空間中的有效粒子數減少,隨着無效粒子數量的增加,使得大量的計算消耗在對估計后驗濾波概率分布幾乎不起作用的粒子上,使得計算量增大且估計性能下降。

SIS算法的具體步驟如下:

1.系統初始化

k = 0,隨機提取N個樣本點

並給采樣粒子賦予相應的初始權值為

2.抽取N個樣本

在時刻k=1,2,…,L通過隨機釆樣的方法從以下分布抽取N個樣本

3.逐點計算對應的p(xk|xk-1)和p(zk|xk)

4.更新權值

該時刻的采樣粒子點的權值按下式計算

3.粒子權值歸一化

權值退化

粒子濾波的核心思想是系統隨機釆樣與重要性重釆樣的相加,粒子濾波首先根據系統在t時刻的系統狀態x(t)和系統的概率分布生成大量的采樣,這些采樣就稱之為粒子,那么這些采樣在狀態空間中的分布實際上就是x(t)的概率分布了,其后根據系統狀態轉移方程就可以對每一個粒子得到一個預測粒子,所有的預測粒子就代表系統所涉及的參數化東西。在濾波的預估計階段,隨機進行粒子采樣,采樣完成后根據特征相似度計算每個粒子的重要性,並根據重要性賦予粒子相應的權值,因此粒子濾波方法較之蒙特卡洛方法計算量較小,但是這種方法嚴重依賴於對初始狀態的估計,隨着迭代算法的進行,在經過數次迭代后其粒子采樣的分配權重的方差會隨着時間的推移而不斷增大,除少數粒子以外的其他所有粒子只具有微小的權值並最終趨向於0,而少數粒子的權值會變的越來越大,隨着采樣過程的繼續,最終所產生的粒子只是最初一小部分粒子權值的衍生,采樣粒子代表的系統就會失去多樣性,而當前系統的后驗概率密度分布情況不能夠有效的表示,我們將其稱之為權值退化問題。

由於采樣粒子權值退化問題在基於序貫重要性采樣的粒子濾波算法中廣泛存在,在經過數次迭代之后,許多粒子的權值變得很小,大量的計算浪費在小權值的粒子上,從而影響系統效率。目前降低該問題影響的最有效方法是選擇重要性函數和采用重采樣方法

重要性函數的選擇

在重要性概率密度函數選擇中融入系統最新的觀測信息,通過選取好的重要性概率密度函數來有效抑制退化問題。

重采樣

通過對粒子及其相應權值表示的概率密度函數進行重新采樣,形成一個新的粒子集,該粒子集構成后驗密度離散近似的一個經驗分布,在釆樣總數保持不變的情況下,權值較大的樣本被多次復制,從而實現重釆樣過程,通過引入重釆樣技術,增加了權值較大的粒子數,在一定程度上能夠有效解決粒子權值退化問題。重采樣過程是以犧牲系統魯棒性和計算效率來解決粒子權值退化問題的,並不是所有狀態下的粒子都需要進行重采樣,因此,我們需要進一步研究粒子權值的退化程度如何進行准確度量,並對粒子是否需要重采樣進行有效判斷。

重新采樣獲得的樣本可以認為是一系列獨立同分布的樣本，其權重是一樣的，均設為1/N。

采樣貧乏

由於包含了大量重復的粒子（這些粒子具有高權值被多次選中），重采樣導致了粒子多樣性的喪失，此類問題在小噪聲的環境下更加突出。事實上如果系統噪聲非常小的話，所有粒子會在幾次迭代之后崩塌為一個單獨的粒子。

經典粒子濾波算法步驟

1.初始化：

取k＝0，按p(x0)抽取N個樣本點，i＝1,…,N。

2.重要性采樣：

令，其中i＝1,…,N。

3.計算權值：

若采用一步轉移后驗狀態分布，該式可簡化為。

4.歸一化權值：

t=   

=/t

5.重采樣：根據各自歸一化權值的大小復制/舍棄樣本，得到N個近似服從分布的樣本。令＝1/N，i＝1,…,N。

6.輸出結果：算法的輸出是粒子集，用它可以近似表示后驗概率和函數的期望

7.K=K+1,重復2步至6步。

粒子濾波其實質是利用一系列隨機抽取的樣本，也就是粒子，來替代狀態的后驗概率分布。當粒子的個數變得足夠大時，通過這樣的隨機抽樣方法就可以得到狀態后驗分布很好的近似。

<span style="font-size:18px;">function ParticleEx1 
 
% Particle filter example, adapted from Gordon, Salmond, and Smith paper. 
 
x = 0.1; % initial state 
Q = 1; % process noise covariance 
R = 1; % measurement noise covariance 
tf = 50; % simulation length 
 
N = 100; % number of particles in the particle filter 
 
xhat = x; 
P = 2; 
xhatPart = x; 
 
% Initialize the particle filter. 
for i = 1 : N 
    xpart(i) = x + sqrt(P) * randn; 
end 
 
xArr = [x]; 
yArr = [x^2 / 20 + sqrt(R) * randn]; 
xhatArr = [x]; 
PArr = [P]; 
xhatPartArr = [xhatPart]; 
 
close all; 
 
for k = 1 : tf 
    % System simulation 
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%狀態方程 
    y = x^2 / 20 + sqrt(R) * randn;%觀測方程 
    % Extended Kalman filter 
    F = 0.5 + 25 * (1 - xhat^2) / (1 + xhat^2)^2; 
    P = F * P * F' + Q; 
    H = xhat / 10; 
    K = P * H' * (H * P * H' + R)^(-1); 
    xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));%預測 
    xhat = xhat + K * (y - xhat^2 / 20);%更新 
    P = (1 - K * H) * P; 
    % Particle filter 
    for i = 1 : N 
        xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; 
        ypart = xpartminus(i)^2 / 20; 
        vhat = y - ypart;%觀測和預測的差 
        q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); 
    end 
    % Normalize the likelihood of each a priori estimate. 
    qsum = sum(q); 
    for i = 1 : N 
        q(i) = q(i) / qsum;%歸一化權重 
    end 
    % Resample. 
    for i = 1 : N 
        u = rand; % uniform random number between 0 and 1 
        qtempsum = 0; 
        for j = 1 : N 
            qtempsum = qtempsum + q(j); 
            if qtempsum >= u 
                xpart(i) = xpartminus(j); 
                break; 
            end 
        end 
    end 
    % The particle filter estimate is the mean of the particles. 
    xhatPart = mean(xpart); 
    % Plot the estimated pdf's at a specific time. 
    if k == 20 
        % Particle filter pdf 
        pdf = zeros(81,1); 
        for m = -40 : 40 
            for i = 1 : N 
                if (m <= xpart(i)) && (xpart(i) < m+1) 
                    pdf(m+41) = pdf(m+41) + 1; 
                end 
            end 
        end 
        figure; 
        m = -40 : 40; 
        plot(m, pdf / N, 'r'); 
        hold; 
        title('Estimated pdf at k=20'); 
        disp(['min, max xpart(i) at k = 20: ', num2str(min(xpart)), ', ', num2str(max(xpart))]); 
        % Kalman filter pdf 
        pdf = (1 / sqrt(P) / sqrt(2*pi)) .* exp(-(m - xhat).^2 / 2 / P); 
        plot(m, pdf, 'b'); 
        legend('Particle filter', 'Kalman filter'); 
    end 
    % Save data in arrays for later plotting 
    xArr = [xArr x]; 
    yArr = [yArr y]; 
    xhatArr = [xhatArr xhat]; 
    PArr = [PArr P]; 
    xhatPartArr = [xhatPartArr xhatPart]; 
end 
 
t = 0 : tf; 
 
%figure; 
%plot(t, xArr); 
%ylabel('true state'); 
 
figure; 
plot(t, xArr, 'b.', t, xhatArr, 'k-', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:'); 
axis([0 tf -40 40]); 
set(gca,'FontSize',12); set(gcf,'Color','White');  
xlabel('time step'); ylabel('state'); 
legend('True state', 'EKF estimate', '95% confidence region');  
 
figure; 
plot(t, xArr, 'b.', t, xhatPartArr, 'k-'); 
set(gca,'FontSize',12); set(gcf,'Color','White');  
xlabel('time step'); ylabel('state'); 
legend('True state', 'Particle filter estimate');  
 
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf); 
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf); 
disp(['Kalman filter RMS error = ', num2str(xhatRMS)]); 
disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]); </span>

下面是我實現的跟蹤結果，白色十字代表粒子，綠色大十字代表目標最可能的位置。

版權聲明：