bayes_demo.asv

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

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


clear all

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

wucha1 = zeros(1,100);
wucha2 = zeros(1,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) + 2 * randn();
        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);
    axis([0,10,0,100]);
    axis manual;

    
end

plot(1:100,wucha1,'r-',1:100,wucha2,'g-');