六、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
- 粒子有了,每个粒子权重有了,进行状态估计
- 重采样
在上面算法中,每进行一次,都必须重采样一次,这是由于权重的计算方式决定的。
分析上面算法中的采样,发现它并没有加入测量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》