【直观理解】粒子滤波 原理及实现

该博文集成了几个重要的参考博客,首先感谢这些博主的讲解和实现,因此是转载,不是原创。

一. 首先从通俗易懂的层面来理解一下粒子滤波,主要是博主(饮水思源)的博客。

粒子滤波可以先分为几个主要的阶段:初始化阶段 ---> 预测阶段--->矫正阶段--->重采样--->滤波

初始化阶段:主要就是选定粒子数量。也就是博主所说的放狗去搜索目标;放狗的方式有很多中,一种是让他们均匀分布,第二种是让他们按照高斯分布,即可能性大的地方就多放一点。一般来讲,比较简单的实现都是先让粒子们均匀分布在一个区域,但是在实际应用中,比如目标跟踪,SLAM中的特征跟踪都是先让这些粒子们呈高斯分布。

预测阶段:说的是预测,其实这个阶段就是对粒子们的返回数据进行分析,比如在目标跟踪中,我的特征是一个向量,那么每个粒子(狗狗)就会返回一个同样大小的向量回来,那么我可以计算这些向量之间的相似度;当然在实际应用中,为了方便比较,所有粒子的相似度之和最好等于1,因此我们需要对这些返回的相似度做归一化处理。

决策阶段: 在这个阶段,首先要知道每个狗狗身上都带着什么,狗狗看到的东西,以及狗狗们的可信度;对应到粒子上也就是每个粒子的返回值(或返回向量)以及这个返回值的置信度;当所有的狗狗们都带着消息过来时,主要就要有所选择,这个部分类似于决策树,可以选择相信置信度最大的分支,也可以考虑所有分支的信息;粒子滤波在这里使用的就是第二种,综合考虑所有的信息,那么最终的结果,也就是预测值x_hat就是这些 信息×置信度 之和;如果你需要预测的变量是坐标,那么预测值x_hat = p1*x1 + p2*x2+...pn*xn; x1..xn是每个狗狗返回的坐标值,p1...pn是每个狗狗消息的可信度;

重采样阶段:在上一步中得到的数值其实还是有很大的误差,所以接下来要做的是进一步的估计,依然是让狗狗们去搜索信息,但是怎样去分布狗狗们呢?还是按照之前的那种均匀分布吗?这次肯定要换别的方法,也就是按照狗狗的可信度,可信度高的狗狗所报告的位置附近就多派一点狗狗过去,而其他可信度比较低的地方,就少派一点过去;也就是博主们所说的根据重要性重采样;Sampling Importance Resampling;

最后,可以再次重复地进行决策阶段和重采样阶段,直到最终的狗狗们的反馈结果符合条件(误差值较小等等),这个时候,根据权重和置信度计算出来的值,也就是我们最终的预测值;

接下来看一个博主的matlab实现:

clc;
clear all;
close all;
x = 0; %初始值
R = 1;
Q = 1;
tf = 100; %跟踪时长
N = 100;  %粒子个数
P = 2;
xhatPart = x;
for i = 1 : N    
    xpart(i) = x + sqrt(P) * randn;%初始状态服从0均值,方差为sqrt(P)的高斯分布
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
for k = 1 : tf    

x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %k时刻真实值
y = x^2 / 20 + sqrt(R) * randn;  %k时刻观测值
 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;%采样获得N个粒子
     ypart = xpartminus(i)^2 / 20;%每个粒子对应观测值
     vhat = y - ypart;%与真实观测之间的似然
     q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); 
     %每个粒子的似然即相似度
 end
 qsum = sum(q);
for i = 1 : N
    q(i) = q(i) / qsum;%权值归一化
end  
%----重采样阶段
  for i = 1 : N %根据权值重新采样
      u = rand;
      qtempsum = 0;
      for j = 1 : N
         qtempsum = qtempsum + q(j);
          if qtempsum >= u
              xpart(i) = xpartminus(j);
              break;
          end
      end
  end
xhatPart = mean(xpart);
%最后的状态估计值即为N个粒子的平均值,这里经过重新采样后各个粒子的权值相同
xArr = [xArr x];   
yArr = [yArr y];  
% xhatArr = [xhatArr xhat]; 
PArr = [PArr P]; 
xhatPartArr = [xhatPartArr xhatPart];
end
t = 0 : tf;
figure;
plot(t, xArr, 'b-.', t, xhatPartArr, 'k-');
legend('Real Value','Estimated Value');
set(gca,'FontSize',10); 
xlabel('time step'); 
ylabel('state');
title('Particle filter')
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
figure;
plot(t,abs(xArr-xhatPartArr),'b');
title('The error of PF')

 

 

 

展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客
应支付0元
点击重新获取
扫码支付

支付成功即可阅读