plot_scn.m

一个目标跟踪系统的MATLAB 源程序包

function fig = plot_scn(target, measurement)

numScans = length(target(1).time);
% plot the ground truth and the measurements
fig = figure;
hold on;

ny = length(target);
for j=1:ny
    p0 = plot(target(j).state(1,:), target(j).state(3,:), 'b');
end

for i=1:numScans
    for j=1:length(measurement(i).flag)
        meas_x = measurement(i).range(j)*cos(measurement(i).bearing(j)) + measurement(i).pos(1);
        meas_y = measurement(i).range(j)*sin(measurement(i).bearing(j)) + measurement(i).pos(2);        
        if measurement(i).flag(j) == 1
            p1 = plot(meas_x, meas_y, 'm.');
        elseif measurement(i).flag(j) == 0
            p2 = plot(meas_x, meas_y, 'k*');
        else
            p3 = plot(meas_x, meas_y, 'c+');
        end
    end
end
legend([p0, p1, p2, p3], 'target', 'resolved', 'unresolved', 'false');
xlabel('x position (m)');
ylabel('y position (m)');
title('Measurements of the two crossing targets from two sensors');
hold off;