粒子滤波算法_粒子滤波目标跟踪算法 - CSDN
精华内容
参与话题
  • 一直都觉得粒子滤波是个挺牛的东西,每次试图看文献都被复杂的数学符号搞得看不下去。一个偶然的机会发现了Rob Hess(http://web.engr.oregonstate.edu/~hess/)实现的这个粒子滤波。从代码入手,一下子就明白了粒子...

    转自:http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html

    一直都觉得粒子滤波是个挺牛的东西,每次试图看文献都被复杂的数学符号搞得看不下去。一个偶然的机会发现了Rob Hess(http://web.engr.oregonstate.edu/~hess/)实现的这个粒子滤波。从代码入手,一下子就明白了粒子滤波的原理。根据维基百科上对粒子滤波的介绍(http://en.wikipedia.org/wiki/Particle_filter),粒子滤波其实有很多变种,Rob Hess实现的这种应该是最基本的一种,Sampling Importance Resampling (SIR),根据重要性重采样。下面是我对粒子滤波实现物体跟踪的算法原理的粗浅理解:

    1)初始化阶段-提取跟踪目标特征

    该阶段要人工指定跟踪目标,程序计算跟踪目标的特征,比如可以采用目标的颜色特征。具体到Rob Hess的代码,开始时需要人工用鼠标拖动出一个跟踪区域,然后程序自动计算该区域色调(Hue)空间的直方图,即为目标的特征。直方图可以用一个向量来表示,所以目标特征就是一个N*1的向量V。

    2)搜索阶段-放狗

    好,我们已经掌握了目标的特征,下面放出很多条狗,去搜索目标对象,这里的狗就是粒子particle。狗有很多种放法。比如,a)均匀的放:即在整个图像平面均匀的撒粒子(uniform distribution);b)在上一帧得到的目标附近按照高斯分布来放,可以理解成,靠近目标的地方多放,远离目标的地方少放。Rob Hess的代码用的是后一种方法。狗放出去后,每条狗怎么搜索目标呢?就是按照初始化阶段得到的目标特征(色调直方图,向量V)。每条狗计算它所处的位置处图像的颜色特征,得到一个色调直方图,向量Vi,计算该直方图与目标直方图的相似性。相似性有多种度量,最简单的一种是计算sum(abs(Vi-V)).每条狗算出相似度后再做一次归一化,使得所有的狗得到的相似度加起来等于1.

    3)决策阶段

    我们放出去的一条条聪明的狗向我们发回报告,“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...那么目标究竟最可能在哪里呢?我们做次加权平均吧。设N号狗的图像像素坐标是(Xn,Yn),它报告的相似度是Wn,于是目标最可能的像素坐标X = sum(Xn*Wn),Y = sum(Yn*Wn).

    4)重采样阶段Resampling

    既然我们是在做目标跟踪,一般说来,目标是跑来跑去乱动的。在新的一帧图像里,目标可能在哪里呢?还是让我们放狗搜索吧。但现在应该怎样放狗呢?让我们重温下狗狗们的报告吧。“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...综合所有狗的报告,一号狗处的相似度最高,三号狗处的相似度最低,于是我们要重新分布警力,正所谓好钢用在刀刃上,我们在相似度最高的狗那里放更多条狗,在相似度最低的狗那里少放狗,甚至把原来那条狗也撤回来。这就是Sampling Importance Resampling,根据重要性重采样(更具重要性重新放狗)。

    (2)->(3)->(4)->(2)如是反复循环,即完成了目标的动态跟踪。

    根据我的粗浅理解,粒子滤波的核心思想是随机采样+重要性重采样。既然我不知道目标在哪里,那我就随机的撒粒子吧。撒完粒子后,根据特征相似度计算每个粒子的重要性,然后在重要的地方多撒粒子,不重要的地方少撒粒子。所以说粒子滤波较之蒙特卡洛滤波,计算量较小。这个思想和RANSAC算法真是不谋而合。RANSAC的思想也是(比如用在最简单的直线拟合上),既然我不知道直线方程是什么,那我就随机的取两个点先算个直线出来,然后再看有多少点符合我的这条直线。哪条直线能获得最多的点的支持,哪条直线就是目标直线。想法非常简单,但效果很好。

    展开全文
  • 粒子滤波原理及其matlab仿真

    千次阅读 多人点赞 2019-01-27 00:59:44
    粒子滤波算法不受线性高斯模型的约束,与卡尔曼滤波器一样,粒子滤波算法同样需要知道系统的模型,如果不知道系统的模型,也要想办法构建一个模型来逼近真实的模型。这个真实模型就是各应用领域内系统的数学表示,...

    Github个人博客:https://joeyos.github.io

    粒子滤波原理及其matlab仿真

    系统建模

    粒子滤波算法不受线性高斯模型的约束,与卡尔曼滤波器一样,粒子滤波算法同样需要知道系统的模型,如果不知道系统的模型,也要想办法构建一个模型来逼近真实的模型。这个真实模型就是各应用领域内系统的数学表示,主要包括状态方程和量测方程。

    状态方程和过程噪声

    X(k) = f (X(k-1), W(k));

    观测方程和测量噪声

    Z(K) = h (X(k), V(k));

    核心思想

    粒子滤波是一种基于蒙特卡洛仿真的近似贝叶斯滤波算法。其核心思想是用一些离散随机采样点近似系统随机变量的概率密度函数,以样本均值代替积分运算,从而获得状态的最小方差估计。

    均值思想

    粒子滤波的均值思想就是利用粒子集合的均值来作为滤波器的估计值。如果粒子集合的分布不能很好的覆盖真实值,那么滤波器经过几次迭代必然会出现滤波发散。

    权重计算

    权重计算时粒子滤波算法的核心,它的重要意义在于,根据权重大小能实现优质粒子的大量复制,对劣质粒子实现淘汰制。另外,经过权重计算,它也是重新指导粒子空间分布的依据。权重最终影响滤波结果。

    优胜劣汰

    粒子的“优胜劣汰”主要体现在对粒子的复制上,这种机制从某种意义上说保证了粒子滤波的最终目标得以实现。实现“优胜劣汰”的重要手段是重采样算法。重采样的思想是通过对样本重新采样,大量繁殖权重高的粒子,淘汰权值低的粒子,从而抑制退化。

    随机重采样(randomR.m)

    多项式重采样(MultinomialR.m)

    系统重采样(systematicR.m)

    残差重采样(residualR.m)

    粒子滤波器(Particle Filter)

    蒙特卡洛采样

    蒙特卡洛方法是从后验概率分别采集带权重的粒子集(样本集),用粒子集表示后验分布,将积分转化为求和形式。

    贝叶斯重要性采样

    对蒙特卡洛采样方法,后验概率分布可以用有限的离散样本集来近似。通常后验概率分布函数是无法直接得到的,贝叶斯采样原理的基本思想是,先从一个已知的且容易采样的参考分布中抽样,通过对参考分布的采样获得的粒子集进行加权求和来近似后验分布。

    序列重要性采样(SIS滤波器)

    贝叶斯重要性采样是一种常用的简单的蒙特卡洛方法,但是没有考虑到递推估计的特点。贝叶斯估计也是一个序列估计问题,因此在采样上也必须有序列关系。序列重要性采样,不改变过去1的序列样本集,而采用递归的形式计算重要性权值。

    Bootstrp/采样-重要性再采样(SIR滤波器)

    SIR滤波器和SIS滤波器都属于基本粒子滤波器,都使用重要性采样算法,但是两者又有区别。对于SIR滤波器,重采样总是会被执行,在算法中通常两次重要性采样之间需要一次重采样,而SIS滤波器只是在需要是才进行重采样,因此SIS的计算量比SIR的计算量要小。

    粒子滤波算法通用流程

    • 初始化
    • 循环开始
    • 重要性采样->计算权重->归一化权重
    • 重采样
    • 输出
    • 循环结束

    粒子滤波仿真实例

    一维系统建模

    状态模型:x(k) = f (x(k-1), k) + w(k-1)

    观测方程:z(k) = x(k)^2 / 20 +v(k)

    f(x((k-1), k) = 0.5x(k-1) + 2.5x(k-1) / (1+x(k-1)^2) + 8cos(1.2k)

    w(k),v(k)为均值为0、方差分别为Q(k)=10,R(k)=1的高斯噪声。状态x(k)与x(k-1)为非线性关系,观测方程中z(k)和x(k)也是非线性关系。

    一维系统仿真

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% 粒子滤波一维系统仿真
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function Particle_For_UnlineOneDiv
    clear all;close all;clc;
    randn('seed',1); %为了保证每次运行结果一致,给定随机数的种子点
    %初始化相关参数
    T=50;%采样点数
    dt=1;%采样周期
    Q=10;%过程噪声方差
    R=1;%测量噪声方差
    v=sqrt(R)*randn(T,1);%测量噪声
    w=sqrt(Q)*randn(T,1);%过程噪声
    numSamples=100;%粒子数
    ResampleStrategy=4;%=1为随机采样,=2为系统采样
    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    x0=0.1;%初始状态
    %产生真实状态和观测值
    X=zeros(T,1);%真实状态
    Z=zeros(T,1);%量测
    X(1,1)=x0;%真实状态初始化
    Z(1,1)=(X(1,1)^2)./20+v(1,1);%观测值初始化
    for k=2:T
    	%状态方程
    	X(k,1)=0.5*X(k-1,1)+2.5*X(k-1,1)/(1+X(k-1,1)^2)+8*cos(1.2*k)+w(k-1,1);
    	%观测方程
    	Z(k,1)=(X(k,1).^2)./20+v(k,1);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    %粒子滤波器初始化,需要设置用于存放滤波估计状态,粒子集合,权重等数组
    Xpf=zeros(numSamples,T);%粒子滤波估计状态
    Xparticles=zeros(numSamples,T);%粒子集合
    Zpre_pf=zeros(numSamples,T);%粒子滤波观测预测值
    weight=zeros(numSamples,T);%权重初始化
    %给定状态和观测预测的初始采样:
    Xpf(:,1)=x0+sqrt(Q)*randn(numSamples,1);
    Zpre_pf(:,1)=Xpf(:,1).^2/20;
    %更新与预测过程
    for k=2:T
    	%第一步:粒子集合采样过程
    	for i=1:numSamples
    		QQ=Q;%跟卡尔曼滤波不同,这里的Q不要求与过程噪声方差一致
    		net=sqrt(QQ)*randn;%这里的QQ可以看成是网的半径,数值可调
    		Xparticles(i,k)=0.5.*Xpf(i,k-1)+2.5.*Xpf(i,k-1)./(1+Xpf(i,k-1).^2)+8*cos(1.2*k)+net;
    	end
    	%第二步:对粒子集合中的每个粒子,计算其重要性权值
    	for i=1:numSamples
    		Zpre_pf(i,k)=Xparticles(i,k)^2/20;
    		weight(i,k)=exp(-.5*R^(-1)*(Z(k,1)-Zpre_pf(i,k))^2);%省略了常数项
    	end
    	weight(:,k)=weight(:,k)./sum(weight(:,k));%归一化权值
    	%第三步:根据权值大小对粒子集合重采样,权值集合和粒子集合是一一对应的
    	%选择采样策略
    	if ResampleStrategy==1
    		outIndex = randomR(weight(:,k));
    	elseif ResampleStrategy==2
    		outIndex = systematicR(weight(:,k)');
    	elseif ResampleStrategy==3
    		outIndex = multinomialR(weight(:,k));
    	elseif ResampleStrategy==4
    		outIndex = residualR(weight(:,k)');
    	end
    	%第四步:根据重采样得到的索引,去挑选对应的粒子,重构的集合便是滤波后的状态集合
    	%对这个状态集合求均值,就是最终的目标状态、
    	Xpf(:,k)=Xparticles(outIndex,k);
    end
    %计算后验均值估计、最大后验估计及估计方差
    Xmean_pf=mean(Xpf);%后验均值估计,及上面的第四步,也即粒子滤波估计的最终状态
    bins=20;
    Xmap_pf=zeros(T,1);
    for k=1:T
    	[p,pos]=hist(Xpf(:,k,1),bins);
    	map=find(p==max(p));
    	Xmap_pf(k,1)=pos(map(1));%最大后验估计
    end
    for k=1:T
    	Xstd_pf(1,k)=std(Xpf(:,k)-X(k,1));%后验误差标准差估计
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %画图
    figure();clf;%过程噪声和测量噪声图
    subplot(221);
    plot(v);%测量噪声
    xlabel('时间');ylabel('测量噪声');
    subplot(222);
    plot(w);%过程噪声
    xlabel('时间');ylabel('过程噪声');
    subplot(223);
    plot(X);%真实状态
    xlabel('时间');ylabel('状态X');
    subplot(224);
    plot(Z);%观测值
    xlabel('时间');ylabel('观测Z');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    figure();
    k=1:dt:T;
    plot(k,X,'b',k,Xmean_pf,'r',k,Xmap_pf,'g');%注:Xmean_pf就是粒子滤波结果
    legend('系统真实状态值','后验均值估计','最大后验概率估计');
    xlabel('时间');ylabel('状态估计');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    figure();
    subplot(121);
    plot(Xmean_pf,X,'+');%粒子滤波估计值与真实状态值如成1:1关系,则会对称分布
    xlabel('后验均值估计');ylabel('真值');
    hold on;
    c=-25:1:25;
    plot(c,c,'r');%画红色的对称线y=x
    hold off;
    subplot(122);%最大后验估计值与真实状态值如成1:1关系,则会对称分布
    plot(Xmap_pf,X,'+');
    xlabel('Map估计');ylabel('真值');
    hold on;
    c=-25:25;
    plot(c,c,'r');%画红色的对称线y=x
    hold off;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %画直方图,此图形是为了看粒子集的后验密度
    domain=zeros(numSamples,1);
    range=zeros(numSamples,1);
    bins=10;
    support=[-20:1:20];
    figure();
    hold on;%直方图
    xlabel('时间');ylabel('样本空间');
    vect=[0 1];
    caxis(vect);
    for k=1:T
    	%直方图反映滤波后的粒子集合的分布情况
    	[range,domain]=hist(Xpf(:,k),support);
    	%调用waterfall函数,将直方图分布的数据画出来
    	waterfall(domain,k,range);
    end
    axis([-20 20 0 T 0 100]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    figure();
    xlabel('样本空间');ylabel('后验密度');
    k=30;%k=?表示要查看第几个时刻的粒子分布与真实状态值的重叠关系
    [range,domain]=hist(Xpf(:,k),support);
    plot(domain,range);
    %真实状态在样本空间中的位置,画一条红色直线表示
    XXX=[X(k,1),X(k,1)];
    YYY=[0,max(range)+10];
    line(XXX,YYY,'Color','r');
    axis([min(domain) max(domain) 0 max(range)+10]);
    figure();
    k=1:dt:T;
    plot(k,Xstd_pf,'-');
    xlabel('时间');ylabel('状态估计误差标准差');
    axis([0,T,0,10]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %函数功能:实现随机重采样算法
    %输入参数:weight为原始数据对应的权重大小
    %输出参数:outIndex是根据weight对inIndex筛选和复制结果
    function outIndex=randomR(weight)
    %获得数据的长度
    L=length(weight);
    %初始化输出索引向量,长度与输入索引向量相等
    outIndex=zeros(1,L);
    %第一步:产生[0,1]上均匀分布的随机数组,并升序排序
    u=unifrnd(0,1,1,L);
    u=sorf(u);
    %u=(1:L)/L%这个是完全均匀
    %第二步:计算粒子权重积累函数cdf
    cdf=cumsum(weight);
    %第三步:核心计算
    i=1;
    for j=1:L
    	%此处的基本原理是:u是均匀的,必然是权值大的地方
    	%有更多的随机数落入该区间,因此会被多次复制
    	while(i<=L)&(u(i)<=cdf(j))
    		%复制权值大的粒子
    		outIndex(i)=j;
    		%继续考察下一个随机数,看它落在哪个区间
    		i=i+1;
    	end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 多项式重采样子函数
    % 输入参数:weight为原始数据对应的权重大小
    % 输出参数:outIndex是根据weight筛选和复制结果
    function outIndex = multinomialR(weight);
    %获取数据长度
    Col=length(weight)
    N_babies= zeros(1,Col);
    
    %计算粒子权重累计函数cdf 
    cdf= cumsum(weight);
     %产生[0,1]均匀分布的随机数
    u=rand(1,Col)
    
    %求u^(j^-1)次方 
    uu=u.^(1./(Col:-1:1))
     %如果A是一个向量,cumprod(A)将返回一个包含A各元素积累连乘的结果的向量
     %元素个数与原向量相同
    ArrayTemp=cumprod(uu)
     %fliplr(X)使矩阵X沿垂直轴左右翻转
    u = fliplr(ArrayTemp);
    j=1;
    for i=1:Col
        %此处跟随机采样相似
        while (u(i)>cdf(j))
            j=j+1;
        end
        N_babies(j)=N_babies(j)+1;
    end;
    index=1;
    for i=1:Col
        if (N_babies(i)>0)
            for j=index:index+N_babies(i)-1
                outIndex(j) = i;
            end;
        end;
        index= index+N_babies(i);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 函数功能说明:残差重采样函数
    % 输入参数:一组权重weight向量
    % 输出参数:为该权重重采样后的索引outIndex
    function outIndex = residualR(weight)
    N= length(weight);
    N_babies= zeros(1,N);
    q_res = N.*weight;
    N_babies = fix(q_res);
    N_res=N-sum(N_babies);
    if (N_res~=0)
        q_res=(q_res-N_babies)/N_res;
        cumDist= cumsum(q_res);
        u = fliplr(cumprod(rand(1,N_res).^(1./(N_res:-1:1))));
        j=1;
        for i=1:N_res
            while (u(1,i)>cumDist(1,j))
                j=j+1;
            end
            N_babies(1,j)=N_babies(1,j)+1;
        end;
    end;
    index=1;
    for i=1:N
        if (N_babies(1,i)>0)
            for j=index:index+N_babies(1,i)-1
                outIndex(j) = i;
            end;
        end;
        index= index+N_babies(1,i);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 系统重采样子函数
    % 输入参数:weight为原始数据对应的权重大小
    % 输出参数:outIndex是根据weight筛选和复制结果
    function outIndex = systematicR(weight);
    N=length(weight);
    N_children=zeros(1,N);
    label=zeros(1,N);
    label=1:1:N;
    s=1/N;
    auxw=0;
    auxl=0;
    li=0;
    T=s*rand(1);
    j=1;
    Q=0;
    i=0;
    u=rand(1,N);
    while (T<1)
        if (Q>T)
            T=T+s;
            N_children(1,li)=N_children(1,li)+1;
        else
            i=fix((N-j+1)*u(1,j))+j;
            auxw=weight(1,i);
            li=label(1,i);
            Q=Q+auxw;
            weight(1,i)=weight(1,j);
            label(1,i)=label(1,j);
            j=j+1;
        end
    end
    index=1;
    for i=1:N
        if (N_children(1,i)>0)
            for j=index:index+N_children(1,i)-1
                outIndex(j) = i;
            end;
        end;
        index= index+N_children(1,i);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    

    数据分析

    数据分析及说明

    仿真过程采用自举滤波(SIR算法),每一步迭代都进行重新抽样,根据ResampleStrategy参数设置1-4之间的整数,分别可以选用随机重采样、系统重采样、残差重采样及多项式重采样策略。

    (1)状态曲线图

    将状态绘制成曲线,能够从宏观上掌握滤波效果,这时往往将滤波器估计的状态和系统真实状态同时展现在图中。得到Q=10,R=1的非线性条件下基于粒子滤波的仿真曲线:
    这里写图片描述
    (2)直线散点图:估计与真值的关系
    这里写图片描述
    (3)直方图
    这里写图片描述
    (4)瀑布图
    这里写图片描述
    (5)噪声影响
    这里写图片描述
    这里写图片描述

    此博客均属原创或译文,欢迎转载但请注明出处
    Github个人博客:https://joeyos.github.io

    展开全文
  • 粒子滤波算法学习

    2019-12-15 13:12:00
    今天我们来讲一下粒子滤波算法,这也是我在学习过程中的一个笔记,由于有很多的个人理解,有不对的地方欢迎大家批评指正。 另:个人博客 Glooow 开业啦!欢迎各位大驾光临 文章目录贝叶斯滤波1. 预测2.更新...

    今天我们来讲一下粒子滤波算法,这也是我在学习过程中的一个笔记,由于有很多的个人理解,有不对的地方欢迎大家批评指正。

    另:个人博客 Glooow 开业啦!欢迎各位大驾光临

    贝叶斯滤波

    贝叶斯滤波是我们理解粒子滤波的基础。假设我们有如下图的隐马尔科夫模型(Hidden Markov Model),并且有如下的系统方程与观测方程,我们只有观测值 y1:T\boldsymbol{y}_{1:T},但是现在想要根据观测值估计隐变量 x1:T\boldsymbol{x}_{1:T},也就是滤波所要做的事情。
    System Model:xk=fk(xk1,vk1)Observation Model:yk=hk(xk,nk) \begin{aligned} \text{System Model}&: x_{k}=f_{k}\left(x_{k-1}, v_{k-1}\right) \\ \text{Observation Model}&: \mathrm{y}_{k}=h_{k}\left(\mathrm{x}_{k}, \mathrm{n}_{k}\right) \end{aligned}
    hmm

    假设我们现在已经处于 tk1t_{k-1},且已经获得了 p(xk1y1:k1)p({x}_{k-1}|\boldsymbol{y}_{1:k-1}),这个概率分布的含义是什么呢?我们现在已经有了前 k1k-1 个时刻的观测值 y1:k1\boldsymbol{y}_{1:k-1},并且根据这些观测值估计了 k1k-1 时刻隐变量 xk1x_{k-1}后验概率分布,也即 p(xk1y1:k1)p({x}_{k-1}|\boldsymbol{y}_{1:k-1})。那么接下来到 kk 时刻,我们又获得了一个观测 yky_k,要怎么估计新的后验概率分布 p(xky1:k)p({x}_{k}|\boldsymbol{y}_{1:k}) 呢?我们通过预测更新两个阶段来估计,下面我就解释一下这两个阶段的含义。

    1. 预测

    前面说了我们有系统模型和观测模型,首先根据系统模型,我们有了 k1k-1 时刻的后验,就可以根据 xk1x_{k-1}xkx_k 的转移模型得到 xkx_k 对应的概率分布,这个过程就叫做预测(Update)。通过预测阶段,我们可以获得先验概率分布(注意这里我们将 p(xky1:k1)p({x}_k|\boldsymbol{y}_{1:k-1}) 称为先验概率分布)
    p(xky1:k1)=p(xkxk1)p(xk1y1:k1)dxk1 \begin{aligned}p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1}\end{aligned}
    也就是说,我们即使没有观测值,也能对 xkx_k 的分布进行估计,但是由于没有利用观测值 yky_k 带来的信息,因此这个估计是不准确的,下面我们就需要用观测值对这个先验概率分布进行纠正,也就是 更新阶段。

    2.更新

    有了先验,又有了观测,根据贝叶斯公式,我们可以很容易得到后验概率分布 p(xky1:k)p\left(x_{k} | \boldsymbol{y}_{1: k}\right)。怎么理解下面一个式子呢?我们有似然函数 p(ykxk)p\left(y_{k} | x_{k}\right),现在有了观测 yky_k,那么似然值越大,表明对应的 xkx_k 的概率应该也越大, 就是用似然函数先验概率进行加权就得到了后验概率
    p(xky1:k)=p(ykxk,y1:k1)p(xky1:k1)p(yky1:k1)p(ykxk,y1:k1)p(xky1:k1)=p(ykxk)p(xky1:k1) \begin{aligned} p\left(x_{k} | \boldsymbol{y}_{1: k}\right)&=\frac{p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)}{p\left(y_{k} | \boldsymbol{y}_{1: k-1}\right)} \\ &\propto p\left(y_{k} | x_{k},\boldsymbol{y}_{1: k-1}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \\ &= p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \end{aligned}

    怎么理解这个加权呢?举个栗子

    比如

    总结

    总结起来,我们滤波分为两个阶段
    Prediction:p(xky1:k1)=p(xkxk1)p(xk1y1:k1)dxk1Update:p(xky1:k)p(ykxk)p(xky1:k1) \begin{aligned} \text{Prediction}&: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\ \text{Update}&: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right) \end{aligned}
    可以看到,上面的预测阶段含有积分,这在实际当中往往是很难计算的,而对于那些非常规的PDF,甚至不能给出解析解。解决这种问题一般有两种思路:

    • 建立简单的模型,获得解析解,如卡尔曼滤波(Kalman Filter)及EKF、UKF等;
    • 建立复杂的模型,获得近似解,如粒子滤波(Particle Filter)等。

    卡尔曼滤波

    卡尔曼滤波的思路就是将系统建模线性模型,隐变量、控制变量、控制噪声与观测噪声均为高斯分布,那么观测变量也是随机变量,整个模型中所有的随机变量都是高斯的!高斯分布是我们最喜欢的分布,因为在前面贝叶斯滤波的预测阶段我们可以不用积分了,只需要计算均值和协方差就可以了!根据系统模型和观测模型,我们可以获得滤波的闭式解,这就是卡尔曼滤波的思想。
    System Model:xk=Axk1+Buk1+vk1Observation Model:yk=Cxk+nk \begin{aligned} \text{System Model}&: \mathrm{x}_{k}=A\mathrm{x}_{k-1} + B\mathrm{u}_{k-1} + v_{k-1} \\ \text{Observation Model}&: \mathrm{y}_{k}=C\mathrm{x}_{k}+ \mathrm{n}_{k} \end{aligned}
    但实际中要求线性系统模型往往是很困难的,对于非线性模型,如果我们还想应用卡尔曼滤波该怎么办呢?线性近似,也就是求一个雅可比矩阵(Jacobi),这就是扩展卡尔曼滤波(EKF)。

    大数定律

    粒子滤波是什么是意思呢?我们可以用大量的采样值来描述一个概率分布,当我们按照一个概率分布进行采样的时候,某个点的概率密度越高,这个点被采到的概率越大,当采样数目足够大(趋于无穷)的时候,粒子出现的频率就可以用来表示对应的分布,实际中我们可以理解一个粒子就是一个采样。

    particle

    但是对于离散型的随机变量还好,可以很容易的求出来状态空间中每个状态的频率。但如果对于连续分布,其实是很不方便的,所幸实际中我们也不需要获得概率分布的全部信息,一般只需要求出期望就可以了。

    下面为了公式书写和推导方便,以离散型随机变量为例,假设状态空间为 Z={1,2,...,K}\mathcal{Z}=\{1,2,...,K\},有 NN 个采样样本 ziZz_i \in \mathcal{Z},服从概率分布 q(z)q(\mathbf{z})。我们将根据 NN 个采样样本 z1:N\boldsymbol{z}_{1:N} 得到的分布记为经验分布 p^(bz1:N)=1NiI(bzi)\hat{p}(b|\boldsymbol{z}_{1:N}) = \frac{1}{N}\sum_i \mathbb{I}(b-z_i),其中 I()\mathbb{I}(\cdot) 为指示函数,那么当 NN 足够大的时候,经验分布 p^(bz1:N)\hat{p}(b|\boldsymbol{z}_{1:N}) 就足够接近真实分布 q(z)q(\mathbf{z})。现在我们想估计随机变量 t=g(z)\mathsf{t}=g(\mathsf{z}) 的期望,即
    E[t]=E[g(z)]=bZg(b)q(b)bg(b)p^(bz1:N)=bg(b)1NiI(bzi)=1Nig(zi) \begin{aligned} \mathbb{E}[\mathsf{t}] &= \mathbb{E}[g(\mathsf{z})]=\sum_{b\in\mathcal{Z}}g(b)q(b) \\ &\approx \sum_b g(b)\hat{p}(b|\boldsymbol{z}_{1:N}) \\ &= \sum_b g(b)\frac{1}{N}\sum_i \mathbb{I}(b-z_i)\\ &= \frac{1}{N}\sum_i g(z_i) \end{aligned}
    也就是说,我们只需要对样本进行简单求和就可以了!连续型随机变量也是类似的,所以在后面的粒子滤波中,我们利用粒子估计了概率密度分布,要想求期望就只需要进行求和平均就可以了。

    粒子滤波简单实例

    好了,有了前面的预备知识,我们可以先看一个粒子滤波的简单例子。

    假设我们现在有采样好的 NN 个粒子 {xk1i}i=1N\{x_{k-1}^i\}_{i=1}^N,他们服从分布 p(xk1y1:k1)p({x}_{k-1}|\boldsymbol{y}_{1:k-1}),那么我们现在如何进行贝叶斯滤波中的预测更新阶段呢?

    1. 预测

    回顾一下预测的公式
    Prediction:p(xky1:k1)=p(xkxk1)p(xk1y1:k1)dxk1 \text{Prediction}: p({x}_k|\boldsymbol{y}_{1:k-1})=\int p({x}_k|x_{k-1})p(x_{k-1}|\boldsymbol{y}_{1:k-1})dx_{k-1} \\
    想一下:只要我们让每个粒子 xk1ix_{k-1}^i进化”一步,也就是说按照分布 p(xkxk1)p(x_k|x_{k-1}) 进行采样,使得 xkip(xkxk1i)x_k^i \sim p(x_k|x_{k-1}^i),这样我们就获得了 NN 个新的粒子 {xki}i=1N\{x_k^i\}_{i=1}^N。很容易验证,如果 {xk1i}i=1N\{x_{k-1}^i\}_{i=1}^N 能够很好的表示 p(xk1y1:k1)p({x}_{k-1}|\boldsymbol{y}_{1:k-1}) 的话,那么 {xki}i=1N\{x_k^i\}_{i=1}^N 也能很好的表示 p(xky1:k1)p({x}_k|\boldsymbol{y}_{1:k-1})(这一部分可以凭直观感觉来理解,这是一个很自然的事情,也可以用前面的经验分布的思路进行公式推导)。

    这样做的好处是什么呢?我们避免了积分!只需要对一个已知的分布 p(xkxk1i)p(x_k|x_{k-1}^i) 进行采样,而这个分布是我们假设的系统模型,可以是很简单的,因此采样也很方便。

    2. 更新

    通过预测我们获得了 xkip(xkxk1i)x_k^i \sim p(x_k|x_{k-1}^i),这 NN 个粒子就描述了先验概率分布。接下来就是更新,再回顾一下更新的公式
    Update:p(xky1:k)p(ykxk)p(xky1:k1) \text{Update}: p\left(x_{k} | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}\right) p\left(x_{k} | \boldsymbol{y}_{1: k-1}\right)
    更新是什么呢?前面提到了:更新就是用似然函数先验概率进行加权就得到了后验概率。如果简单的把 xkix_k^i 带入到上面的公式里,就可以得到下面的式子
    p(xkiy1:k)p(ykxki)p(xkiy1:k1) p\left(x_{k}^i | \boldsymbol{y}_{1: k}\right)\propto p\left(y_{k} | x_{k}^i\right) p\left(x_{k}^i | \boldsymbol{y}_{1: k-1}\right)
    实际上就表示每个粒子有不同的权重

    这里怎么理解呢?想一下前面提到的经验分布,我们只需要用粒子出现的频率来表示对应的概率,实际上就是在统计频率的时候每个粒子的权重都是相同的,出现一次计数就加一。

    而这里的区别是什么呢?由于我们有一个观测值 yky_k,根据 yky_k 来反推,某一个粒子 i1i_1 的导致 yky_k 出现的概率更大,那么我们在统计频率的时候,就给这个粒子加一个更大的权重,以表示我们更相信/重视这个粒子。

    那么问题来了,前面我们说了滤波过程中要想求期望,只需要简单求和就可以了
    E[t]=1Nig(zi) \mathbb{E}[\mathsf{t}] = \frac{1}{N}\sum_i g(z_i)
    现在粒子有了权重怎么办呢,同理,加个权就好了(推导也很简单,相信大家都会),下面的式子里对权重进行了归一化
    E[t]=iwijwjg(zi) \mathbb{E}[\mathsf{t}] = \sum_i \frac{w_i}{\sum_j w_j}g(z_i)

    3. 递推

    前面只讲了一次预测和一次更新的过程,注意到我们前面只是从 k1k-1kk 时刻的滤波过程,但每一轮循环原理都是相同的。

    不过细心的朋友们可能会觉得不太对劲,前面一个阶段的例子里,预测之前我们有采样 {xk1i}i=1N\{x_{k-1}^i\}_{i=1}^N,推导过程中我们是默认这些粒子的权重都是相同的,然后我们进行了预测和更新,但是更新之后我们给每个粒子加权了呀,到下一个阶段怎么办?!!!也很简单,预测阶段不必要求每个粒子的权重都相同,也加上一个权重就好了。也就是说我们时时刻刻都保存有每个粒子的权重信息。

    这样我们就可以得到一个完整的粒子滤波算法了!但是还有问题!

    重采样

    但是呢,有人发现了上面的算法不好啊!不是因为麻烦,而是滤波到最后会发现只有少数一部分粒子有很大的权重,而其他绝大部分粒子的权重几乎为 0,这就意味着对于那些“不重要”的粒子,他们在我们求期望的时候贡献并不大,但是我们却花费了大量的计算精力来维护这些粒子,这样不好不好!于是就有人提出了重采样

    重采样什么意思呢?你们粒子的权重不是不同吗,那我就采取手段给你们整相同了!简单理解,有两个粒子,粒子 aa 的权重是 8,粒子 bb 权重是 2,那我就把粒子 aa 复制 8 份,粒子 bb 复制 2 份,这样得到的 10 个粒子权重就都是 1 了。但是另一个问题是一开始我们只有 2 个粒子,这样弄完我们就有 10 个粒子了,如果一直这么做我们的粒子数会越来越多,算力跟不上。那就从这 10 个粒子里均匀分布地随机抽 2 个!那么粒子 aa 的概率就是 0.80.8,这就成了。

    resample

    至此,加上重采样我们就获得了一个完整的比较好用的粒子滤波算法

    sir

    粒子滤波原理推导

    但是还有一个问题,就是前面我们直接说有 NN 个粒子 xk1ip(xk1y1:k1)x_{k-1}^i \sim p({x}_{k-1}|\boldsymbol{y}_{1:k-1}),但是第 1 步(刚开始)的时候我们怎么获得这些粒子来描述 p(x1y1)p(x_1|y_1) 啊?如果是高斯分布还好,我们可以很方便的对高斯分布进行采样,但是如果是一个特别特别特别复杂的分布呢?我们甚至不能写出解析表达式,更没办法按照这个分布进行随机采样,那我们是不是就凉了?不!我们前面不是讲了粒子可以有权重嘛。

    假如我们现在有另一个分布 q(x1:ky1:k)q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k}),这个分布是我们自己指定的,可以很简单,而且我们可以按照这个分布来进行采样,那么我们就有 xk1iq(x1:ky1:k)x_{k-1}^i \sim q(\mathbf{x}_{1:k}|\mathbf{y}_{1:k}),那么我们怎么用这些粒子来表示我们想要得到的真正的分布 p(x0:ky1:k)p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k}) 呢?再加个权就行了,就像前面用似然函数进行加权。
    p(x0:ky1:k)i=1Nwkiδ(x0:kx0:ki)wkip(x0:kiy1:k)q(x0:kiy1:k) \begin{aligned} p(\mathbf{x}_{0:k}|\mathbf{y}_{1:k})&\approx \sum_{i=1}^{N}w_k^i\delta(\mathbf{x}_{0:k}-\mathbf{x}_{0:k}^i) \\ w_{k}^{i} &\propto \frac{p\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)}{q\left(\mathbf{x}_{0: k}^{i} | \mathbf{y}_{1: k}\right)} \end{aligned}
    再考虑前面提到的的预测更新两个递推进行的阶段,就有
    q(x0:ky1:k)=q(xkx0:k1,y1:k)q(x0:k1y1:k1)=q(xkxk1,yk)q(x0:k1y1:k1)wkiwk1ip(ykxki)p(xkixk1i)q(xkixk1i,yk) \begin{aligned}q\left(\mathbf{x}_{0: k} | \mathbf{y}_{1: k}\right)&=q\left(\mathbf{x}_{k} | \mathbf{x}_{0: k-1}, \mathbf{y}_{1: k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\ &= q\left(\mathbf{x}_{k} | \mathbf{x}_{k-1}, \mathbf{y}_{ k}\right) q\left(\mathbf{x}_{0: k-1} | \mathbf{y}_{1: k-1}\right) \\ w_{k}^{i} &\propto w_{k-1}^{i} \frac{p\left(\mathbf{y}_{k} | \mathbf{x}_{k}^{i}\right) p\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}\right)}{q\left(\mathbf{x}_{k}^{i} | \mathbf{x}_{k-1}^{i}, \mathbf{y}_{k}\right)} \end{aligned}
    然后我们就得到了一个更加 universal/generic 的粒子滤波算法(下面图片所示算法中没加重采样步骤)。

    sis

    总结

    这篇文章主要是自己学习粒子滤波之后的一些理解,主要参考了文章 A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking,但是这篇文章中是从 general 的情况开始讲,然后举了一些常用的特例,个人感觉不利于初学者理解,因此本文写作过程中的思路是从简单的特例开始再到更一般的情况。

    最后一部分 [粒子滤波原理推导](## 粒子滤波原理推导) 其实写的并没有很清楚,主要是因为自己太懒,到最后懒得一个个手敲公式了,如果想更清楚地了解其中的细节,可以阅读上面那篇论文。

    Reference

    1. M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” in IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174-188, Feb. 2002.
    展开全文
  • 基于粒子滤波的定位算法 ——原理、理解与仿真

    万次阅读 多人点赞 2019-06-01 15:10:47
    1 算法原理 1.1 机器人定位问题 关于机器人定位,有三大问题,它们分别是: (1)“全局定位”:指初始位置未知,机器人靠自身运动确定自己在地图中的位姿。 (2)“位姿跟踪”:指已知自身位姿或者已经通过“全局定位”...

    在这里插入图片描述

    1 算法原理

    1.1 机器人定位问题

    关于机器人定位,有三大问题,它们分别是:

    (1)“全局定位”:指初始位置未知,机器人靠自身运动确定自己在地图中的位姿。

    (2)“位姿跟踪”:指已知自身位姿或者已经通过“全局定位”得到了一个较好的位姿估计,在后续运动时补偿精度较差的运动控制误差;

    (3)“绑架劫持”:指机器人在已知自身位姿的情况下,得到了一个错误的位姿信息或者外界将其放到另外一个位姿,而里程计信息给出了错误的信息甚至没有给出控制信息。

    1.2 粒子滤波步骤(可结合2中例题)

    (1)初始状态:用大量粒子模拟运动状态,使粒子在空间内均匀分布;

    (2)预测阶段:根据状态转移方程,将每一个粒子带入,得到一个预测粒子;

    (3)校正阶段:对预测粒子进行评价(计算权重),越接近于真实状态的粒子,其权重越大;

    (4)重采样:根据粒子权重对粒子进行筛选,筛选过程中,既要大量保留权重大的粒子,又要有一小部分权重小的粒子;

    (5)滤波:将重采样后的粒子带入状态转移方程得到新的预测粒子,即步骤(2)。

    2 一个二维定位的例子 (基于粒子滤波的定位算法)

    2.1 问题

    在二维空间,假设运动小车的初始位置、状态方程(运动预测方程)、传感器测量数据,用粒子滤波方法进行对其进行定位。

    2.2 预设参数

    (1)粒子总数N=200;

    (2)运动时间T=10秒,且假设每秒进行一步动作;

    (3)运动场地大小WorldSizeWorldSize=100100平方米;

    (4)控制小车运动的方程,小车沿着某曲线运动(但实际的运动情况肯定和给的控制有些差别,这理解为叠加在理想运动方程上的控制误差,控制误差假设为5米);

    (5)传感器测量的小车位置数据同样也和实际情况不一致,这也可以理解为叠加在真实位置上的测量误差,测量误差假设为5米;

    (6)小车初始位置已知,假设处在100*100场地中的(50,20)位置;

    2.3 步骤与理解分析

    (1)初始化粒子群

    在整个场地内,将总粒子数N进行均匀分布,得到如图所示结果。
    在这里插入图片描述

    (2)小车开始运动(同时每运动一步进行一次测量)

    小车按照控制给定的叠加了控制噪声的运动方程进行运动,运动达到下一位置后,传感器对当前位置进行测量,并得到一个测量的位置(不准确,含有测量噪声)

    (3)粒子群更新(预测步骤)

    把粒子群中的全部粒子逐个带入小车的运动方程中,得到粒子群的下一步位置。同时,计算此时每个粒子的位置和测量得到的小车位置 这两个位置之间的几何距离,按照距离的不同给每个粒子添加一个权重,用于重采样。显然,距离越近,权重越大(权重和距离关系的函数,这里采用高斯分布钟形曲线的右侧,即随距离增大,权重单调递减)。下一步,得到全部粒子的权重后,将它们进行归一化。

    (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)); %初始系统的观测状态(为真实位姿叠加高斯噪声)
                                                 %y = wgn(m,n,p) 产生一个m行n列的高斯白噪声的矩阵,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)的高斯分布曲线)  因为均值为0且距离大于0 因此权重随着距离增加沿高斯曲线右侧递减
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); 
    end
    PCenter(:, 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 on
    set(0,'defaultfigurecolor','w')
    plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %真实的初始状态位置(红点表示)
    %grid on
    axis([0 100 0 100]);
    set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10
    set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
    plot(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)); %观测方程(状态上叠加测量的高斯噪声) 
        %粒子滤波
        %预测
        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 = randi(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轴坐标间隔显示 这里间隔为10
    set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
    legend('真实轨迹', '测量轨迹', '粒子群几何中心轨迹');
    xlabel('横坐标 x'); ylabel('纵坐标 y');
    %% 绘制误差
    figure(4);
    set(0,'defaultfigurecolor','w')
    %set(gca,'FontSize',12);%设置图标字体大小
    plot(err(1,:),'b.-');%err1为各时刻 真实位置与粒子群中心的几何距离
    hold on
    plot(err(2,:),'r.-');%err2为各时刻 真实位置与测量位置的几何距离
    hold off
    xlabel('步数 t');
    legend('粒子群误差', '测量误差');
    title('真实位置与粒子群中心的集合距离');
    

    3 更多

    3.1 失效恢复问题

    蒙特卡罗定位以目前的形式能够解决全局定位问题,但是不能从机器人绑架中或全局定位失效中恢复。定位过程中,获取位置的同时,不在最可能位置处的粒子会逐渐消失,在某个时候,只有单一位置的粒子能够“幸存”,如果这个位姿碰巧是错误的,算法不能恢复。实际上,任何随机算法(如蒙特卡罗定位算法),在重采样步骤中可能意外地丢弃所有正确位置附近的粒子,特别是当粒子数较少,且粒子扩散到较大空间时,这个问题就显得很重要了。

    • 解决办法:
      通过简单的探索算法可以解决这个问题,探索算法的思想是:增加随机粒子到粒子集合。通过假设机器人可能以小概率遭到绑架,注入一些随机粒子,从而在运动模型上产生一些随机状态,即使机器人不被绑架,随机粒子也能提升额外的鲁棒性级别。

    • 这引起两个问题:
      (1)在每次算法迭代中,应该增加多少粒子;(2)从哪种分布产生这些粒子。

    • 解答:
      (1)一种简单的方法是每次迭代增加固定数目的随机粒子;另一种更好的想法是基于某些定位性能的评估增加粒子。其中,实现第二种想法的一个方法是监控传感器测量的概率分布,在粒子滤波里,重要性权重是这个概率分布的随机估计,其均值(即增加的粒子数目)为:
      在这里插入图片描述

    (2)一种简单的方法是根据均匀分布在位置空间产生粒子,并用当前观测值加权这些粒子;另一种更好的想法是根据测量分布直接产生粒子,根据观测似然分布,附加的粒子能够直接放置在相应的位置上。

    展开全文
  • 粒子滤波算法

    2020-06-16 00:58:31
    粒子滤波器是一新类型的非线性滤波器,已成为一个解决非线性滤波问题的重要工具,因为它能应用于很多领域,如信号处理、雷达和声音媒体的目标跟踪、计算机视觉、神经计算。 令XnX_nXn​表示所有的目标状态序列{xi}i...
  • particle filtering---粒子滤波(讲的很通俗易懂)

    万次阅读 多人点赞 2018-09-19 21:04:17
    在论文中看到粒子滤波的知识点,在网上找到的几篇讲的很易的文章: http://blog.csdn.net/heyijia0327/article/details/40899819 http://blog.csdn.net/heyijia0327/article/details/40929097 ...
  • Opencv实现粒子滤波算法

    千次阅读 2017-09-28 11:22:48
    Opencv实现粒子滤波算法  摘要    本文通过OpenCV实现了一种目标跟踪算法——粒子滤波算法,算法的思想来源于文献[1][2],且在其思想上稍微做了些修改。其大概过程是:首先手动用鼠标框出一个目标区域,计算其...
  • 经典算法粒子滤波

    千次阅读 2014-12-02 17:51:23
    粒子滤波(PF: Particle Filter)算法来源于蒙特卡洛方法(Monte Carlo method),它是利用粒子集来表示概率,可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表示其分布情况,是...
  • 通俗易懂的粒子滤波算法(PF)

    万次阅读 热门讨论 2018-07-11 09:05:33
    在论文中看到粒子滤波的知识点,在网上找到的几篇讲的很易的文章:...
  • 粒子滤波(Particle filter)算法简介及MATLAB实现

    万次阅读 多人点赞 2018-11-12 10:56:05
    因此,想要掌握粒子滤波,对于上述两个基本内容必须有一个初步的了解。重要性采样呢,其实就是根据对粒子的信任程度添加不同的权重,添加权重的规则就是:对于我们信任度高的粒子,给它们添加的权重就相对大一些;...
  • 本文是《Improved Techniques for Grid Mapping_with Rao-Blackwellized Particle Filters》的大致翻译,难免有不通顺与错误的地方,如有错误请指出,谢谢! 设想一个机器人在一个未知环境中移动,其目的是获得当前...
  • 用俗话讲讲卡尔曼滤波与粒子滤波

    万次阅读 多人点赞 2016-07-24 21:34:15
    一,卡尔曼滤波 卡尔曼滤波可以根据一些已知的量来预测未知的量,这些量受到的干扰必须得近似高斯噪声。这个东西可以用来干什么呢?例如我们可以用来预测明天,后天,未来好几天的温度。我们可以在前几天用温度计...
  • 基于粒子滤波的SLAM(GMapping)算法分析

    万次阅读 多人点赞 2019-07-26 16:56:30
    本文是《Improved Techniques for Grid Mapping_with Rao-Blackwellized Particle Filters》的大致翻译,难免有不通顺与错误的地方,如有错误请指出,谢谢! 设想一个机器人在一个未知环境中移动,其目的是获得当前...
  • 粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods),它是利用粒子集来表示概率,可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布,是一种...
  • 粒子滤波算法源于蒙特卡洛思想,即以某事件出现的频率来指代该事件的概率。通俗的讲,粒子滤波也是能用已知的一些数据预测未来的数据。我们知道,科尔曼滤波限制噪声时服从高斯分布的,但是粒子滤波可以不局限于高斯...
  • 粒子滤波总结(摘来总结)

    千次阅读 2014-12-16 15:42:33
    粒子滤波总结(摘来总结) OpenCV中实现了粒子滤波的代码,位置在opencv\cv\src\cvcondens.cpp文件 粒子滤波跟踪器的数据结构: typedef struct CvConDensation { int MP; // 测量向量的维数: Dimension of ...
  • 粒子滤波(Particle Filter)概念浅谈

    万次阅读 2013-05-19 15:56:31
    粒子滤波  也就是知名的连续蒙特卡洛方法(SMC),是一种基于仿真的成熟模型估计技术。粒子滤波通常用于估计与马尔科夫链联系的潜在变量的贝叶斯模型---类似于隐式马尔科夫模型,但是在潜在变量的状态空间是连续而...
  • 粒子滤波代码

    千次下载 热门讨论 2020-07-29 15:33:30
    压缩包中有三个粒子滤波的演示程序,一个滤波,一个目标跟踪,一个机器人定位。关于效果,大家可以先看看http://blog.csdn.net/heyijia0327/article/details/41142679。再决定是否下载。
  • 在前一个博客中已经对机器视觉及粒子滤波算法的相关原理进行了介绍,在基于410c平台实现粒子滤波算法的前期,我们在上位机上应用QT和Opencv完成了粒子滤波算法的实现和测试。
  • 本文已搬家至【OpenCV学习】物体跟踪的粒子滤波算法
1 2 3 4 5 ... 20
收藏数 3,937
精华内容 1,574
关键字:

粒子滤波算法