flying_airplane_localization_tracking.m

对运动声目标进行航迹跟踪

%% Target Localization;
%% 2005-11-17;MaDengYong;

clc;
clear all;
close all;

Vxy=[0 200;200 0;100 100;100 200;100 0;200 200;0 0];

XY_i=Vxy;
fai=0*pi/180; 
W=[cos(fai) sin(fai);-sin(fai) cos(fai)];
XY_r=(inv(W)*Vxy.').';

IP_set=[0 3 4 5 6 7 8];


%%   197-6      195-4     190-1


%%              194-3   


%%   193-2      196-5      198-7


% IP_id=[2;7];
IP_id=[4;6];
% IP_id=[1;4];
% IP_id=[1;6];
ID=[IP_set(IP_id(1));IP_set(IP_id(2))];
LP=[48:137];

cp='sixth data\';

sr=['F:\2005_11_15\2005_11_15\afternoon_second_group\' cp];


path1=[sr 'AoA_tracking_values_paper\'];


load([path1 '19' num2str(ID(1)) '_thita_long']); 
thite11=thit_r;
clear thit_r;

load([path1 '19' num2str(ID(2)) '_thita_long']);
thite22=thit_r;
clear thit_r;

lp_l=LP(:,1);
lp_r=LP(:,length(LP));
[APr_xy,APi_xy]=airplane_tracking(XY_r,thite11,thite22,IP_id,LP);
lp_n=size(APr_xy,1);

% APrr_xy=[];
% % for lp_1=2:lp_n
% %     APrr_xy=[ APrr_xy;(APr_xy(lp_1-1,:)+APr_xy(lp_1,:))/2];
% % end
%   for lp4=1:lp_n-5
%           
%        a=APr_xy(lp4,:);
%        b=APr_xy(lp4+1,:);
%        c=APr_xy(lp4+2,:);
%        d=APr_xy(lp4+3,:);
%        e=APr_xy(lp4+4,:);
%        r1=[a;b;c];r2=[b;c;d];r3=[c;d;e];
%        ra=max([min(r1);min(r2);min(r3)]);
%        rb=min([max(r1);max(r2);max(r3)]);
%       APrr_xy=[APrr_xy;0.5*ra+0.5*rb];
%    end
%    
% APr_xy=medfilt2(APr_xy);
% APi_xy=medfilt2(APr_xy);

path2=[sr '\location_tracking_curves\'];

figure(1);
hold on;
plot_node4(XY_r,ID);

plot(APr_xy(:,1),APr_xy(:,2),'r- .','linewidth',2,'markersize',15);
plot(APr_xy(1,1),APr_xy(1,2),'ko','markersize',10);
plot(APr_xy(size(APr_xy,1),1),APr_xy(size(APr_xy,1),2),'ro','markersize',10);

hold off;
grid;
set(gca,'xlim',[-20,220],'ylim',[-20,220],'linewidth',2,'fontsize',12,'fontweight','bold');
set(gca,'linewidth',2,'fontsize',12,'fontweight','bold');
xlabel('X Axis (meter) ','FontSize',12,'FontWeight','bold');
ylabel('Y Axis (meter) ','FontSize',12,'FontWeight','bold');
title1='Airplane Location Tracking(Rotational Coordinate) ';
title([title1 '19' num2str(ID(1))  '-19' num2str(ID(2)) ' (' num2str(lp_l) '~' num2str(lp_r) ')'],'FontSize',12,'FontWeight','bold');
% saveas(gcf,[path2 'airplane_location_tracking_rotational_smooth' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);
% saveas(gcf,[path2 'airplane_location_tracking_rotational' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);


% figure(1);
% hold on;
% plot_node4(XY_r,ID);
% 
% plot(APrr_xy(:,1),APrr_xy(:,2),'r- .','linewidth',2,'markersize',15);
% plot(APrr_xy(1,1),APrr_xy(1,2),'ko','markersize',10);
% plot(APrr_xy(size(APrr_xy,1),1),APrr_xy(size(APrr_xy,1),2),'ro','markersize',10);
% 
% hold off;
% grid;
% set(gca,'xlim',[-20,220],'ylim',[-20,220],'linewidth',2,'fontsize',12,'fontweight','bold');
% set(gca,'linewidth',2,'fontsize',12,'fontweight','bold');
% xlabel('X Axis (meter) ','FontSize',12,'FontWeight','bold');
% ylabel('Y Axis (meter) ','FontSize',12,'FontWeight','bold');
% title1='Airplane Location Tracking(Rotational Coordinate) ';
% title([title1 '19' num2str(ID(1))  '-19' num2str(ID(2)) ' (' num2str(lp_l) '~' num2str(lp_r) ')'],'FontSize',12,'FontWeight','bold');
% saveas(gcf,[path2 'airplane_location_tracking_rotational_smooth' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);
% saveas(gcf,[path2 'airplane_location_tracking_rotational' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);


figure(2);
hold on;
plot_node4(XY_i,ID);

plot(APi_xy(:,1),APi_xy(:,2),'r- .','linewidth',2,'markersize',15);
plot(APi_xy(1,1),APi_xy(1,2),'ko','markersize',10);
plot(APi_xy(size(APi_xy,1),1),APi_xy(size(APi_xy,1),2),'ro','markersize',10);

hold off;
grid;
% set(gca,'xlim',[-20,220],'ylim',[-20,220],'linewidth',2,'fontsize',12,'fontweight','bold');
set(gca,'linewidth',2,'fontsize',12,'fontweight','bold');
xlabel('X Axis (meter) ','FontSize',12,'FontWeight','bold');
ylabel('Y Axis (meter) ','FontSize',12,'FontWeight','bold');
title1='Airplane Location Tracking(Conventional Coordinate)- ';
title([title1 '19' num2str(ID(1))  '-19' num2str(ID(2)) ' (' num2str(lp_l) '~' num2str(lp_r) ')'],'FontSize',12,'FontWeight','bold');
% saveas(gcf,[path2 'airplane_location_tracking_conventional_smooth' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);
% saveas(gcf,[path2 'airplane_location_tracking_conventional' '_19' num2str(ID(1))   '_19' num2str(ID(2))]);