現代濾波_粒子濾波學習筆記（一）

本文主要對粒子濾波（Particle Filter）的要點進行概要性的闡述，體現粒子濾波理論的基本衍生過程及基本原理，並在最後給出序貫重要性重採樣算法的一般步驟。

1. 貝葉斯濾波（Bayesian Filter）

1.1 基本原理

將狀態估計視爲一個概率推理過程，即將目標狀態的估計問題轉換爲利用貝葉斯公式求解後驗概率密度 $p(X_{k}|Y_{k})$ 或濾波概率密度 $p(x_{k}|Y_{k})$ ，進而獲得目標的最優估計。

根據最小均方誤差（MMSE），得出最優估計爲

MMSE: $\widehat{F(x_{k})}=E[F(x_{k})|Y_{k}]=\int F(x_{k})p(x_{k}|Y_{k})dx_{k}$

其中 $F(x_{k})$ 爲任意函數。

1.2 存在問題

貝葉斯濾波需要進行積分計算，除了一些特殊的系統模型，如線性高斯系統、有限狀態的離散系統之外，對於一般的非線性、非高斯系統，貝葉斯濾波很難得到後驗概率密度函數的封閉解析式。因此，現有的非線性濾波器多采用近似的計算方法解決積分問題，依次來估計次優解。爲解決這個問題，人們引入了蒙特卡洛（Monte Carlo）模擬方法。

2. 蒙特卡洛（Monte Carlo）方法

2.1 基本原理

蒙特卡洛（Monte Carlo）模擬方法利用所求狀態空間中大量的樣本點來近似逼近帶估計量的後驗概率分佈，從而將積分問題轉換爲有限樣本點求和問題。

MMSE: $\widehat{F(x_{k})}=E[F(x_{k})|Y_{k}]=\int F(x_{k})p(x_{k}|Y_{k})dx_{k}=\tfrac{1}{N}\sum_{i=1}^{N}F(x_{k}^{(i)})$

其中 $F(x_{k})$ 爲任意函數， $x_{k}^{(i)}$ 爲在後驗概率密度 $p(x_{k}^{(i)}|Y_{k})$ 中獲取的採樣粒子。

2.2 存在問題

在實際計算中，通常是無法直接從後驗概率分佈中採樣，如何得到服從後驗概率分佈的隨機樣本是蒙特卡洛中基本的問題之一。爲解決這個問題，人們引入了重要性採樣方法。

3. 序貫重要性採樣（Sequential Importance Sampling, SIS）

3.1 基本原理

序貫重要性採樣（Sequential Importance Sampling, SIS）作爲粒子濾波的基礎，它將統計學中的序貫分析方法應用到的蒙特卡洛方法中，從而實現後驗（濾波）概率密度的遞推估計。從重要性概率密度函數 $q(x_{k}^{(i)}|x_{k-1}^{(i)},y_{k})$ 中生成採樣粒子，並隨着測量值的依次到來遞推求得相應的權值，最終以粒子加權和的形式來描述後驗濾波概率密度，進而得到狀態估計。

MMSE: $\widehat{F(x_{k})}=E[F(x_{k})|Y_{k}]=\int F(x_{k})p(x_{k}|Y_{k})dx_{k}=\sum_{i=1}^{N}F(x_{k}^{(i)})w_{k}^{(i)}$

其中 $F(x_{k})$ 爲任意函數， $x_{k}^{(i)}$ 爲在後驗概率密度 $q(x_{k}^{(i)}|x_{k-1}^{(i)},y_{k})$ 中獲取的採樣粒子， $w_{k}^{(i)}$ 爲隨着測量值的依次到來遞推求得相應的歸一化權值。

3.2 存在問題

爲了得到正確的狀態估計，通常希望粒子權值的方差儘可能趨近於零。然而，序貫蒙特卡洛模擬方法一般都存在權值退化問題。在實際計算中，經過數次迭代，只有少數粒子的權值較大，其餘粒子的權值可忽略不計。粒子權值的方差隨着時間增大，狀態空間中的有效粒子數較少。隨着無效採樣粒子數目的增加，使得大量的計算浪費在對估計後驗濾波概率分佈幾乎不起作用的粒子更新上，使得估計性能下降。

4. 重要性概率密度函數的選擇

4.1 工程常用

工程上常採用如下重要性概率密度函數：

$q(x_{k}^{(i)}|x_{k-1}^{(i)},y_{k})=p(x_{k}^{(i)}|x_{k-1}^{(i)})$

此時，粒子權重爲：

$w_{k}^{(i)}\propto w_{k-1}^{(i)}p(y_{k}|x_{k}^{(i)})$

轉移概率的形式簡單且易於實現，在觀測精度不高的場合，將其作爲重要性概率密度函數可以取得較好的濾波效果。然而，採用轉移概率密度函數作爲重要性概率密度函數沒有考慮最新觀測數據所提供的信息，從中抽取的樣本與真實後驗分佈產生的樣本存在一定的偏差，特別是當觀測模型具有較高的精度或預測先驗與似然函數之間重疊部分較少時，這種偏差尤爲明顯。

4.2 最優選擇

最優選擇採用如下重要性概率密度函數：

$q(x_{k}^{(i)}|x_{k-1}^{(i)},y_{k})=p(x_{k}^{(i)}|x_{k-1}^{(i)},y_{k-1})$

此時，粒子權重爲：

$w_{k}^{(i)}\propto w_{k-1}^{(i)}p(y_{k}|x_{k-1}^{(i)})$

在實際情況中，構造最優重要性概率密度函數的困難程度與直接從後驗概率分佈中抽取樣本的困難程度等同。

5. 序貫重要性重採樣（Sequential Importance Resampling, SIR）

5.1 基本原理

針對序貫重要性採樣算法存在的權值退化問題，由Gordon等提出了一種名爲Bootstrap的粒子濾波算法。該算法在每步迭代過程中，根據粒子權值對離散粒子進行重採樣，在一定程度上克服了這個問題。

重採樣的方法分爲:

(1) 固定時間間隔的靜態重採樣

(2) 根據粒子權值進行的動態重採樣

動態重採樣通常根據當前的有效粒子數或最大與最小權值比來判斷是否需要進行重採樣。

常用的動態重採樣方法包括：

a. 多項式重採樣(Multinomial Resampling)

b. 殘差重採樣(Residual Resampling)

c. 分層重採樣(Stratified Resampling)

d. 系統重採樣(Systematic Resampling)

5.2 存在問題

粒子濾波中的權值退化問題是不可避免的。雖然重採樣方法可以在一定程度上緩解權值退化現象，但重採樣方法也會帶來一些其它的問題。重採樣需要綜合所有的粒子才能實現，限制了粒子濾波的並行計算。另外，根據重採樣的原則，粒子權值較大的粒子必然會更多的被選中複製，經過若干步迭代後，必然導致相同的粒子越來越多，粒子將缺乏多樣性，可能出現粒子退化現象，從而使狀態估計產生較大偏差。針對粒子退化的問題，學術界相繼提出馬爾可夫蒙特卡洛（Markov Chain Monte Carlo,MCMC）方法和正則化粒子濾波方法。

6. ISR算法標準流程

(1) 粒子集初始化：

對於，由先驗 $p(x_{0})$ 生成採樣粒子 $\left \{ x_{0}^{(i)} \right \}_{i=1}^{N}$ ，。

(2) 對於，循環執行以下步驟：

① 重要性採樣（以工程常用選擇爲例）

從重要性概率密度 $p(x_{k}^{(i)}|x_{k-1}^{(i)})$ 中生成採樣粒子 $\left \{ x_{k}^{(i)} \right \}_{i=1}^{N}$ ，;

根據 $p(y_{k}|x_{k}^{(i)})$ 計算粒子權值，生成 $\left \{ w_{k}^{(i)} \right \}_{i=1}^{N}$ ，，並進行歸一化；

② 重採樣

對粒子集 $\left \{ x_{k}^{(i)} ,w_{k}^{(i)}\right \}$ 進行重採樣，重採樣後的粒子集爲 $\left \{x_{k}^{(i)}, \frac{1}{N}\right \}$ ；

③ 輸出

計算時刻的狀態估計值： $\widehat{x}_{k}^{(i)} =\sum_{i=1}^{N}w_{k}^{(i)}x_{k}^{(i)$ 。

7. 仿真

使用Matlab的代碼如下：

close all 
clear all      
clc    
%% Moden description
    % System state equation
    % x(k) = f(x(k-1)) + v(k)
    % System watching equation
    % y(k) = h(x(k)) + n(k)
    % Transfer function
    % f(x(k-1)) = (1/2)*x(k-1) + 25*x(k-1)/(1 + x(k-1)^2) + 8*cos(1.2*(k-1))
    % System process noise obeyed standard normal distribution. 
    % v(k) = sqrt(v_Cov)*randn
    % Watching function
    % h(x(k)) = x(k)^2/20    
    % Watching process noise obeyed standard normal distribution. 
    % n(k) = sqrt(n_Cov)*randn  
%% Initialize the variables    
x = 0.1; % Initial state    
v_Cov = 1; % The covariance of system process nosie vetcor.  
n_Cov = 1; % The covariance of watching nosie vetcor.
         % Attention: 
         % As 1 dimension vector here, the covariance just is the variance. 
t_Sam = 75;  % The sampling times.    
p_Num = 100; % The number of particles.
         % Attention: 
         % The Number is bigger the poformance better, witch will increase 
         % computational cost at the same time. 

x_Cov = 2;          % The initialized covariance of state vetcor.   
P_sta = zeros(1,p_Num); % The particles vector of  
% Initialized particles at random by Gaussian Distribution. 
for i = 1:p_Num    
    P_sta(i) = x + sqrt(x_Cov)*randn;    
end   
x_est = mean(P_sta);  % The estimated state value.

X_sta = zeros(1,t_Sam+1); % The state vector from system state equation.
Y_wat = zeros(1,t_Sam+1); % The watching vector from system watching equation.
X_est = zeros(1,t_Sam+1); % The state estimate vector of Particle Filter.
X_sta(1) = x; % The initialized state value in state vector.  
Y_wat(1) = x^2/20 + sqrt(n_Cov)*randn; % The initialized watching value in watching vector. 
X_est(1) = X_sta(1); % The initialized state estimate value in watching vector.   
   
P_is = zeros(1,p_Num); % The Importacnce Sampling vector of Particle Filter.
P_wei = zeros(1,p_Num); % The weight vector of particle vector.
Y_pre = zeros(1,p_Num); % The predicted value vector.
%% SIR Particle Filter
for t = 1:t_Sam    
    % Update the x(k) and y(k)
    x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) +  sqrt(v_Cov)*randn; 
    y = x^2/20 + sqrt(n_Cov)*randn;
    
    % Implement the Importance Sampling
    for i = 1:p_Num    
        % Make p(x(i,k)|x(i,k-1)) as the importance probability density function,
        % and complete the Imortance Sampling.
        P_is(i) = 0.5*P_sta(i) + 25*P_sta(i)/(1 + P_sta(i)^2) + 8*cos(1.2*(t-1)) + sqrt(v_Cov)*randn; 
        % Calculate y(k) predicted value by x(i,k).  
        Y_pre(i) = P_is(i)^2/20;    
        % Calculate p(y(k)|x(i,k)) by Gaussian Distribution.
        P_wei(i) = (1/sqrt(2*pi*n_Cov)) * exp(-(y - Y_pre(i))^2/(2*n_Cov));    
    end
    % Particle weight should be normalized.
    P_wei = P_wei ./ sum(P_wei);    
      
    % Implement the Importance Resampling    
    for i = 1 : p_Num
        % The particles with bigger weight should generate more offsprings
        P_sta(i) = P_is(find(rand <= cumsum(P_wei),1));
    end    
     
    %Implement the estimatation of state.
    %x_est = mean(P_sta);
    x_est = P_is * P_wei';
    % Attention:
    % After resampling, every particl's weight is changed to 1/p_Num.
      
    % Save data in arrays for later plotting    
    X_sta(t+1) = x;    
    Y_wat(t+1) = y;    
    X_est(t+1) = x_est;        
end    

%% Plotting 
t = 0:t_Sam;    
figure(1);    
clf    
plot(t, X_sta, '.-b',t, Y_wat, '-*g', t, X_est, '-.r','linewidth',3);    
set(gca,'FontSize',12); set(gcf,'Color','White');    
xlabel('time step'); ylabel('flight position');    
legend('True flight position', 'Watching flight position', 'Particle filter estimate');
grid on

仿真結果如下圖所示：