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

最新推荐文章于 2024-09-10 21:51:28 发布

三轮车的视觉进阶_

最新推荐文章于 2024-09-10 21:51:28 发布

阅读量4.3k

点赞数 3

分类专栏： SLAM基础算法文章标签：粒子滤波算法理论 SLAM 状态估计

原文链接：https://blog.csdn.net/qq_27923041/article/details/56008756#commentBox

版权

SLAM基础算法专栏收录该内容

23 篇文章 16 订阅

订阅专栏

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

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

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

初始化阶段：主要就是选定粒子数量。也就是博主所说的放狗去搜索目标；放狗的方式有很多中，一种是让他们均匀分布，第二种是让他们按照高斯分布，即可能性大的地方就多放一点。一般来讲，比较简单的实现都是先让粒子们均匀分布在一个区域，但是在实际应用中，比如目标跟踪，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')