模糊信息粒化代码,粒子滤波算法及其应用
粒子过滤(particle filter )1)了解初始化阶段-提取要跟踪的特征在此阶段手动指定要跟踪的特征，然后程序计算要跟踪的特征。 例如，可以采用目标的颜色特征。 具体来说，Rob Hess代码必须首先手动用鼠标拖动跟踪区域，然后程序自动计算该区域的色相(Hue )空间直方图。 这就是目标的特征。 由于直方图可以由一个向量表示，目标特征是一个N*1的向量v。
2 )搜索阶段-请放开狗。 我们掌握了目标的特征。 在下面放很多狗，搜索目标对象。 这里的狗是粒子粒子。 狗有各种各样的放法。 例如，(a )均匀放置)即，在整个图像平面上均匀地散布粒子； b )根据rydbl分布放置在前一帧中得到的目标附近。 可以理解为，离目标近的地方多放，离目标远的地方少放。 Rob Hess的代码使用了后者的方法。 狗出去后，每只狗都怎么搜索目标？ 是遵循初始化阶段得到的目标特征(灰度直方图、矢量v )。 每只狗计算位于其位置的图像的颜色特征，得到色调直方图、矢量Vi，计算其直方图和目标直方图之间的相似性。 相似性有各种各样的尺度，但最简单的方法是计算sum(ABS(VI-v ) )。 为使所有狗得到的相似度为1，对每只狗计算出相似度，然后再归一化一次。
3 )在决策阶段被放生的聪明的狗给我们发来了报告。 “1号狗的图像与目标的相似度为0.3”、“2号狗的图像与目标的相似度为0.02”、“3号狗的图像与目标的相似度为0.0003”、“n号狗的图像与目标的相似度为0.013”.那么，目标在哪里呢？ 做加权平均吧。 如果第n个狗的图像的像素坐标为(Xn，Yn )，其报告的相似度为Wn，则目标的最可能像素坐标x=sum(xnwn )，y=sum ) ynwn )。
4 )重采样阶段Resampling既然我们在进行目标跟踪，一般来说目标都是来回奔跑。 在新的帧图像中，目标可能在哪里？ 还是放狗搜索吧。 但是现在应该怎么放狗？ 再检查一下狗们的报告吧。 “1号狗的图像与目标相似度为0.3”、“2号狗的图像与目标相似度为0.02”、“3号狗的图像与目标相似度为0.0003”、“n号狗的图像与目标相似度为0.013”……综合所有狗的报告，得出1号狗的结论必须重新分布这就是Sampling Importance Resampling，根据重要性重新采样(重新定位更重要的狗)。
(2) ) ((4) ) )的重复循环的话，目标的动态追踪就完成了。
5 )根据我粗略的理解，粒子滤波的中心思想是重新采样随机采样的重要性。 既然不知道目标在哪里，我就随机撒粒子吧。 撒粒子后，根据特征相似度计算每个粒子的重要性，重要的地方多撒粒子，不重要的地方少撒粒子。 因此，粒子滤波与蒙特卡罗滤波相比计算量较少。 这个思想和RANSAC算法真的很协调。 兰撒克的思想也是(例如用于最简单的直线拟合)，既然不知道直线方程是什么，就随机取两点先计算出一条直线，然后看有多少点符合我这条直线。 哪些直线受最多点的支持，哪些是目标直线？ 想法很简单，但很有效。
基于粒子滤波的定位算法——的原理、理解与仿真
1算法原理1.1机器人定位问题
关于机器人的定位，有三个大问题。 分别是以下问题
(1)“全球定位”)初始位置不明，指机器人通过自己的运动确定自己在地图中的姿态。
)2)“姿态跟踪”)指知道自身姿态，或通过“全球定位系统”获得较好的姿态估计，在后续运动时补偿精度差的运动控制误差。
(3)“劫持”:指机器人在知道自己姿态的情况下，获得了一个错误的姿态信息，或者被外界放置到了另一个姿态，里程表信息给出了错误的信息，甚至没有给出控制信息。
1.2粒子滤波步骤(可以组合两个例题) )。
(1)初始状态：用多个粒子模拟运动状态，使粒子在空间内均匀分布；
(预测阶段)基于状态转移方程，引入每个粒子，得到一个预测粒子；
(3)校正阶段：评价预测粒子(计算权重)，越接近真实状态粒子权重越大；
(4)重采样：基于粒子权重筛选粒子。 在分选过程中，在大量保持权重大的粒子的同时，具有一部分权重小的粒子。
(5)过滤)将重采样粒子带入状态转移方程，得到新的预测粒子。 即，步骤)2)。
2二维定位示例(基于粒子滤波的定位算法) 2.1问题
在二维空间中，假设运动小车的初始位置、状态方程(运动预测方程)、传感器测量数据，用粒子滤波方法进行定位。
2.2预设参数
)1)粒子总数N=200；
)2)假设运动时间T=10秒，且每秒进行进一步动作；
(3)运动场大小worldsizeworldsize=100(100平方米
)4)控制小车运动的方程。 小车沿一条曲线运动(但实际运动情况一定与给定控制略有不同。 这应理解为叠加在理想运动方程上的控制误差，假设控制误差为5米
)；
(5)传感器测量的小车位置数据同样也和实际情况不一致，这也可以理解为叠加在真实位置上的测量误差，测量误差假设为5米；
(6)小车初始位置已知，假设处在100*100场地中的（50,20)位置；
2.3 步骤与理解分析
(1)初始化粒子群
在整个场地内，将总粒子数N进行均匀分布，得到如图所示结果。
(2)小车开始运动(同时每运动一步进行一次测量)
小车按照控制给定的叠加了控制噪声的运动方程进行运动，运动达到下一位置后，传感器对当前位置进行测量，并得到一个测量的位置(不准确,含有测量噪声)
(3)粒子群更新(预测步骤)
把粒子群中的全部粒子逐个带入小车的运动方程中，得到粒子群的下一步位置。同时，计算此时每个粒子的位置和测量得到的小车位置 这两个位置之间的几何距离，按照距离的不同给每个粒子添加一个权重，用于重采样。显然，距离越近，权重越大（权重和距离关系的函数，这里采用rydbl分布钟形曲线的右侧，即随距离增大，权重单调递减）。下一步，得到全部粒子的权重后，将它们进行归一化。
(4)重采样
对于全部M个粒子，它们的归一化权重集合为 \omega ，第 i 个粒子的权重为 \omega[i] 。则重采样过程可理解为旋转轮盘抽奖。如下图。
由于不同粒子的归一化权重不同，它们占轮盘的面积也不同，因此权重大的粒子更容易被抽中。
现在，我们需要重采样得到新的M个粒子组成的粒子群。因此，我们按照上述轮盘，抽M次，抽到某个权重 \omega[i] ，则把该权重对应的粒子放入新的M个粒子组成的粒子群中。这样，那些权重大的粒子可能被反复抽到，从而重复出现在新的粒子群中，而那些权重小的粒子可能会在新的粒子群中被丢弃。
某一次运动过程结束后，可画出小车实际位置、重采样后粒子群位置、以及粒子群的几何中心位置，如图所示。
(5)重复步骤(2)~(4)，直到结束。
最后，我们可以得到整个运动过程中，小车实际路径、测量得到的路径、粒子群中心位置构成的路径，如图所示。
另外，还可以得到测量位置与真实位置间的误差，以及粒子群中心位置与真实位置间的误差，如图所示。
简单分析可知，综合控制和测量，利用粒子滤波进行定位，较单纯相信测量而言，更加精确。
2.4 本例的MATLAB源代码（附有详细注释）
%粒子滤波(定位运动轨迹)%在二维空间,假设运动物体的一组(非线性)运动位置、速度、加速度数据,用粒子滤波方法进行处理clc,clear,close all%% 参数设置N = 200; %粒子总数Q = 5; %过程噪声(控制误差) 状态转移方程中使用R = 5; %测量噪声 由真实位置叠加测量噪声得到测量位置T = 10; %测量时间(总步数)theta = pi/T; %旋转角度distance = 80/T; %每次走的距离(步长)WorldSize = 100; %世界大小X = zeros(2, T); %存储系统状态(每列存储二维位置坐标(x,y),共T个位置)Z = zeros(2, T); %存储系统的观测状态(每列存储二维位置坐标(x,y),共T次测量)P = zeros(2, N); %建立粒子群(每列存储当前粒子的二维位置坐标,共N个粒子)PCenter = zeros(2, T); %所有粒子的中心位置w = zeros(N, 1); %每个粒子的权重err = zeros(2,T); %误差(第一行为粒子与真实路径误差 第二行为测量与真实路径误差)X(:, 1) = [50; 20]; %初始系统状态 即初始位置在坐标(50,20)Z(:, 1) = X(:,1) + wgn(2,1,10*log10(R)); %初始系统的观测状态(为真实位姿叠加rydbl噪声) %y = wgn(m,n,p) 产生一个m行n列的rydbl白噪声的矩阵，p以dBW为单位指定输出噪声的强度%% 初始化粒子群for i = 1 : N P(:, i) = [WorldSize*rand; WorldSize*rand];%随机产生第i个粒子的坐标(rand为产生[0,1]之间均匀分布) dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离 %求权重 (权重与距离的关系 为 均值是0,方差是sqrt(R)的rydbl分布曲线) 因为均值为0且距离大于0 因此权重随着距离增加沿rydbl曲线右侧递减 w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); endPCenter(:, 1) = sum(P, 2) / N;%t=1时刻(初始时刻)所有粒子的几何中心位置% 初始状态(t=1)画图err(1,1) = norm(X(:, 1) - PCenter(:, 1));%粒子群几何中心与系统真实状态的误差err(2,1) = wgn(1, 1, 10*log10(R));figure(1);hold onset(0,'defaultfigurecolor','w')plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %真实的初始状态位置(红点表示)%grid onaxis([0 100 0 100]);set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置(N个黑点)plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置(蓝点表示)legend('真实位置', '粒子群', '粒子群的几何中心');title('初始状态');hold off%% 开始运动for k = 2 : T %从t=2到T %模拟一个弧线运动的状态 X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q)); %状态方程 Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R)); %观测方程(状态上叠加测量的rydbl噪声) %粒子滤波 %预测 for i = 1 : N P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));%粒子群带入状态方程 dist = norm(P(:, i)-Z(:, k)); %粒子群中各粒子 与 测量位置 的距离 w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); %求权重(距离近权重大) end %归一化权重 wsum = sum(w); for i = 1 : N w(i) = w(i) / wsum; end %重采样（更新） for i = 1 : N wmax = 2 * max(w) * rand; %另一种重采样规则 index = gldhl(N, 1);%生成一个在[1(默认值),N]之间均匀分布的伪随机整数 while(wmax > w(index)) wmax = wmax - w(index); index = index + 1; if index > N index = 1; end end Pnext(:, i) = P(:, index); %得到新粒子放入临时集Pnext end P=Pnext;%用临时集Pnext更新粒子集P PCenter(:, k) = sum(P, 2) / N; %重采样后所有粒子的中心位置 %计算误差 err(1,k) = norm(X(:, k) - PCenter(:, k)); %粒子几何中心与系统真实状态的误差 err(2,k) = norm(X(:, k) - Z(:, k)); %画图 figure(2); set(0,'defaultfigurecolor','w') clf;%清空figure(2)中的图像 以便循环重新画 hold on plot(X(1, k), X(2, k), 'r.', 'markersize',30); %系统状态位置 plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置 plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置 axis([0 100 0 100]); title('运动过程'); legend('真实状态', '粒子群', '粒子群的几何中心'); hold off pause(0.1);%停0.1s开始下次迭代end%% 绘制轨迹figure(3);set(0,'defaultfigurecolor','w')plot(X(1,:), X(2,:), 'r.-', Z(1,:), Z(2,:), 'g.-', PCenter(1,:), PCenter(2,:), 'b.-');axis([0 100 0 100]);set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10legend('真实轨迹', '测量轨迹', '粒子群几何中心轨迹');xlabel('横坐标 x'); ylabel('纵坐标 y');%% 绘制误差figure(4);set(0,'defaultfigurecolor','w')%set(gca,'FontSize',12);%设置图标字体大小plot(err(1,:),'b.-');%err1为各时刻 真实位置与粒子群中心的几何距离hold onplot(err(2,:),'r.-');%err2为各时刻 真实位置与测量位置的几何距离hold offxlabel('步数 t');legend('粒子群误差', '测量误差');title('真实位置与粒子群中心的集合距离'); 3 更多
3.1 失效恢复问题
蒙特卡罗定位以目前的形式能够解决全局定位问题，但是不能从机器人绑架中或全局定位失效中恢复。定位过程中，获取位置的同时，不在最可能位置处的粒子会逐渐消失，在某个时候，只有单一位置的粒子能够“幸存”，如果这个位姿碰巧是错误的，算法不能恢复。实际上，任何随机算法（如蒙特卡罗定位算法），在重采样步骤中可能意外地丢弃所有正确位置附近的粒子，特别是当粒子数较少，且粒子扩散到较大空间时，这个问题就显得很重要了。
解决办法：
通过简单的探索算法可以解决这个问题，探索算法的思想是：增加随机粒子到粒子集合。通过假设机器人可能以小概率遭到绑架，注入一些随机粒子，从而在运动模型上产生一些随机状态，即使机器人不被绑架，随机粒子也能提升额外的鲁棒性级别。
这引起两个问题：
(1)在每次算法迭代中，应该增加多少粒子；(2)从哪种分布产生这些粒子。
解答：
(1)一种简单的方法是每次迭代增加固定数目的随机粒子；另一种更好的想法是基于某些定位性能的评估增加粒子。其中，实现第二种想法的一个方法是监控传感器测量的概率分布，在粒子滤波里，重要性权重是这个概率分布的随机估计，其均值(即增加的粒子数目)为：
(2)一种简单的方法是根据均匀分布在位置空间产生粒子，并用当前观测值加权这些粒子；另一种更好的想法是根据测量分布直接产生粒子，根据观测似然分布，附加的粒子能够直接放置在相应的位置上。