bayes_demo.m

目标跟踪的扩展卡尔曼滤波算法主函数的文件是：kal_demo.m 近似网格滤波的主函数文件是：bayes_demo.m 近似网格滤波划分网格的方法是：以目标上一个时刻的位置作为中心进行网格

%
%
%
%
%
%
%
%
%
%
%
%


clear all

sensors = dispose_sensors();
slot = create_slot();
com_radius = 10;
leader = 0;
detect_max = 10;
temp_area = zeros(2,25);
bayesXY = zeros(2,100);
Ppre = ones(1,25);

wucha1 = zeros(1,100);
%wucha2 = zeros(1,100);

mf = moviein(100);

for t=1:100
    
    if t == 1
        temp_area = find_area([1;0]);
        leader = 1;
    end
    
    if t > 2
        Ppre = predict_area(slot(:,t-2),slot(:,t-1));
    end
    
    [distances count list] = measurement(sensors, leader, slot(:, t), detect_max, com_radius);
    temp_xy = zeros(2,count);
%    temp_xy2 = zeros(2,count);
    
    for i=1:count
        
        P = zeros(25);
        Pnext = zeros(25);
%        Pnext2 = zeros(25);
        Ptotal = 0;
%        Ptotal2 = 0;
        distance = distances(i)*randint(1,1,[8000,12000])*0.0001
        for j=1:25
            sensor_object = sqrt((temp_area(1,j) + 0.5 - sensors(1,list(i)))^2 ...
                + (temp_area(2,j) + 0.5 - sensors(2,list(i)))^2);
            P(j) = N((distance - sensor_object + 0.5)/sqrt(2)) ...
                - N((distance - sensor_object - 0.5)/sqrt(2));
            Ptotal = Ptotal + P(j) * Ppre(j);
 %           Ptotal2 = Ptotal2 + P(j);
        end
        for k=1:25
            Pnext(k) = P(k)*Ppre(k) / Ptotal;
%            Pnext2(k) = P(k) / Ptotal2;
            temp_xy(:,i) = temp_xy(:,i) + Pnext(k) * (temp_area(:,k) + 0.5);
%            temp_xy2(:,i) = temp_xy2(:,i) + Pnext2(k) * (temp_area(:,k) + 0.5);
        end
    end
    
    
    bayesXY(:,t) = mean(temp_xy')';
%    bayesXY2(:,t) = mean(temp_xy2')';
        
    loc = index(bayesXY(:,t));
        
    temp_area = find_area(loc);
        
    leader = predict_leader(loc);
    
    wucha1(t) = sqrt((slot(1,t)-bayesXY(1,t))^2 + (slot(2,t)-bayesXY(2,t))^2);
%    wucha2(t) = sqrt((slot(1,t)-bayesXY2(1,t))^2 + (slot(2,t)-bayesXY2(2,t))^2);

plot(slot(1,1:t),slot(2,1:t),'r-',bayesXY(1,1:t),bayesXY(2,1:t),'g-');    
axis([-20,120,-20,120]);
axis manual;
mf(t) = getframe;

end
avp=mean(wucha1)

%plot(1:100,wucha1(:),'b-',1:100,avp,'r-');
%axis([0,100,0,10]);
%axis manual;