better AERPAW collision analysis

aerpaw/controller.m

@@ -7,18 +7,21 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-% Three waypoints per UAV: one axis-aligned move per dimension (taxicab flyout/flyback)
+% Three waypoints per UAV: stage (climb) → traverse (XY) → settle (descend)
 MAX_TARGETS = MAX_CLIENTS * 3;
 
-% Taxicab flyout/flyback only supports exactly 2 UAVs
+% Altitude-staggered flyout/flyback currently supports exactly 2 UAVs
 if numClients ~= int32(2)
-    error('Taxicab flyout/flyback requires exactly 2 UAVs');
+    error('Altitude-staggered flyout/flyback requires exactly 2 UAVs');
 end
 
+% Minimum vertical separation maintained between UAVs during XY transit (metres)
+TRANSIT_SEP = 30.0;
+
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
-numWaypoints = int32(0);
+numWaypoints = int32(0); %#ok<NASGU>
-totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
+totalLoaded  = int32(0);
 
 % Experiment start positions from scenario CSV (N x 3)
 scenarioPositions = zeros(MAX_CLIENTS, 3);
@@ -92,11 +95,16 @@ else
     initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
 end
 
-% ---- Build taxicab flyout waypoints ------------------------------------------
+% ---- Build altitude-staggered flyout waypoints --------------------------------
-% Determine which UAV has the higher final altitude; it moves Z first so it
+% Each UAV follows three phases:
-% clears vertical separation before the lower UAV converges on the same X/Y.
+%   WP1: Climb to transit altitude at pad XY
-%   Higher UAV order: Z → Y → X
+%   WP2: Traverse to final XY at transit altitude  (>= TRANSIT_SEP vertical gap)
-%   Lower  UAV order: X → Y → Z
+%   WP3: Descend to final altitude at final XY
+%
+% Transit altitude assignment: the UAV with the highest final altitude keeps
+% that altitude as its transit altitude; the other UAV climbs TRANSIT_SEP above
+% it, guaranteeing >= 30 m vertical separation during the simultaneous XY
+% traverse (WP2).  Both transit altitudes are well above minAlt.
 if ~coder.target('MATLAB')
     if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
         higherIdx = int32(1);
@@ -106,20 +114,18 @@ if ~coder.target('MATLAB')
         lowerIdx  = int32(1);
     end
 
-    hBase = double(higherIdx - 1) * double(numWaypoints);
+    transitAlt = zeros(1, MAX_CLIENTS);
-    lBase = double(lowerIdx  - 1) * double(numWaypoints);
+    transitAlt(lowerIdx)  = scenarioPositions(lowerIdx, 3);
+    transitAlt(higherIdx) = scenarioPositions(lowerIdx, 3) + TRANSIT_SEP;
 
-    % Higher UAV: Z first
+    for iUAV = 1:numClients
-    targets(hBase + 1, :) = [initialPositions(higherIdx,1), initialPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+        base = double(iUAV - 1) * double(numWaypoints);
-    targets(hBase + 2, :) = [initialPositions(higherIdx,1), scenarioPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+        targets(base + 1, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), transitAlt(iUAV)];
-    targets(hBase + 3, :) =  scenarioPositions(higherIdx, :);
+        targets(base + 2, :) = [scenarioPositions(iUAV,1), scenarioPositions(iUAV,2), transitAlt(iUAV)];
-
+        targets(base + 3, :) =  scenarioPositions(iUAV, :);
-    % Lower UAV: X first
-    targets(lBase + 1, :) = [scenarioPositions(lowerIdx,1), initialPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
-    targets(lBase + 2, :) = [scenarioPositions(lowerIdx,1), scenarioPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
-    targets(lBase + 3, :) =  scenarioPositions(lowerIdx, :);
     end
-% ------------------------------------------------------------------------------
+end
+% -------------------------------------------------------------------------------
 
 % Waypoint loop: send each waypoint to all clients, wait for all to arrive
 for w = 1:numWaypoints
@@ -231,10 +237,11 @@ if ~coder.target('MATLAB')
 end
 % --------------------------------------------------------------------------
 
-% ---- Taxicab flyback: return each UAV to its takeoff-pad position ---------
+% ---- Altitude-staggered flyback -------------------------------------------
-% The UAV that ended guidance at the higher altitude moves Z last (X → Y → Z)
+% Mirror of flyout: stage (climb) → traverse (XY) → settle (descend to pad).
-% so it stays high while the lower UAV descends first, maintaining separation
+% Transit altitudes are based on post-guidance positions so the UAV that ended
-% as both converge back on their respective home X/Y positions.
+% higher stays highest, guaranteeing >= TRANSIT_SEP vertical clearance during
+% the simultaneous XY traverse.
 NUM_RETURN_WP = int32(3);
 returnTargets = zeros(MAX_TARGETS, 3);
 
@@ -247,18 +254,16 @@ if ~coder.target('MATLAB')
         lowerRetIdx  = int32(1);
     end
 
-    hRetBase = double(higherRetIdx - 1) * double(NUM_RETURN_WP);
+    returnTransitAlt = zeros(1, MAX_CLIENTS);
-    lRetBase = double(lowerRetIdx  - 1) * double(NUM_RETURN_WP);
+    returnTransitAlt(lowerRetIdx)  = positions(lowerRetIdx, 3);
+    returnTransitAlt(higherRetIdx) = positions(lowerRetIdx, 3) + TRANSIT_SEP;
 
-    % Higher post-guidance UAV: X → Y → Z (descend last)
+    for iUAV = 1:numClients
-    returnTargets(hRetBase + 1, :) = [initialPositions(higherRetIdx,1), positions(higherRetIdx,2),        positions(higherRetIdx,3)];
+        retBase = double(iUAV - 1) * double(NUM_RETURN_WP);
-    returnTargets(hRetBase + 2, :) = [initialPositions(higherRetIdx,1), initialPositions(higherRetIdx,2), positions(higherRetIdx,3)];
+        returnTargets(retBase + 1, :) = [positions(iUAV,1),         positions(iUAV,2),         returnTransitAlt(iUAV)];
-    returnTargets(hRetBase + 3, :) =  initialPositions(higherRetIdx, :);
+        returnTargets(retBase + 2, :) = [initialPositions(iUAV,1),  initialPositions(iUAV,2),  returnTransitAlt(iUAV)];
-
+        returnTargets(retBase + 3, :) =  initialPositions(iUAV, :);
-    % Lower post-guidance UAV: Z → Y → X (descend first)
+    end
-    returnTargets(lRetBase + 1, :) = [positions(lowerRetIdx,1),        positions(lowerRetIdx,2),        initialPositions(lowerRetIdx,3)];
-    returnTargets(lRetBase + 2, :) = [positions(lowerRetIdx,1),        initialPositions(lowerRetIdx,2), initialPositions(lowerRetIdx,3)];
-    returnTargets(lRetBase + 3, :) =  initialPositions(lowerRetIdx, :);
 
     for w = 1:NUM_RETURN_WP
         for i = 1:numClients
@@ -270,7 +275,7 @@ if ~coder.target('MATLAB')
         coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
     end
 else
-    disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.');
+    disp('Altitude-staggered return (simulation): UAVs commanded back to takeoff positions.');
 end
 % --------------------------------------------------------------------------
 

aerpaw/results/plotGpsLogs.m

@@ -78,6 +78,9 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
             d = d(~isnan(d));
 
             fprintf("Minimum distance between agents %d and %d is %2.3f\n", ii, jj, min(d));
+            figure;
+            plot(G{1}.Timestamp(2:(end - 1)), d);
+            xlabel("Time"); ylabel("Distance"); title("UAV0 and UAV1 distance"); grid("on");
             fprintf("Average distance between agents %d and %d is %2.3f\n", ii, jj, mean(d));
             fprintf("Median distance between agents %d and %d is %2.3f\n", ii, jj, median(d));
             if min(d) < 6