基于多项重采样的粒子滤波-matlab

最新推荐文章于 2023-06-13 15:30:03 发布

weixin_30856725

最新推荐文章于 2023-06-13 15:30:03 发布

阅读量183

点赞数

CC 4.0 BY-SA版权

文章标签： matlab

原文链接：http://www.cnblogs.com/zjuhjm/archive/2013/02/27/2935676.html

本文详细阐述了粒子滤波算法在跟踪领域的应用，通过实例代码展示了如何使用该算法进行目标跟踪。包括初始化粒子、更新粒子权重、重采样等关键步骤，并最终实现目标位置的估计。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

clear
xp=0.1;
Q=5;
R=1;
P=10;
tf=100;
Np=10000;

particle_p=sqrt(P)*randn(1,Np);
wk_i=ones(1,Np)/Np;
c=zeros(Np,1);

for k=1:tf
    xk=xp/2+(25*xp/(1+xp^2))+8*cos(1.2*(k-1))+sqrt(Q)*randn;
    zk=xk^2/20+sqrt(R)*randn;
    xp=xk;
    x(k,1)=xk;
    
    %Selecting particles
    particle=0.5*particle_p + (25*particle_p./(1+particle_p.^2))+...
    8*cos(1.2*(k-1))+sqrt(Q)*randn;  
    z_p=particle.^2/20;
    
    wk=(1/sqrt(R)/sqrt(2*pi))*exp(-0.5/R*(z_p-zk).*(z_p-zk));
    wk=wk_i.*wk;  
    wk=wk/sum(wk); wk_i=wk;
    xest=sum(particle.*wk);
    y(k,1)=xest;
    
 %多项重采样算法     
    c(1)=wk(1);
    for i=2:1:Np
        c(i)=c(i-1)+wk(i);
    end
    u=zeros(Np,1);
    u(1)=rand/Np;
    i=1;
    for j=1:1:Np
       u(j)=u(1)+(j-1)/Np; 
        if u(j)>c(i)
            i=i+1;
        else
            particle(j)=particle_p(i);
            wk_i(j)=1/Np;
        end 
    end
  %  wk_i=wk_i/sum(wk_i);
    particle_p=particle;
end
hold on
t=1:tf;
plot(t,x)
plot(t,y,'r')