Paritcle Filter Tutorial 粒子滤波：从推导到应用（四）

六、Sampling Importance Resampling Filter (SIR)

       SIR滤波器很容易由前面的基本粒子滤波推导出来，只要对粒子的重要性概率密度函数做出特定的选择即可。在SIR中，选取：

       

p( x(k)|x(k-1) )这是先验概率，在第一章贝叶斯滤波预测部分已经说过怎么用状态方程来得到它。将这个式子代入到第二章SIS推导出的权重公式中：

       

得到：

                 (1)式

由之前的重采样我们知道，实际上每次重采样以后，有。

所以(1)式可以进一步简化成：

           

上式中的 p( y(k)|x(k) )也由系统状态方程可以得到。到这里，就可以看成SIR只和系统状态方程有关了，不用自己另外去设计概率密度函数，所以在很多程序中都是用的这种方法。

下面以伪代码的形式给出SIR滤波器：

----------------------SIR Particle Filter pseudo code-----------------------------------

• FOR i = 1:N

                (1)采样粒子：

              (2)计算粒子的权重：

• END FOR
• 计算粒子权重和，t=sum(w)
• 对每个粒子，用上面的权重和进行归一化,w = w/t
• 粒子有了，每个粒子权重有了，进行状态估计     
• 重采样

-----------------------end -----------------------------------------------

在上面算法中，每进行一次，都必须重采样一次，这是由于权重的计算方式决定的。

      分析上面算法中的采样，发现它并没有加入测量y(k)。只是凭先验知识p( x(k)|x(k-1) )进行的采样，而不是用的修正了的后验概率。所以这种算法存在效率不高和对奇异点(outliers)敏感的问题。但不管怎样，SIR确实简单易用。

七、粒子滤波的应用

      在这里主要以第一章的状态方程作为例子进行演示。

         

          

在这个存在过程噪声和量测噪声的系统中，估计状态x(k)。

 %% SIR粒子滤波的应用，算法流程参见博客http://blog.csdn.net/heyijia0327/article/details/40899819
 clear all
 close all
 clc
 %% initialize the variables
 x = 0.1; % initial actual state
 x_N = 1; % 系统过程噪声的协方差  (由于是一维的，这里就是方差)
 x_R = 1; % 测量的协方差
 T = 75;  % 共进行75次
 N = 100; % 粒子数，越大效果越好，计算量也越大

 %initilize our initial, prior particle distribution as a gaussian around
 %the true initial value

 V = 2; %初始分布的方差
 x_P = []; % 粒子
 % 用一个高斯分布随机的产生初始的粒子
 for i = 1:N
     x_P(i) = x + sqrt(V) * randn;
 end

 z_out = [x^2 / 20 + sqrt(x_R) * randn];  %实际测量值
 x_out = [x];  %the actual output vector for measurement values.
 x_est = [x]; % time by time output of the particle filters estimate
 x_est_out = [x_est]; % the vector of particle filter estimates.

 for t = 1:T
     x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) +  sqrt(x_N)*randn;
     z = x^2/20 + sqrt(x_R)*randn;
     for i = 1:N
         %从先验p(x(k)|x(k-1))中采样
         x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
         %计算采样粒子的值，为后面根据似然去计算权重做铺垫
         z_update(i) = x_P_update(i)^2/20;
         %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
         P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
     end
     % 归一化.
     P_w = P_w./sum(P_w);

     %% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的
     for i = 1 : N
         x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代
     end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标

     %状态估计，重采样以后，每个粒子的权重都变成了1/N
     x_est = mean(x_P);

     % Save data in arrays for later plotting
     x_out = [x_out x];
     z_out = [z_out z];
     x_est_out = [x_est_out x_est];

 end

 t = 0:T;
 figure(1);
 clf
 plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
 set(gca,'FontSize',12); set(gcf,'Color','White');
 xlabel('time step'); ylabel('flight position');
 legend('True flight position', 'Particle filter estimate');

滤波后的结果如下：

    

       这是粒子滤波的一个应用，还有一个目标跟踪(matlab)，机器人定位(python)的例子，我一并放入压缩文件，供大家下载，下载请点击。(下载需要1个积分，下载完评论资源你就可以赚回那1积分，相当于没损失)。请原谅博主的这一点点自私。两个程序得效果如下：

      

      

      粒子滤波从推导到应用这个系列到这里就结束了。结合前面几章的问题起来看，基本的粒子滤波里可改进的地方很多，正由于此才诞生了很多优化了的算法，而这篇博客只理顺了基本算法的思路，希望有帮到大家。

     另外，个人感觉粒子滤波和遗传算法真是像极了。同时，如果你觉得这种用很多粒子来计算的方式效率低，在工程应用中不好接受，推荐看看无味卡尔曼滤波（UKF）,他是有选择的产生粒子，而不是盲目的随机产生。

      ps:今年上半年的时候就很想写这篇博客了，但是每次看到那么多公式，实在有点不愿意去敲。到最近又用到，才下了决心，希望以后自己改了这个毛病，扎扎实实的做好每一步，不眼高手低。

reference：

1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》