现代滤波_粒子滤波学习笔记（一）

本文主要对粒子滤波（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

仿真结果如下图所示：