propagation.m

粒子滤波全套源代码

function [Sample_Set,after_prop]=Propagation(New_Sample_Set,Hx,Hy,vx,vy,image_boundary_x,image_boundary_y,I,N)

sigma_x=12;
sigma_y=12;


after_prop=I;

u1=double(vx);
u2=double(vy);
%u1=0;
%u2=0;
rn=random('Normal',u1,sigma_x,1,100*N);
rn2=random('Normal',u2,sigma_y,1,100*N);

count=1;
i=1;


while count<=N
    current_x=int16(New_Sample_Set(count).x);
    current_y=int16(New_Sample_Set(count).y);
    rand_x=int16(rn(i));
    rand_y=int16(rn2(i));
    while (current_x+rand_x-Hx<1||current_y+rand_y-Hy<1||current_x+rand_x+Hx>image_boundary_x||current_y+rand_y+Hy>image_boundary_y)
        i=i+1;
        rand_x=int16(rn(i));
        rand_y=int16(rn2(i));
    end
 %after select born new sample set 
 %linear stochastic differential equation  rand_x/y 
    Sample_Set(count).x=current_x+rand_x;
    Sample_Set(count).y=current_y+rand_y;     
     count=count+1;
    i=i+1;
end

%sample_draw    %to draw the samples out in output images
i=1;
for  i=1:1:N
  
            after_prop(Sample_Set(i).y,Sample_Set(i).x,2)=0;
            after_prop(Sample_Set(i).y,Sample_Set(i).x,3)=255;
            after_prop(Sample_Set(i).y,Sample_Set(i).x,1)=0;
end;