145 lines
4.4 KiB
Matlab
145 lines
4.4 KiB
Matlab
|
|
|
|
%tgt_data = readtable('gmc_table_4d2258.txt');
|
|
%tgt_truth = readtable('air_data');
|
|
%tgt_lalonidl = readtable('latlonidl.dat');
|
|
|
|
|
|
[file,location] = uigetfile('C:\Progetti_MATLAB\Search&Track\Data\*.dat');
|
|
|
|
|
|
tgt_gmtt = readtable(fullfile(location,file));
|
|
|
|
size_tgt = size(tgt_gmtt);
|
|
|
|
lattgtpath= [];
|
|
lontgtpath= [];
|
|
|
|
%tgt_data.az= rad2deg(tgt_data.az);
|
|
%tgt_data.el= rad2deg(tgt_data.el);
|
|
|
|
|
|
heading_own = 205;
|
|
wander_angle = -0.8;
|
|
|
|
wgs84 = wgs84Ellipsoid('meter');
|
|
|
|
%%%%%%%%%PLOT SCAN%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(1)));
|
|
|
|
%wrapTo180(heading_own-tgt_data.az(k))
|
|
|
|
OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(1));
|
|
OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(1));
|
|
|
|
currentaz = heading_own;
|
|
maxrng= max(tgt_gmtt.Range);
|
|
beamw= 4;
|
|
|
|
scanregion = [-60, 60, 0, 30];
|
|
azscanspan = diff(scanregion(1:2));
|
|
numazscan = ceil(azscanspan/beamw);
|
|
azscanangles = linspace(scanregion(1),scanregion(2),numazscan);
|
|
|
|
angmin = azscanangles(1)-beamw/2;
|
|
angmax = azscanangles(end)+beamw/2;
|
|
|
|
sareax = [0 maxrng*sind(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
|
|
sareay = [0 maxrng*cosd(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
|
|
|
|
sizeaz = size(sareax);
|
|
|
|
[x1,y1,z1] = enu2ecef(sareax,zeros(1,sizeaz(2)),zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
|
|
[~,lonP1,~] = ecef2gno(x1,y1,z1);
|
|
|
|
[x2,y2,z2] = enu2ecef(zeros(1,sizeaz(2)),sareay,zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
|
|
[latP2,~,~] = ecef2gno(x2,y2,z2);
|
|
|
|
figure;
|
|
ploth.own = geoplot(latP2,lonP1,'blue-',LineWidth=2);
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Set the basemap
|
|
geobasemap('topographic'); % Choose your preferred basemap
|
|
title('MAP Tracking');
|
|
|
|
OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(1)));
|
|
|
|
tgt_east= - tgt_gmtt.Range(1) * sind(wrapTo180(rad2deg(tgt_gmtt.Azimuth(1))+wander_angle));
|
|
tgt_north= tgt_gmtt.Range(1) * cosd(wrapTo180(rad2deg(tgt_gmtt.Azimuth(1))+wander_angle));
|
|
|
|
|
|
OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(1));
|
|
OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(1));
|
|
|
|
[x1,y1,z1] = enu2ecef(tgt_east,tgt_north,200,OwnPosLatitude,OwnPosLongitude,2000,wgs84);
|
|
[lattgt,lontgt,~] = ecef2gno(x1,y1,z1);
|
|
|
|
|
|
hold on;
|
|
|
|
ploth.marker{1} = geoplot(lattgt(1),lontgt(1),"ow","MarkerSize",10,"MarkerFaceColor","r");
|
|
|
|
step=10;
|
|
for k=1:step:size_tgt(1)
|
|
|
|
k
|
|
tic
|
|
%heading_tgt= rad2deg(atan2(-tgt_data.vy(k),tgt_data.vx(k)));%tgt_heading(XR,XAZ,XEL);
|
|
OwnPosHeading = wrapTo360(rad2deg(tgt_gmtt.Heading(k)));
|
|
|
|
tgt_east= - tgt_gmtt.Range(k) * sind(wrapTo180(rad2deg(tgt_gmtt.Azimuth(k))+wander_angle));
|
|
tgt_north= tgt_gmtt.Range(k) * cosd(wrapTo180(rad2deg(tgt_gmtt.Azimuth(k))+wander_angle));
|
|
|
|
%wrapTo180(heading_own-tgt_data.az(k))
|
|
|
|
OwnPosLatitude = rad2deg(tgt_gmtt.Latitude(k));
|
|
OwnPosLongitude = rad2deg(tgt_gmtt.Longitude(k));
|
|
|
|
[x1,y1,z1] = enu2ecef(tgt_east,tgt_north,200,OwnPosLatitude,OwnPosLongitude,800,wgs84);
|
|
[lattgt,lontgt,~] = ecef2gno(x1,y1,z1);
|
|
|
|
|
|
set(ploth.marker{1},"LatitudeData",lattgt,"LongitudeData",lontgt);
|
|
|
|
lattgtpath = cat(1,lattgtpath,lattgt);
|
|
lontgtpath = cat(1,lontgtpath,lontgt);
|
|
|
|
geoplot(lattgtpath, lontgtpath, 'magenta', LineWidth=2)
|
|
|
|
%Aggiornamento Ownship ------------------------
|
|
|
|
sareax = [0 maxrng*sind(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
|
|
sareay = [0 maxrng*cosd(-[angmax -azscanangles angmin]+OwnPosHeading) 0];
|
|
|
|
sizeaz = size(sareax);
|
|
|
|
[x1,y1,z1] = enu2ecef(sareax,zeros(1,sizeaz(2)),zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
|
|
[~,lonP1,~] = ecef2gno(x1,y1,z1);
|
|
|
|
[x2,y2,z2] = enu2ecef(zeros(1,sizeaz(2)),sareay,zeros(1,sizeaz(2)),OwnPosLatitude,OwnPosLongitude,0.0,wgs84);
|
|
[latP2,~,~] = ecef2gno(x2,y2,z2);
|
|
|
|
%sareax = lonP1;
|
|
%sareay = latP2;
|
|
|
|
set(ploth.own,"LatitudeData",latP2,"LongitudeData",lonP1);
|
|
|
|
%-----------------------------------------------
|
|
|
|
|
|
dt.DataIndex = k;
|
|
|
|
drawnow
|
|
TimeSearch = 0.1;
|
|
TimeElab=toc;
|
|
PauseVal=max(TimeSearch-TimeElab,0);
|
|
pause(PauseVal)
|
|
|
|
end
|
|
|
|
|
|
|
|
aa='fine' |