diff --git a/aerpaw/results/plotGpsLogs.m b/aerpaw/results/plotGpsLogs.m index 33d4185..637bfd9 100644 --- a/aerpaw/results/plotGpsLogs.m +++ b/aerpaw/results/plotGpsLogs.m @@ -29,6 +29,7 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight) logDirs = dir(logDirs); logDirs = logDirs(3:end); logDirs = logDirs([logDirs.isdir] == 1); + logDirs = logDirs(~startsWith({logDirs.name}, "controller_")); G = cell(size(logDirs)); for ii = 1:size(logDirs, 1) @@ -60,6 +61,27 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight) enu(startIdx:stopIdx, :) = lla2enu([G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx)], lla0, "flat"); enu = array2table(enu, 'VariableNames', ["East", "North", "Up"]); G{ii} = [G{ii}, enu]; + + % Do crude comparison of pairwise distances between this UAV and + % all previous UAVs + for jj = 1:(ii - 1) + Ai = G{ii}(:, [1, end-2:end]); + Aj = G{jj}(:, [1, end-2:end]); + + % Trim data to match sizes + idx = min([size(Ai, 1), size(Aj, 1)]); + Ai = Ai(1:idx, :); Aj = Aj(1:idx, :); + + pos_i = [Ai.East, Ai.North, Ai.Up]; + pos_j = [Aj.East, Aj.North, Aj.Up]; + d = vecnorm(pos_i - pos_j, 2, 2); + d = d(~isnan(d)); + + fprintf("Minimum distance between agents %d and %d is %2.3f\n", ii, jj, min(d)); + if min(d) < 6 + warning("Minimum distance between agents %d and %d of %2.3f is questionable for AERPAW", ii, jj, min(d); + end + end % Plot recorded trajectory over specified range of indices geoplot3(gf, G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx) + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5); diff --git a/aerpaw/results/resultsAnalysis.m b/aerpaw/results/resultsAnalysis.m index 1c7b59e..d480a72 100644 --- a/aerpaw/results/resultsAnalysis.m +++ b/aerpaw/results/resultsAnalysis.m @@ -16,7 +16,9 @@ fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(dt))); fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(dt))); fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(dt))); fprintf("Median command spacing: %2.3f seconds\n", seconds(median(dt))); -assert(seconds(min(dt)) > 4, "Minimum command spacing questionably short"); +if seconds(min(dt)) > 4 + warning("Minimum command spacing %2.3f questionably short for AERPAW", min(dt)); +end % Plot GPS logged data and scenario information (domain, objective, obstacles) seaToGroundLevel = 110; % measured approximately from USGS national map viewer