observe_and_estimate.m

粒子滤波全套源代码

function [Sample_probability,Estimate,vx,vy,after_prop]=Observe_and_Estimate(Sample_Set,Estimate,Hx,Hy,target_histgram,new_sita,loop,after_prop,I,N)

Total_probability=0;
for i=1:1:N
    Sample_histgram(i).element=histgram((Sample_Set(i).x),(Sample_Set(i).y),Hx,Hy,I); 
    Sample_probability(i)=weight(target_histgram,Sample_histgram(i).element,new_sita);
    Total_probability=Total_probability+Sample_probability(i);
end
Total_probability
Estimate(loop).x=0;
Estimate(loop).y=0;

for i=1:1:N
    Estimate(loop).x=Estimate(loop).x+double(Sample_Set(i).x)*(Sample_probability(i)/Total_probability); 
    Estimate(loop).y=Estimate(loop).y+double(Sample_Set(i).y)*(Sample_probability(i)/Total_probability);
end
Estimate(loop).histgram=histgram(round(Estimate(loop).x),round(Estimate(loop).y),Hx,Hy,I);
Estimate(loop).prob=weight(target_histgram,Estimate(loop).histgram,new_sita);


%计算速度
a=floor(Estimate(loop).x);
b=floor(Estimate(loop-1).x);
vx=a-b;
c=floor(Estimate(loop).y);
d=floor(Estimate(loop-1).y);
vy=c-d;


i=1;
for new_x=a-i:a+i;
        for new_y=c-i:c+i;
            after_prop(new_y,new_x,:)=255;
        end;
    end;

% edge_x=a-Hx;
% edge_y=c-Hy;
% for i=1:2*Hx
%     after_prop(c-Hy,edge_x+i,:)=255;
%     after_prop(c+Hy,edge_x+i,:)=255;
% end;
% for i=1:2*Hy
%     after_prop(edge_y+i,a-Hx,:)=255;
%     after_prop(edge_y+i,a+Hx,:)=255;
% end;
    
a=num2str(loop,'%03.3g');
b=['d:\result\',a,'.jpg'];
imwrite(after_prop,b);
%imshow([b]);