-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathplotDaaRrVsAzimuthWseeAndAvoid.m
119 lines (98 loc) · 5.8 KB
/
plotDaaRrVsAzimuthWseeAndAvoid.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
function [daaRr, dsaaRr]= plotDaaRrVsAzimuthWseeAndAvoid(azimuthDegOncoming,RminOncoming,azimuthDegOvertake,RminOvertake,VcloseOncoming,VcloseOvertake, daaSpec,IntSpeed,showPlot)
%[daaRr, dsaaRr]= plotDaaRrVsAzimuth(azimuthDegOncoming,RminOncoming,azimuthDegOvertake, RminOvertake,FOV,Range, OwnSpeed,IntSpeed,showPlot)
% daaRr is the RR for just the technical DAA system
% dsaaRr includes cases that are mitigated by intruder see and avoid
%Units should be in meters...that's what my labels say!....make showPlot 0 to not show plots
% NOTE: We need something passed for overtake cases. They can be empty vectors [] for cases where no overtakes are possible
%Does a polar plot for a given Rmin azimuth combination
% NOTE2: This code curently assumes a fixed range over the whole FOV...it's Iryna's job to convert this to make it variable range over the FOV
% NOTE3: This code also assumes a fixed azimuth interval for both oncoming and overtaking...don't change this, or the maths won't work
% DAA System Specs Here
FOV = daaSpec.FOV_deg; % Deg
Range = daaSpec.range_m; % Range in meters
OwnSpeed = daaSpec.ownSpeed_kts; % Leave in kts as it's only used for plot title
%Specs for See and avoid here:
detectionThreshArcMin = 30; % Fairly conservative
intruderManeuverDelay = 12.5; % Right out of AC 90-48D
[detectionRange,maxClosingVelForSeeAndAvoid] = calculateSeeAndAvoidForRpaSizeAndIntruderManeuverDelay(daaSpec.ownSize_m,detectionThreshArcMin,intruderManeuverDelay);
% Now we need to figure out what to do
%For plotting
daaSystemAzimuth = [-FOV/2:1:FOV/2]; % A vector of azimuths covered by the DAA system
daaRange = Range*ones(length(daaSystemAzimuth),1);
% Now we want to look at the azimuthDeg vector to see if there is a corresponding daaSystemAzimuth for whatever value is in there
%Start with Oncoming because there will always be oncoming cases.
numberOfAzimuthsEvaluated = 0;
numberOfAzimuthsPassed = 0;
indexOfOncomingAzimuthsPassed = [];
for (i=1:length(azimuthDegOncoming))
if (~isnan(RminOncoming(i)))
if (abs(azimuthDegOncoming(i))<=(FOV/2))
% The case is within the FOV
% Now evaluate for range
if (RminOncoming(i)<=Range)
numberOfAzimuthsPassed = numberOfAzimuthsPassed + 1;
indexOfOncomingAzimuthsPassed = [indexOfOncomingAzimuthsPassed i];
end
end
numberOfAzimuthsEvaluated = numberOfAzimuthsEvaluated + 1;
end
end % for loop through all oncoming azimuths
%Now lets calculate which oncoming azimuths were below the closing speed threshold for See and avoid to work
seeAndAvoidOncomingIndex = find(abs(VcloseOncoming)<maxClosingVelForSeeAndAvoid);
% Now we need to see if there are any Overtake azimuths
if (~isempty(azimuthDegOvertake))
indexOfOvertakeAzimuthsPassed = [];
for (i=1:length(azimuthDegOvertake))
if (~isnan(RminOvertake(i)))
if (abs(azimuthDegOvertake(i))<=(FOV/2)) %% The geometry is in the FOV
if (RminOvertake(i)< Range)
numberOfAzimuthsPassed = numberOfAzimuthsPassed + 1;
indexOfOvertakeAzimuthsPassed =[indexOfOvertakeAzimuthsPassed i];
end
end
numberOfAzimuthsEvaluated = numberOfAzimuthsEvaluated + 1;
end
end % for loop
seeAndAvoidOvertakeIndex = find(abs(VcloseOvertake)<maxClosingVelForSeeAndAvoid);
end % if we have overtakes
%Now we plot the results
if (showPlot ~= 0)
figure;
myPolarAxis = polaraxes;
%Start by plotting the failed cases
polarplot(myPolarAxis,azimuthDegOncoming*pi/180,RminOncoming,'.r');
hold on
if (~isempty(azimuthDegOvertake))
polarplot(myPolarAxis,azimuthDegOvertake*pi/180,RminOvertake,'.r');
end
%Now add in the cases that are mitigated by see and avoid
%Now overplot the see and avoid canses in blue?
polarplot(myPolarAxis,azimuthDegOncoming(seeAndAvoidOncomingIndex)*pi/180,RminOncoming(seeAndAvoidOncomingIndex),'.b');
if (~isempty(azimuthDegOvertake))
polarplot(myPolarAxis,azimuthDegOvertake(seeAndAvoidOvertakeIndex)*pi/180,RminOncoming(seeAndAvoidOvertakeIndex),'.b');
end
%Now plot the DAA system range/fov in black --
polarplot(myPolarAxis,[0 daaSystemAzimuth*pi/180 0],[0; daaRange; 0],'--k');
%Now overplot the passed cases in green
polarplot(myPolarAxis,azimuthDegOncoming(indexOfOncomingAzimuthsPassed)*pi/180,RminOncoming(indexOfOncomingAzimuthsPassed),'.g');
if (~isempty(azimuthDegOvertake))
polarplot(myPolarAxis,azimuthDegOvertake(indexOfOvertakeAzimuthsPassed)*pi/180,RminOvertake(indexOfOvertakeAzimuthsPassed),'.g');
end
myPolarAxis.ThetaZeroLocation = 'top';
myPolarAxis.ThetaDir = 'clockwise';
totalPassedOncoming = union(indexOfOncomingAzimuthsPassed,seeAndAvoidOncomingIndex);
totalPassedOvertake = union(indexOfOvertakeAzimuthsPassed, seeAndAvoidOvertakeIndex);
totalPassedDetectAndSeeAndAvoid = length(totalPassedOncoming) + length(totalPassedOvertake);
daaRr = (numberOfAzimuthsEvaluated-numberOfAzimuthsPassed)/numberOfAzimuthsEvaluated;
dsaaRr = (numberOfAzimuthsEvaluated-totalPassedDetectAndSeeAndAvoid)/numberOfAzimuthsEvaluated;
%Now formulate a label
myLabel = sprintf('OwnSpeed:%1.1f kts, IntSpeed:%1.1f kts, FOV:%1.0f, Range:%1.0f, DAA RR:%1.2f, D/SAA RR:%1.2f',OwnSpeed,IntSpeed,FOV,Range, daaRr, dsaaRr);
title(myLabel);
else
totalPassedOncoming = union(indexOfOncomingAzimuthsPassed,seeAndAvoidOncomingIndex);
totalPassedOvertake = union(indexOfOvertakeAzimuthsPassed, seeAndAvoidOvertakeIndex);
totalPassedDetectAndSeeAndAvoid = length(totalPassedOncoming) + length(totalPassedOvertake);
daaRr = (numberOfAzimuthsEvaluated-numberOfAzimuthsPassed)/numberOfAzimuthsEvaluated;
dsaaRr = (numberOfAzimuthsEvaluated-totalPassedDetectAndSeeAndAvoid)/numberOfAzimuthsEvaluated;
end
end