From cd3463d47911a9c5acf3c61aef503cd1d569bf30 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Wed, 4 Mar 2026 22:32:39 -0800 Subject: [PATCH] finalized plotting utility --- @miSim/miSim.m | 2 +- aerpaw/results/plotGpsLogs.m | 24 ++++--- aerpaw/results/plotRadioLogs.m | 4 +- aerpaw/results/plotResults.m | 14 ---- aerpaw/results/resultsAnalysis.m | 66 +++++++++++++++++++ .../7gy57K96rjt00mZ48_G_oNq20IMp.xml | 2 - ...d.xml => qpxmhcwIacwqdjVXqua3hmJxvJYd.xml} | 0 .../qpxmhcwIacwqdjVXqua3hmJxvJYp.xml | 2 + 8 files changed, 86 insertions(+), 28 deletions(-) delete mode 100644 aerpaw/results/plotResults.m create mode 100644 aerpaw/results/resultsAnalysis.m delete mode 100644 resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMp.xml rename resources/project/cCclYJTOop6jkdZsItlf7iNuov4/{7gy57K96rjt00mZ48_G_oNq20IMd.xml => qpxmhcwIacwqdjVXqua3hmJxvJYd.xml} (100%) create mode 100644 resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYp.xml diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 8f15aa7..dd1d75b 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -19,6 +19,7 @@ classdef miSim barrierExponent = NaN; % CBF exponent parameter minAlt = 0; % minimum allowable altitude (m) artifactName = ""; + f; % main plotting tiled layout figure fPerf; % performance plot figure end @@ -31,7 +32,6 @@ classdef miSim % Plot objects makePlots = true; % enable/disable simulation plotting (performance implications) makeVideo = true; % enable/disable VideoWriter (performance implications) - f; % main plotting tiled layout figure connectionsPlot; % objects for lines connecting agents in spatial plots graphPlot; % objects for abstract network graph plot partitionPlot; % objects for partition plot diff --git a/aerpaw/results/plotGpsLogs.m b/aerpaw/results/plotGpsLogs.m index 1750b70..abcbec1 100644 --- a/aerpaw/results/plotGpsLogs.m +++ b/aerpaw/results/plotGpsLogs.m @@ -1,9 +1,11 @@ -function f = plotGpsLogs(logDirs) +function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel) arguments (Input) logDirs (1, 1) string; + seaToGroundLevel (1, 1) double = 110; % measured approximately from USGS national map viewer for the AERPAW test field end arguments (Output) f (1, 1) matlab.ui.Figure; + G cell; end % Plot setup f = uifigure; @@ -17,9 +19,6 @@ function f = plotGpsLogs(logDirs) % configured data params = readScenarioCsv(scenarioCsv); - % coordinate system constants - seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer - fID = fopen(fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "client1.yaml"), 'r'); yaml = fscanf(fID, '%s'); fclose(fID); @@ -45,12 +44,19 @@ function f = plotGpsLogs(logDirs) % Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight) pctThreshold = 60; % pctThreshold may need adjusting depending on your flight - startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'first'); - stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'last'); + startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "first"); + stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last"); % % Plot whole flight, including setup/cleanup % startIdx = 1; % stopIdx = length(verticalSpeed); + + % Convert LLA trajectory data to ENU for external analysis + % NaN out entries outside the algorithm flight range so they don't plot + enu = NaN(height(G{ii}), 3); + 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]; % 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); @@ -58,7 +64,7 @@ function f = plotGpsLogs(logDirs) % Plot domain altOffset = 1; % to avoid clipping into the ground when displayed - domain = [lla0; enu2lla(params.domainMax, lla0, 'flat')]; + domain = [lla0; enu2lla(params.domainMax, lla0, "flat")]; geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k'); geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k'); geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k'); @@ -72,12 +78,12 @@ function f = plotGpsLogs(logDirs) % Plot objective objectivePos = [params.objectivePos, 0]; - llaObj = enu2lla(objectivePos, lla0, 'flat'); + llaObj = enu2lla(objectivePos, lla0, "flat"); geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y'); % Plot obstacles for ii = 1:params.numObstacles - obstacle = enu2lla([params.obstacleMin((1 + (ii - 1) * 3):(ii * 3)); params.obstacleMax((1 + (ii - 1) * 3):(ii * 3))], lla0, 'flat'); + obstacle = enu2lla([params.obstacleMin((1 + (ii - 1) * 3):(ii * 3)); params.obstacleMax((1 + (ii - 1) * 3):(ii * 3))], lla0, "flat"); geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r'); geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r'); geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r'); diff --git a/aerpaw/results/plotRadioLogs.m b/aerpaw/results/plotRadioLogs.m index 1f5e7be..dd5efe0 100644 --- a/aerpaw/results/plotRadioLogs.m +++ b/aerpaw/results/plotRadioLogs.m @@ -1,10 +1,10 @@ -function f = plotRadioLogs(resultsPath) +function [f, R] = plotRadioLogs(resultsPath) arguments (Input) resultsPath (1, 1) string; end - arguments (Output) f (1, 1) matlab.ui.Figure; + R cell; end logDirs = dir(resultsPath); diff --git a/aerpaw/results/plotResults.m b/aerpaw/results/plotResults.m deleted file mode 100644 index f02131c..0000000 --- a/aerpaw/results/plotResults.m +++ /dev/null @@ -1,14 +0,0 @@ -%% Plot AERPAW logs (trajectory, radio) -resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "t1"); % Define path to results copied from AERPAW platform - -% Plot GPS logged data and scenario information (domain, objective, obstacles) -fGlobe = plotGpsLogs(resultsPath); - -% Plot radio statistics -fRadio = plotRadioLogs(resultsPath); - -%% Run simulation -% Run miSim using same AERPAW scenario definition CSV - - -%% Plot AERPAW trajectory logs onto simulated result for comparison \ No newline at end of file diff --git a/aerpaw/results/resultsAnalysis.m b/aerpaw/results/resultsAnalysis.m new file mode 100644 index 0000000..9e0afda --- /dev/null +++ b/aerpaw/results/resultsAnalysis.m @@ -0,0 +1,66 @@ +%% Plot AERPAW logs (trajectory, radio) +resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "t1"); % Define path to results copied from AERPAW platform + +% Plot GPS logged data and scenario information (domain, objective, obstacles) +seaToGroundLevel = 110; % measured approximately from USGS national map viewer +[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel); + +% Plot radio statistics +[fRadio, R] = plotRadioLogs(resultsPath); + +%% Run simulation +% Run miSim using same AERPAW scenario definition CSV +csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv"); +params = readScenarioCsv(csvPath); + +% Visualization settings +plotCommsGeometry = false; +makePlots = true; +makeVideo = true; + +% Define scenario according to CSV specification +domain = rectangularPrism; +domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain"); +domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum); + +agents = cell(size(params.initialPositions, 2) / 3, 1); +for ii = 1:size(agents, 1) + agents{ii} = agent; + + sensorModel = sigmoidSensor; + sensorModel = sensorModel.initialize(params.alphaDist(ii), params.betaDist(ii), params.alphaTilt(ii), params.betaTilt(ii)); + + collisionGeometry = spherical; + collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii)); + + agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), plotCommsGeometry); +end + +% Create obstacles +obstacles = cell(params.numObstacles, 1); +for ii = 1:size(obstacles, 1) + obstacles{ii} = rectangularPrism; + obstacles{ii} = obstacles{ii}.initialize([params.obstacleMin((((ii - 1) * 3) + 1):(ii * 3)); params.obstacleMax((((ii - 1) * 3) + 1):(ii * 3))], "OBSTACLE", sprintf("Obstacle %d", ii)); +end + +% Set up simulation +sim = miSim; +sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo); + +% Save simulation parameters to output file +sim.writeInits(); + +% Run +sim = sim.run(); + +%% Plot AERPAW trajectory logs onto simulated result for comparison +% Duplicate plot to overlay with logged trajectories +comparison = figure; +copyobj(sim.f.Children, comparison); + +% Plot trajectories on top +hold(comparison.Children.Children(end), "on"); +for ii = 1:size(G, 1) + plot3(comparison.Children(1).Children(end), G{ii}.East, G{ii}.North, G{ii}.Up + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1); +end +hold(comparison.Children.Children(end), "off"); \ No newline at end of file diff --git a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMp.xml b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMp.xml deleted file mode 100644 index 046f9a9..0000000 --- a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMp.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMd.xml b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYd.xml similarity index 100% rename from resources/project/cCclYJTOop6jkdZsItlf7iNuov4/7gy57K96rjt00mZ48_G_oNq20IMd.xml rename to resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYd.xml diff --git a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYp.xml b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYp.xml new file mode 100644 index 0000000..4d63f33 --- /dev/null +++ b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/qpxmhcwIacwqdjVXqua3hmJxvJYp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file