ran 3 new experiments on AERPAW simulation to test before submitting

.gitignore

@@ -4,6 +4,7 @@
 *.autosave
 *.slx.r*
 *.mdl.r*
+*.bak
 
 # Derived content-obscured files
 *.p

@miSim/constrainMotion.m

@@ -61,7 +61,9 @@ function [obj] = constrainMotion(obj)
     end
 
     idx = length(h(triu(true(size(h)), 1)));    
+    if coder.target('MATLAB')
 	obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
+    end
     idx = idx + 1;
 
     hObs = NaN(nAgents, size(obj.obstacles, 1));
@@ -84,7 +86,9 @@ function [obj] = constrainMotion(obj)
         end
     end
 
+    if coder.target('MATLAB')
 	obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+    end
     idx = idx + numel(hObs);
 
     % Set up domain constraints (walls and ceiling only)
@@ -128,13 +132,10 @@ function [obj] = constrainMotion(obj)
         b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
 
-        obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-        idx = idx + 6;
-    end
-
         if coder.target('MATLAB')
-        % Save off h function values (logging only — not needed in compiled mode)
+	    obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-        obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
+        end
+	idx = idx + 6;
     end
 
     % Add communication network constraints
@@ -164,7 +165,10 @@ function [obj] = constrainMotion(obj)
             end
         end
     end
+    
+    if coder.target('MATLAB')
 	obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
+    end
 
     % Double-integrator: transform QP from velocity to acceleration space.
     % Single-integrator constraint: A * v <= b

@miSim/initialize.m

@@ -109,8 +109,6 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Prepare performance data store (at t = 0, all have 0 performance)
         obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
-        % Prepare h function data store
-        obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
     end
 
     % Create initial partitioning
@@ -143,6 +141,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
         obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
 
+
         % Set up plots showing initialized state
         obj = obj.plot();
 

@miSim/initializeFromInits.m

@@ -0,0 +1,87 @@
+function obj = initializeFromInits(obj, initsPath)
+% INITIALIZEFROMINITS  Initialize miSim from a saved simInits matfile.
+%
+% Loads all simulation parameters and initial agent states written by
+% writeInits(), reconstructs domain, objective, agents, and obstacles, then
+% calls the standard obj.initialize() method.  Plots and video are disabled.
+%
+% Usage:
+%   sim = sim.initializeFromInits('sandbox/2025_01_01_12_00_00_miSimInits.mat');
+
+arguments (Input)
+    obj       (1, 1) {mustBeA(obj, 'miSim')};
+    initsPath (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+inits = load(initsPath);
+
+% ---- Build domain ------------------------------------------------------------
+dom = rectangularPrism;
+dom = dom.initialize([inits.domainMin; inits.domainMax], REGION_TYPE.DOMAIN, "Domain");
+
+% ---- Build sensing objective -------------------------------------------------
+dom.objective = sensingObjective;
+% reshape guards against MATLAB flattening the 1×2×2 singleton dimension on load
+objSigma = reshape(inits.objectiveSigma, [1 2 2]);
+objFcn = objectiveFunctionWrapper(inits.objectivePos, objSigma);
+dom.objective = dom.objective.initialize(objFcn, dom, ...
+    inits.discretizationStep, inits.protectedRange, inits.sensorPerformanceMinimum, ...
+    inits.objectivePos, objSigma);
+
+% ---- Build agents ------------------------------------------------------------
+numAgents = inits.numAgents;
+agentList = cell(numAgents, 1);
+for ii = 1:numAgents
+    pos = inits.pos(ii, :);
+
+    sensor = sigmoidSensor;
+    sensor = sensor.initialize(inits.alphaDist(ii), inits.betaDist(ii), ...
+                               inits.alphaTilt(ii), inits.betaTilt(ii));
+
+    geom = spherical;
+    geom = geom.initialize(pos, inits.collisionRadius(ii), REGION_TYPE.COLLISION, ...
+                           sprintf("UAV %d Collision", ii));
+    ag = agent;
+    ag = ag.initialize(pos, geom, sensor, inits.comRange(ii), inits.maxIter, ...
+                       inits.initialStepSize(ii), sprintf("UAV %d", ii));
+    agentList{ii} = ag;
+end
+
+% ---- Build obstacles ---------------------------------------------------------
+numObstacles = inits.numObstacles;
+obstacleList = cell(numObstacles, 1);
+if numObstacles > 0
+    for ii = 1:numObstacles
+        obs = rectangularPrism;
+        obs = obs.initialize([inits.obsMinCorners(ii, :); inits.obsMaxCorners(ii, :)], ...
+                             REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+        obstacleList{ii} = obs;
+    end
+end
+
+% ---- Optional backward-compat fields -----------------------------------------
+if isfield(inits, 'useDoubleIntegrator')
+    useDoubleIntegrator = logical(inits.useDoubleIntegrator);
+else
+    useDoubleIntegrator = false;
+end
+if isfield(inits, 'dampingCoeff')
+    dampingCoeff = inits.dampingCoeff;
+else
+    dampingCoeff = 2.0;
+end
+if isfield(inits, 'useFixedTopology')
+    useFixedTopology = logical(inits.useFixedTopology);
+else
+    useFixedTopology = false;
+end
+
+% ---- Initialize simulation (plots and video disabled) ------------------------
+obj = obj.initialize(dom, agentList, inits.barrierGain, inits.barrierExponent, ...
+                     inits.minAlt, inits.timestep, inits.maxIter, obstacleList, ...
+                     false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology);
+
+end

@miSim/miSim.m

@@ -54,7 +54,6 @@ classdef miSim
         partitionGraphIndex = 1;
 
         % CBF plotting
-        h; % h function values
         hf; % h function plotting figure
         caPlot; % objects for collision avoidance h function plot
         obsPlot; % objects for obstacle h function plot
@@ -70,8 +69,10 @@ classdef miSim
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology);
         [obj] = initializeFromCsv(obj, csvPath);
+        [obj] = initializeFromInits(obj, initsPath);
+        [obj] = plotFromSimHist(obj, initsPath, histPath);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
         [obj] = constrainMotion(obj);

@miSim/plotFromSimHist.m

@@ -0,0 +1,93 @@
+function obj = plotFromSimHist(obj, initsPath, histPath)
+% PLOTFROMSIMHIST  Reconstruct all three miSim plots from saved matfiles.
+%
+% Loads the simInits matfile to rebuild domain/obstacle/objective/agent
+% geometry, then loads the simHist matfile to restore the full time-history
+% arrays.  Produces the same three figures that a live run would generate:
+%   1. Sensor performance vs. time  (obj.fPerf)
+%   2. Barrier function values vs. time  (obj.hf)
+%   3. 3-D spatial figure with domain, obstacles, objective, agent trails,
+%      and final-timestep communications topology  (obj.f)
+%
+% Usage:
+%   sim = miSim;
+%   sim = sim.plotFromSimHist( ...
+%       'sandbox/2025_01_01_12_00_00_miSimHist.mat', ...
+%       'sandbox/2025_01_01_12_00_00_miSimInits.mat');
+
+arguments (Input)
+    obj       (1, 1) {mustBeA(obj, 'miSim')};
+    initsPath (1, 1) string;
+    histPath  (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+% ---- Reconstruct geometry from inits (plots disabled) --------------------
+obj = obj.initializeFromInits(initsPath);
+nAgents = size(obj.agents, 1);
+
+% ---- Load history data ---------------------------------------------------
+data = load(histPath);
+out  = data.out;
+
+nHistTimesteps = size(out.barriers, 2);
+nPosTimesteps  = size(out.agent(1).pos, 1);
+
+% ---- Populate barrier history --------------------------------------------
+% out.barriers may be narrower than the pre-allocated obj.barriers if the
+% run was shorter than maxIter; fill what we have and leave the rest NaN.
+obj.barriers(:, 1:nHistTimesteps) = out.barriers;
+
+% ---- Populate position history and advance agents to final positions -----
+for ii = 1:nAgents
+    agentPos = out.agent(ii).pos;           % (nPosTimesteps × 3)
+    nPts = size(agentPos, 1);
+    obj.posHist(ii, 1:nPts, :) = reshape(agentPos, [1, nPts, 3]);
+    obj.agents{ii}.pos = agentPos(end, :);  % show final position in spatial plot
+end
+
+% ---- Set final constraint topology ---------------------------------------
+obj.constraintAdjacencyMatrix = out.constraintAdjacency(:, :, end);
+
+% ---- Recompute partitioning at final agent positions ---------------------
+obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+
+% ---- Enable plotting and produce spatial + barrier figures ---------------
+obj.makePlots = true;
+obj = obj.plot();
+
+% ---- Performance figure (built directly — live machinery is incremental) -
+nPerfTimesteps = numel(out.perf);
+times = (0:nPerfTimesteps - 1) * obj.timestep;
+normFactor = 1 / max(out.perf);
+
+obj.fPerf = figure;
+ax = axes(obj.fPerf);
+hold(ax, "on");
+title(ax,  "Sensor Performance");
+xlabel(ax, "Time (s)");
+ylabel(ax, "Sensor Performance");
+grid(ax, "on");
+
+legendStrings = strings(nAgents + 1, 1);
+legendStrings(1) = "Total";
+plot(ax, times, out.perf * normFactor, "LineWidth", 1.5);
+for ii = 1:nAgents
+    agentPerf  = out.agent(ii).perf;
+    agentTimes = times(1:numel(agentPerf));
+    plot(ax, agentTimes, agentPerf * normFactor);
+    if isfield(out.agent(ii), 'label')
+        legendStrings(ii + 1) = string(out.agent(ii).label);
+    else
+        legendStrings(ii + 1) = sprintf("Agent %d", ii);
+    end
+end
+legend(ax, legendStrings, "Location", "northwest");
+hold(ax, "off");
+
+% Bring spatial figure to the front
+figure(obj.f);
+
+end

@miSim/plotH.m

@@ -6,6 +6,10 @@ function obj = plotH(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
+    nCA  = size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2;
+    nObs = size(obj.agents, 1) * size(obj.obstacles, 1);
+    nDom = size(obj.agents, 1) * 6;
+
     obj.hf = figure;
     tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
 
@@ -15,7 +19,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
+    obj.caPlot = plot(obj.barriers(1:nCA, :)');
     legendStrings = [];
     for ii = 2:size(obj.agents, 1)
         for jj = 1:(ii - 1)
@@ -31,7 +35,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
+    obj.obsPlot = plot(obj.barriers((nCA + 1):(nCA + nObs), :)');
     legendStrings = [];
     for ii = 1:size(obj.obstacles, 1)
         for jj = 1:size(obj.agents, 1)
@@ -47,8 +51,13 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
+    obj.domPlot = plot(obj.barriers((nCA + nObs + 1):(nCA + nObs + nDom), :)');
-    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
+    domLabels = ["X Min", "X Max", "Y Min", "Y Max", "Z Min", "Z Max"];
+    legendStrings = strings(nDom, 1);
+    for ii = 1:size(obj.agents, 1)
+        legendStrings((ii - 1) * 6 + (1:6)) = sprintf("A%d ", ii) + domLabels;
+    end
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));

@miSim/run.m

@@ -11,6 +11,10 @@ function [obj] = run(obj)
         if obj.makeVideo
             v = obj.setupVideoWriter();
             v.open();
+            
+            % Write initialization state frame in to video
+            I = getframe(obj.f);
+            v.writeVideo(I);
         end
     end
 

@miSim/updatePlots.m

@@ -61,13 +61,15 @@ function [obj] = updatePlots(obj)
     end
 
     % Update h function plots
-    for ii = 1:size(obj.caPlot, 1)
+    nCA  = size(obj.caPlot, 1);
-        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
+    nObs = size(obj.obsPlot, 1);
+    for ii = 1:nCA
+        obj.caPlot(ii).YData(obj.timestepIndex) = obj.barriers(ii, obj.timestepIndex);
     end
-    for ii = 1:size(obj.obsPlot, 1)
+    for ii = 1:nObs
-        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
+        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + ii, obj.timestepIndex);
     end
     for ii = 1:size(obj.domPlot, 1)
-        obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
+        obj.domPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + nObs + ii, obj.timestepIndex);
     end
 end

@miSim/writeInits.m

@@ -5,29 +5,35 @@ function writeInits(obj)
     arguments (Output)
     end
 
+    % User-supplied obstacles only: initialize() appends a floor obstacle at
+    % the end when minAlt > 0, so exclude it here to avoid double-counting on
+    % reconstruction (initializeFromInits re-adds the floor via minAlt).
+    numInputObs = size(obj.obstacles, 1) - (obj.minAlt > 0);
+    userObstacles = obj.obstacles(1:numInputObs);
+
     % Collect agent parameters
     collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
     alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
     betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
     alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
-    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents);
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
-    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
+    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, userObstacles, 'UniformOutput', false));
-    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
+    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, userObstacles, 'UniformOutput', false));
-
 
     % Combine with simulation parameters
-    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
+    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter + 1, "minAlt", obj.minAlt, ...
                     "discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ...
                     "sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ...
-                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
+                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ...
                     "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
-                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
+                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
                     "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
                     "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
+                    "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...
                     "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
                     "objectiveIntegral", sum(obj.domain.objective.values(:)));
 

@sensingObjective/initialize.m

@@ -47,5 +47,5 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     end
     obj.objectiveSigma = objectiveSigma;
 
-    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective");
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -16,11 +16,11 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     end
 
     % Set random distribution parameters
-    sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
+    sig = reshape([2 + rand * 2, 1; 1, 2 + rand * 2], [1 2 2]);
 
     % Set up random bivariate normal distribution function
     objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
-    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange, 1e-6, mu(1:2), sig);
 end

aerpaw/client/uav_runner.py

@@ -164,7 +164,7 @@ class UAVRunner(BasicRunner):
 
         # Retry connection up to 10 times (~30 seconds total)
         reader, writer = None, None
-        for attempt in range(10):
+        for attempt in range(100):
             try:
                 reader, writer = await asyncio.wait_for(
                     asyncio.open_connection(self.server_ip, self.server_port),

aerpaw/config/client1.yaml

@@ -28,11 +28,11 @@ environments:
       port: 5000
 
   testbed:
-    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects to us (UDP)
     mavlink:
       ip: "192.168.32.26"
       port: 14550
-    # Controller runs on host machine (192.168.109.1 from E-VM perspective)
+    # Controller runs on host machine (192.168.X.1, generally)
     controller:
-      ip: "192.168.109.1"
+      ip: "192.168.112.1"
       port: 5000

aerpaw/config/client2.yaml

@@ -28,11 +28,11 @@ environments:
       port: 5000
 
   testbed:
-    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects to us (UDP)
     mavlink:
       ip: "192.168.32.26"
       port: 14550
-    # Controller runs on host machine (192.168.109.1 from E-VM perspective)
+    # Controller runs on host machine (192.168.X.1, generally)
     controller:
-      ip: "192.168.109.1"
+      ip: "192.168.112.1"
       port: 5000

aerpaw/config/scenario.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
+1, 50, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "10.0, 10.0, 50.0, 40.0, 15.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_columns.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_lock.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_two_around_wall.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 100, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 80.0", "0.25, 0.25", "8.0, 8.0", "0.1, 0.1", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "66.6, 66.6", "55, 35, 35, 55", 0.15, "15.0, 15.0, 50.0, 40.0, 15.0, 50.0", 1, "0.0, 35.0, 0.0", "50, 40.0, 60", 1, 2.0, 1

aerpaw/config/scenario_walls.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1

aerpaw/controller.coderprj

@@ -138,6 +138,41 @@
                   <Size type="coderapp.internal.codertype.Dimension"/>
                   <Size type="coderapp.internal.codertype.Dimension"/>
                 </Types>
+                <Types id="28" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="29" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="30" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="31" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="32" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="33" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="34" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
               </Types>
             </coderapp.internal.interface.project.Interface>
           </MF0>
@@ -379,719 +414,773 @@
                 </Artifacts>
                 <Artifacts id="40" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="41" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="42" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="43" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="44" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="45" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="46" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="47" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="48" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="49" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="50" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="51" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="52" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="53" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="54" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="55" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="56" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="57" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="58" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="59" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="60" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="61" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="62" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="63" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="64" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="65" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="66" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="67" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="68" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="69" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="70" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="71" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="72" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="73" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="74" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="75" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="76" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="77" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="78" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="79" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="80" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="81" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="82" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="83" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="84" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="85" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="86" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="87" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="88" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="89" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="90" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="91" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="92" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="93" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="94" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="95" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="96" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="97" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="98" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="99" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="100" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="101" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="102" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="103" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="104" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="105" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="106" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="107" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="108" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="109" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="110" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="111" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="112" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="113" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="114" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="115" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="116" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="117" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="118" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="119" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="120" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="121" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="122" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="123" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="124" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="125" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="126" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="127" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="128" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="129" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="130" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="131" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="132" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="133" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="134" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="135" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="136" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="137" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="138" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="139" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="140" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="141" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="142" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="143" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="144" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="145" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="146" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/triu.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="147" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/triu.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="148" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="149" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="150" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="151" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="152" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="153" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="154" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="155" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="156" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="157" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/matlab/R2025a/extern/include/tmwtypes.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="158" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="159" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="160" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="161" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="162" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="163" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="164" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="165" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="166" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/matlab/R2025a/extern/include/tmwtypes.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="167" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="168" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
                     <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h</Path>
                   </File>
@@ -1099,7 +1188,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-03-11T17:11:03</Timestamp>
+                <Timestamp>2026-04-07T22:43:01</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

aerpaw/controller.m

@@ -7,34 +7,47 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-MAX_TARGETS = MAX_CLIENTS;
+% Three waypoints per UAV: one axis-aligned move per dimension (taxicab flyout/flyback)
+MAX_TARGETS = MAX_CLIENTS * 3;
+
+% Taxicab flyout/flyback only supports exactly 2 UAVs
+if numClients ~= int32(2)
+    error('Taxicab flyout/flyback requires exactly 2 UAVs');
+end
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
 numWaypoints = int32(0);
 totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
 
-% Load initial UAV positions from scenario CSV
+% Experiment start positions from scenario CSV (N x 3)
+scenarioPositions = zeros(MAX_CLIENTS, 3);
+
+% Load experiment start positions from scenario CSV
 if coder.target('MATLAB')
     disp('Loading initial positions from scenario.csv (simulation)...');
     tmpSim = miSim;
     sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
     flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
-    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
+    posMatrix = reshape(flatPos, 3, [])';   % N×3
     totalLoaded = int32(size(posMatrix, 1));
+    scenarioPositions(1:totalLoaded, :) = posMatrix;
+    % MATLAB path: send directly to scenario positions in one waypoint
     targets(1:totalLoaded, :) = posMatrix;
     numWaypoints = int32(1);
     disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
 else
     coder.cinclude('controller_impl.h');
     filename = ['config/scenario.csv', char(0)];
+    % Load into targets temporarily; copy to scenarioPositions below
     totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
                 coder.ref(targets), int32(MAX_TARGETS));
-    numWaypoints = totalLoaded / int32(numClients);
+    scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    numWaypoints = int32(3);
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 45;
+NUM_SCENARIO_PARAMS = 48;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
@@ -68,6 +81,46 @@ for i = 1:numClients
     end
 end
 
+% Query takeoff-pad GPS positions before sending any waypoints.
+% These are also the flyback targets so each UAV lands where it took off.
+initialPositions = zeros(MAX_CLIENTS, 3);
+if ~coder.target('MATLAB')
+    coder.ceval('sendRequestPositions', int32(numClients));
+    coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS));
+else
+    % Simulation: treat scenario positions as the takeoff locations
+    initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
+end
+
+% ---- Build taxicab flyout waypoints ------------------------------------------
+% Determine which UAV has the higher final altitude; it moves Z first so it
+% clears vertical separation before the lower UAV converges on the same X/Y.
+%   Higher UAV order: Z → Y → X
+%   Lower  UAV order: X → Y → Z
+if ~coder.target('MATLAB')
+    if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
+        higherIdx = int32(1);
+        lowerIdx  = int32(2);
+    else
+        higherIdx = int32(2);
+        lowerIdx  = int32(1);
+    end
+
+    hBase = double(higherIdx - 1) * double(numWaypoints);
+    lBase = double(lowerIdx  - 1) * double(numWaypoints);
+
+    % Higher UAV: Z first
+    targets(hBase + 1, :) = [initialPositions(higherIdx,1), initialPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 2, :) = [initialPositions(higherIdx,1), scenarioPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 3, :) =  scenarioPositions(higherIdx, :);
+
+    % 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
+% ------------------------------------------------------------------------------
+
 % Waypoint loop: send each waypoint to all clients, wait for all to arrive
 for w = 1:numWaypoints
     % Send TARGET for waypoint w to each client
@@ -78,7 +131,7 @@ for w = 1:numWaypoints
         target = targets(targetIdx, :);
 
         if coder.target('MATLAB')
-            disp([datestr(now, 'HH:MM:SS'), ' Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
+            disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
                   num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
         else
             coder.ceval('sendTarget', int32(i), coder.ref(target));
@@ -103,8 +156,13 @@ for w = 1:numWaypoints
 end
 
 % ---- Phase 2: miSim guidance loop ----------------------------------------
-% Guidance parameters (adjust here and recompile as needed)
+% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2))
-MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+% so the controller and the agent step-decay schedule stay in sync.
+if coder.target('MATLAB')
+    MAX_GUIDANCE_STEPS = int32(sc.maxIter);
+else
+    MAX_GUIDANCE_STEPS = int32(scenarioParams(2));
+end
 
 % Enter guidance mode on all clients
 if ~coder.target('MATLAB')
@@ -117,14 +175,18 @@ if ~coder.target('MATLAB')
     coder.ceval('sendRequestPositions', int32(numClients));
     coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
 else
-    % Simulation: seed positions from CSV waypoints so agents don't start at origin
+    % Simulation: seed positions from scenario positions so agents don't start at origin
-    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
 end
 guidance_step(positions(1:numClients, :), true, ...
               scenarioParams, obstacleMin, obstacleMax, numObstacles);
 
 % Main guidance loop (event-triggered)
 for step = 1:MAX_GUIDANCE_STEPS
+    if ~coder.target('MATLAB')
+        coder.ceval('setGuidanceStep', int32(step), int32(MAX_GUIDANCE_STEPS));
+    end
+
     % Run one guidance step: feed current GPS positions in, get targets out
     nextPositions = guidance_step(positions(1:numClients, :), false, ...
                                   scenarioParams, obstacleMin, obstacleMax, numObstacles);
@@ -135,7 +197,7 @@ for step = 1:MAX_GUIDANCE_STEPS
         if ~coder.target('MATLAB')
             coder.ceval('sendTarget', int32(i), coder.ref(target));
         else
-            disp([datestr(now, 'HH:MM:SS'), ' [guidance] target UAV ', num2str(i), ': ', num2str(target)]);
+            disp(['[step ', num2str(step), '] target UAV ', num2str(i), ': ', num2str(target)]);
         end
     end
 
@@ -164,6 +226,51 @@ if ~coder.target('MATLAB')
     % last guidance navigation and is back in sequential (ACK/READY) mode.
     coder.ceval('waitForAllMessageType', int32(numClients), ...
                 int32(MESSAGE_TYPE.ACK));
+    % Reset step counter so post-guidance logging carries no step prefix.
+    coder.ceval('setGuidanceStep', int32(0), int32(MAX_GUIDANCE_STEPS));
+end
+% --------------------------------------------------------------------------
+
+% ---- Taxicab flyback: return each UAV to its takeoff-pad position ---------
+% The UAV that ended guidance at the higher altitude moves Z last (X → Y → Z)
+% so it stays high while the lower UAV descends first, maintaining separation
+% as both converge back on their respective home X/Y positions.
+NUM_RETURN_WP = int32(3);
+returnTargets = zeros(MAX_TARGETS, 3);
+
+if ~coder.target('MATLAB')
+    if positions(1, 3) >= positions(2, 3)
+        higherRetIdx = int32(1);
+        lowerRetIdx  = int32(2);
+    else
+        higherRetIdx = int32(2);
+        lowerRetIdx  = int32(1);
+    end
+
+    hRetBase = double(higherRetIdx - 1) * double(NUM_RETURN_WP);
+    lRetBase = double(lowerRetIdx  - 1) * double(NUM_RETURN_WP);
+
+    % Higher post-guidance UAV: X → Y → Z (descend last)
+    returnTargets(hRetBase + 1, :) = [initialPositions(higherRetIdx,1), positions(higherRetIdx,2),        positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 2, :) = [initialPositions(higherRetIdx,1), initialPositions(higherRetIdx,2), positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 3, :) =  initialPositions(higherRetIdx, :);
+
+    % Lower post-guidance UAV: Z → Y → X (descend first)
+    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
+            retIdx = double(i - 1) * double(NUM_RETURN_WP) + w;
+            retTarget = returnTargets(retIdx, :);
+            coder.ceval('sendTarget', int32(i), coder.ref(retTarget));
+        end
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
+    end
+else
+    disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.');
 end
 % --------------------------------------------------------------------------
 

aerpaw/guidance_step.m

@@ -29,6 +29,9 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                                               39-40  objectivePos
 %                                               41-44  objectiveVar (2x2, col-major)
 %                                               45     sensorPerformanceMinimum
+%                                               46     useDoubleIntegrator
+%                                               47     dampingCoeff
+%                                               48     useFixedTopology
 %   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
 %   obstacleMax       (MAX_OBSTACLES × 3) double
 %   numObstacles      (1,1) int32                 actual obstacle count
@@ -94,6 +97,9 @@ if isInit
         OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
         OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
         SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(46));
+        DAMPING_COEFF               = scenarioParams(47);
+        USE_FIXED_TOPOLOGY          = logical(scenarioParams(48));
 
         % --- Build domain geometry ---
         dom = rectangularPrism;
@@ -146,7 +152,8 @@ if isInit
         % --- Initialise simulation (plots and video disabled) ---
         sim = miSim;
         sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
-                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
+                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
+                             USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
     end
 
     % On the init call return current positions unchanged
@@ -176,7 +183,9 @@ else
     sim.timestepIndex = sim.timestepIndex + 1;
 
     % 3. Update communications topology (Lesser Neighbour Assignment)
+    if ~sim.useFixedTopology
         sim = sim.lesserNeighbor();
+    end
 
     % 4. Compute Voronoi partitioning
     sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
@@ -184,7 +193,8 @@ else
     % 5. Unconstrained gradient-ascent step for each agent
     for ii = 1:size(sim.agents, 1)
         sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, sim.agents);
+                                             sim.timestepIndex, ii, sim.agents, ...
+                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

aerpaw/impl/controller_impl.cpp

@@ -16,6 +16,44 @@
 
 static int serverSocket = -1;
 static std::vector<int> clientSockets;
+static int guidanceStep = 0;
+static int guidanceTotalSteps = 0;
+static struct timespec lastStepTime = {0, 0};
+
+// During guidance returns "(%d/%d) "; outside guidance returns "HH:MM:SS ".
+static std::string logPrefix() {
+    if (guidanceStep > 0) {
+        char buf[32];
+        snprintf(buf, sizeof(buf), "(%d/%d) ", guidanceStep, guidanceTotalSteps);
+        return std::string(buf);
+    }
+    time_t now = time(nullptr);
+    struct tm* lt = localtime(&now);
+    char ts[16];
+    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
+    return std::string(ts) + " ";
+}
+
+void setGuidanceStep(int step, int totalSteps) {
+    struct timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+
+    // From step 2 onward, elapsed = setGuidanceStep(N-1) → setGuidanceStep(N),
+    // which spans the full prior iteration: guidance computation + target send
+    // + flight + position request/receive.
+    if (step > 1 && lastStepTime.tv_sec != 0) {
+        double elapsed = (now.tv_sec - lastStepTime.tv_sec)
+                       + (now.tv_nsec - lastStepTime.tv_nsec) * 1e-9;
+        guidanceStep = step;
+        guidanceTotalSteps = totalSteps;
+        std::cout << logPrefix() << "Iteration duration: " << elapsed << " s\n";
+    } else {
+        guidanceStep = step;
+        guidanceTotalSteps = totalSteps;
+    }
+
+    lastStepTime = now;
+}
 
 void initSockets() {}
 void cleanupSockets() {}
@@ -186,7 +224,10 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
 //   35-37: domainMax  (east, north, up)
 //   38-39: objectivePos (east, north)
 //   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
-//   44   : sensorPerformanceMinimum
+//   44   : sensorPerformanceMinimum  (CSV column 18)
+//   45   : useDoubleIntegrator       (CSV column 23; 0=single-integrator, 1=double-integrator)
+//   46   : dampingCoeff              (CSV column 24)
+//   47   : useFixedTopology          (CSV column 25; 0=dynamic lesser-neighbor, 1=fixed)
 // Returns 1 on success, 0 on failure.
 int loadScenario(const char* filename, double* params) {
     char line[4096];
@@ -198,8 +239,8 @@ int loadScenario(const char* filename, double* params) {
 
     char* fields[32];
     int nf = splitCSVRow(copy, fields, 32);
-    if (nf < 19) {
+    if (nf < 26) {
-        fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf);
+        fprintf(stderr, "loadScenario: expected >=26 columns, got %d\n", nf);
         return 0;
     }
 
@@ -290,6 +331,24 @@ int loadScenario(const char* filename, double* params) {
         params[44] = atof(trimField(tmp));
     }
 
+    // useDoubleIntegrator: column 23
+    {
+        char tmp[64]; strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[45] = atof(trimField(tmp));
+    }
+
+    // dampingCoeff: column 24
+    {
+        char tmp[64]; strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[46] = atof(trimField(tmp));
+    }
+
+    // useFixedTopology: column 25
+    {
+        char tmp[64]; strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[47] = atof(trimField(tmp));
+    }
+
     printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
            params[32], params[33], params[34], params[35], params[36], params[37]);
     return 1;
@@ -430,18 +489,22 @@ static const char* messageTypeName(uint8_t msgType) {
     }
 }
 
-// Send a single-byte message type to a client
+// Send a single-byte message type to a client (no logging)
-int sendMessageType(int clientId, int msgType) {
+static int sendMessageTypeRaw(int clientId, int msgType) {
     if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-
     uint8_t msg = (uint8_t)msgType;
     ssize_t sent = send(clientSockets[clientId - 1], &msg, 1, 0);
     if (sent != 1) {
         std::cerr << "Send failed for client " << clientId << "\n";
         return 0;
     }
+    return 1;
+}
 
-    std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
+// Send a single-byte message type to a client
+int sendMessageType(int clientId, int msgType) {
+    if (!sendMessageTypeRaw(clientId, msgType)) return 0;
+    std::cout << logPrefix() << "Sent " << messageTypeName((uint8_t)msgType) << " to client " << clientId << "\n";
     return 1;
 }
 
@@ -460,13 +523,7 @@ int sendTarget(int clientId, const double* coords) {
         return 0;
     }
 
-    // Timestamp
+    std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
-    time_t now = time(nullptr);
-    struct tm* lt = localtime(&now);
-    char ts[16];
-    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
-
-    std::cout << ts << " Sent TARGET to client " << clientId << ": "
               << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     return 1;
 }
@@ -514,31 +571,36 @@ int waitForAllMessageType(int numClients, int expectedType) {
                     return 0;
                 }
 
-                std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
-
                 if (msgType == expected) {
                     completed[i] = true;
                     completedCount++;
+                } else {
+                    std::cerr << logPrefix() << "Unexpected " << messageTypeName(msgType)
+                              << " from client " << (i + 1)
+                              << " (expected " << messageTypeName(expected) << ")\n";
                 }
             }
         }
     }
 
+    std::cout << logPrefix() << "Received " << messageTypeName(expected) << " from all clients\n";
     return 1;
 }
 
 // Broadcast GUIDANCE_TOGGLE to all clients
 void sendGuidanceToggle(int numClients) {
     for (int i = 1; i <= numClients; i++) {
-        sendMessageType(i, 6);  // GUIDANCE_TOGGLE = 6
+        sendMessageTypeRaw(i, 6);  // GUIDANCE_TOGGLE = 6
     }
+    std::cout << logPrefix() << "Sent GUIDANCE_TOGGLE to clients\n";
 }
 
 // Send REQUEST_POSITION to all clients
 int sendRequestPositions(int numClients) {
     for (int i = 1; i <= numClients; i++) {
-        if (!sendMessageType(i, 7)) return 0;  // REQUEST_POSITION = 7
+        if (!sendMessageTypeRaw(i, 7)) return 0;  // REQUEST_POSITION = 7
     }
+    std::cout << logPrefix() << "Sent REQUEST_POSITION to clients\n";
     return 1;
 }
 
@@ -573,7 +635,7 @@ int recvPositions(int numClients, double* positions, int maxClients) {
         positions[i + 1 * maxClients] = coords[1];  // north (y)
         positions[i + 2 * maxClients] = coords[2];  // up    (z)
 
-        std::cout << "Position from client " << (i + 1) << ": "
+        std::cout << logPrefix() << "Position from client " << (i + 1) << ": "
                   << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     }
     return 1;

aerpaw/impl/controller_impl.h

@@ -30,7 +30,10 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   38-39 objectivePos
 //   40-43 objectiveVar (2x2 col-major)
 //   44    sensorPerformanceMinimum
-#define NUM_SCENARIO_PARAMS 45
+//   45    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
+//   46    dampingCoeff
+//   47    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
+#define NUM_SCENARIO_PARAMS 48
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8
@@ -59,6 +62,7 @@ int sendTarget(int clientId, const double* coords);
 int waitForAllMessageType(int numClients, int expectedType);
 
 // Guidance loop operations
+void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration
 void sendGuidanceToggle(int numClients);
 int  sendRequestPositions(int numClients);
 int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3

aerpaw/radio/updateScripts.sh

@@ -13,3 +13,4 @@ cp ../scripts/startVehicle.sh /root/Profiles/ProfileScripts/Vehicle/.
 
 echo "REMEMBER! Manually edit startexperiment.sh to point to the correct client.yaml"
 echo "REMEMBER! Manually copy startexperiment_controller.sh to startexperiment.sh on the fixed node"
+echo "REMEMBER! Manually copy startVehicle_controller.sh to ~/Profiles/ProfileScripts/Vehicle/startVehicle.sh on the fixed node"

aerpaw/results/plotGpsLogs.m

@@ -1,7 +1,8 @@
-function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
+function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
     arguments (Input)
         logDirs (1, 1) string;
         seaToGroundLevel (1, 1) double = 110; % measured approximately from USGS national map viewer for the AERPAW test field
+        plotWholeFlight (1, 1) logical = false;
     end
     arguments (Output)
         f (1, 1) matlab.ui.Figure;
@@ -28,6 +29,7 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
     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)
@@ -48,8 +50,10 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
         stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last");
     
         % % Plot whole flight, including setup/cleanup
-        % startIdx = 1;
+        if plotWholeFlight
-        % stopIdx = length(verticalSpeed);
+            startIdx = 1;
+            stopIdx = length(verticalSpeed);
+        end
 
         % Convert LLA trajectory data to ENU for external analysis
         % NaN out entries outside the algorithm flight range so they don't plot
@@ -58,6 +62,27 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
         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);
     end

aerpaw/results/readControllerLogs.m

@@ -0,0 +1,32 @@
+function T2 = readControllerLogs(filepath)
+    arguments (Input)
+        filepath (1, 1) string;
+    end
+    arguments (Output)
+        T2 table;
+    end
+    assert(isfile(filepath), "File not found at %s", filepath);
+
+    T = readtable(filepath, 'VariableNamingRule', 'preserve');
+    s = split(T.(T.Properties.VariableNames{1}), ']');
+    s2 = strip(s(startsWith(s(:, 2), " ("), 1), 'left', '[');
+    d = datetime(s2, "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS")';
+    it = s(startsWith(s(:, 2), " ("), 2);
+    it = str2double(strip(strip(it, 'left'), 'left', '('));
+    T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", T.Var7));
+    T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = [];
+    msg = T.(T.Properties.VariableNames{2});
+    msg = msg(startsWith(s(:, 2), " ("), :);
+    s3 = split(msg, ') ');
+    s3 = s3(:, 2);
+    msg = append(s3, T.Var3(startsWith(s(:, 2), " (")));
+    T2 = table(it, d', msg, 'VariableNames', ["iteration", "timestamp", "message"]);
+    % T.Var1 = datetime(strip(strip(append(T.Var1, " ", T.Var2), 'left', '['), 'right', ']'), "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS");
+    % T.Var2 = [];
+    % T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", string(T.Var7), " ", T.Var8, " ", T.Var9));
+    % T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = []; T.Var8 = []; T.Var9 = [];
+    % T.Properties.VariableNames{1} = 'timestamp';
+    % T.Properties.VariableNames{2} = 'message';
+
+    % T(ismissing(T.message), :) = [];
+end

aerpaw/results/readRadioLogs.m

@@ -2,7 +2,6 @@ function R = readRadioLogs(logPath)
     arguments (Input)
         logPath (1, 1) string {isfolder(logPath)};
     end
-
     arguments (Output)
         R (:, 8) table;
     end
@@ -31,14 +30,26 @@ function R = readRadioLogs(logPath)
         end
 
         fid = fopen(filepath, 'r');
-        % Skip 3 lines: 2 junk (tail errors) + 1 header (tx_uav_id,value)
+        % Skip header lines: some files have 2 tail-error lines + 1 column
-        for k = 1:3
+        % header ("tx_uav_id,value"), others start with data immediately.
-            fgetl(fid);
+        % Read until a line that looks like a data record, then rewind to it.
+        dataPattern = '^\[\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}\.\d+\] [-\d]';
+        linePos = ftell(fid);
+        while true
+            line = fgetl(fid);
+            if ~ischar(line)
+                break;  % EOF
+            end
+            if ~isempty(regexp(line, dataPattern, 'once'))
+                fseek(fid, linePos, 'bof');  % rewind to start of this line
+                break;
+            end
+            linePos = ftell(fid);
         end
         data = textscan(fid, '[%26c] %d,%f');
         fclose(fid);
 
-        ts = datetime(data{1}, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
+        ts = datetime(cellstr(data{1}), 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
         txId = int32(data{2});
         val = data{3};
 

aerpaw/results/resultsAnalysis.m

@@ -1,9 +1,30 @@
 %% Plot AERPAW logs (trajectory, radio)
 resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
 
+% Measure intervals between issuing commands from the controller 
+% (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
+r = dir(resultsPath);
+controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
+controllerPath = dir(controllerPath);
+controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
+controller = readControllerLogs(controllerPath);
+rpIdx = startsWith(controller.message, "Iteration duration: ");
+s = split(controller.message(rpIdx), "Iteration duration: ");
+s = split(s(:, 2), ' s');
+s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
+s.Format = "mm:ss.SSS";
+fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
+fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
+fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
+fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
+if seconds(min(s)) < 4 
+    warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
+end
+
 % Plot GPS logged data and scenario information (domain, objective, obstacles)
 seaToGroundLevel = 110; % measured approximately from USGS national map viewer
-[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel);
+plotWholeFlight = true; % do not attempt to automatically trim initial and final positioning and landing from flight plot (buggy)
+[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel, true);
 
 % Plot radio statistics
 [fRadio, R] = plotRadioLogs(resultsPath);
@@ -21,7 +42,7 @@ 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);
+domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [1, 2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
 
 agents = cell(size(params.initialPositions, 2) / 3, 1);
 for ii = 1:size(agents, 1)

plots_1_2.m

@@ -1,174 +0,0 @@
-clear;
-
-%% Load data
-dataPath = fullfile('.', 'sandbox', 'plot1');
-dataFiles = dir(dataPath);
-dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
-simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
-simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
-assert(length(simHists) == length(simInits), "input data availability mismatch");
-
-%% Aggregate run data
-nRuns = length(simHists);
-Cfinal = NaN(nRuns, 1);
-nAgents = NaN(nRuns, 1);
-doubleIntegrator = NaN(nRuns, 1);
-numObjective = NaN(nRuns, 1);
-commsRadius = NaN(nRuns, 1);
-collisionRadius = NaN(nRuns, 1);
-maxAgents = 6;
-alphaDist = NaN(maxAgents, nRuns);
-positions = cell(maxAgents, nRuns);
-adjacency = cell(nRuns, 1);
-
-for ii = 1:nRuns
-    initName = strrep(simInits(ii).name, "_miSimInits.mat", "");
-    histName = strrep(simHists(ii).name, "_miSimHist.mat", "");
-    assert(initName == histName);
-
-    init = load(fullfile(simInits(ii).folder, simInits(ii).name));
-    hist = load(fullfile(simHists(ii).folder, simHists(ii).name));
-
-    Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral;
-    nAgents(ii) = init.numAgents;
-    doubleIntegrator(ii) = init.useDoubleIntegrator;
-    numObjective(ii) = size(init.objectivePos, 1);
-    commsRadius(ii) = unique(init.comRange);
-    collisionRadius(ii) = unique(init.collisionRadius);
-
-    adjacency{ii} = hist.out.constraintAdjacency(:, :, 1);
-    for jj = 1:nAgents(ii)
-        alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist;
-        positions{jj, ii} = hist.out.agent(jj).pos;
-        assert(hist.out.agent(jj).commsRadius == commsRadius(ii));
-        assert(hist.out.agent(jj).collisionRadius == collisionRadius(ii));
-    end
-end
-
-commsRadius = unique(commsRadius); assert(isscalar(commsRadius));
-collisionRadius = unique(collisionRadius); assert(isscalar(collisionRadius));
-sensorTypes = flip(unique(alphaDist(1, :)));
-nValues = sort(unique(nAgents));
-nGroups = length(nValues);
-
-%% Build config labels
-baseConfig = strings(nRuns, 1);
-for ii = 1:nRuns
-    s = "";
-    if numObjective(ii) == 1
-        s = s + "A";
-    elseif numObjective(ii) == 2
-        s = s + "B";
-    end
-    if alphaDist(1, ii) == sensorTypes(1)
-        s = s + "_I";
-    elseif alphaDist(1, ii) == sensorTypes(2)
-        s = s + "_II";
-    end
-    if ~doubleIntegrator(ii)
-        s = s + "_alpha";
-    else
-        s = s + "_beta";
-    end
-    baseConfig(ii) = s;
-end
-configOrder = unique(baseConfig(nAgents == nValues(1)), 'stable');
-nConfigs = length(configOrder);
-configLabels = ["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"];
-
-%% Plot 1: Final normalized coverage
-close all;
-f1 = figure;
-x1 = axes;
-
-C_mean = NaN(nGroups, nConfigs);
-C_var = NaN(nGroups, nConfigs);
-for ii = 1:nGroups
-    for jj = 1:nConfigs
-        mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
-        C_mean(ii, jj) = mean(Cfinal(mask));
-        C_var(ii, jj) = var(Cfinal(mask));
-    end
-end
-
-bar(x1, C_mean);
-set(x1, 'XTickLabel', string(nValues));
-xlabel(x1, "Number of agents");
-ylabel(x1, "Final coverage (normalized)");
-title(x1, "Final performance of parameterizations");
-legend(x1, configLabels, "Interpreter", "latex", "Location", "northwest");
-grid(x1, "on");
-ylim(x1, [0, 1/2]);
-
-savefig(f1, "plot1.fig");
-exportgraphics(f1, "plot1.png");
-
-%% Plot 2: Pairwise agent distances
-f2 = figure;
-x2 = axes;
-
-% Compute pairwise distances only for connected agents (static topology)
-maxPairs = nchoosek(maxAgents, 2);
-pairDist = cell(maxPairs, nRuns);
-for ii = 1:nRuns
-    A = adjacency{ii};
-    pp = 0;
-    for jj = 1:nAgents(ii)-1
-        for kk = jj+1:nAgents(ii)
-            pp = pp + 1;
-            if A(jj, kk)
-                pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2);
-            end
-        end
-    end
-end
-
-% Per-run statistics across all pairs and timesteps
-meanPairDist = NaN(nRuns, 1);
-minPairDist = NaN(nRuns, 1);
-maxPairDist = NaN(nRuns, 1);
-for ii = 1:nRuns
-    nPairs = nchoosek(nAgents(ii), 2);
-    D = vertcat(pairDist{1:nPairs, ii});
-    meanPairDist(ii) = mean(D, "omitmissing");
-    minPairDist(ii) = min(D);
-    maxPairDist(ii) = max(D);
-end
-
-% Aggregate across trials per (n, config) group
-meanD = NaN(nGroups, nConfigs);
-minD = NaN(nGroups, nConfigs);
-maxD = NaN(nGroups, nConfigs);
-for ii = 1:nGroups
-    for jj = 1:nConfigs
-        mask = (nAgents == nValues(ii)) & (baseConfig == configOrder(jj));
-        meanD(ii, jj) = mean(meanPairDist(mask));
-        minD(ii, jj) = min(minPairDist(mask));
-        maxD(ii, jj) = max(maxPairDist(mask));
-    end
-end
-
-% Plot whiskers (min to max) with mean markers
-barWidth = 0.8;
-groupWidth = barWidth / nConfigs;
-hold(x2, 'on');
-for jj = 1:nConfigs
-    xPos = (1:nGroups) + (jj - (nConfigs + 1) / 2) * groupWidth;
-    errorbar(x2, xPos, meanD(:, jj), meanD(:, jj) - minD(:, jj), maxD(:, jj) - meanD(:, jj), ...
-        'o', 'LineWidth', 1.5, 'MarkerSize', 6, 'CapSize', 10);
-end
-hold(x2, 'off');
-set(x2, 'XTick', 1:nGroups, 'XTickLabel', string(nValues));
-xlabel(x2, "Number of agents");
-ylabel(x2, "Pairwise distance");
-title(x2, "Pairwise Agent Distances (min/mean/max)");
-legend(x2, configLabels, "Interpreter", "latex");
-grid(x2, "on");
-yline(x2, collisionRadius, 'r--', "Label", "Collision Radius", ...
-    "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
-yline(x2, commsRadius, 'r--', "Label", "Communications Radius", ...
-    "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
-ylim(x2, [0, commsRadius + 5]);
-
-savefig(f2, "plot2.fig");
-exportgraphics(f2, "plot2.png");

plots_3_4.m

@@ -1,120 +0,0 @@
-clear;
-
-%% Load data
-dataPath = fullfile('.', 'sandbox', 'plot3');
-dataFiles = dir(dataPath);
-dataFiles = dataFiles(~startsWith({dataFiles.name}, '.'));
-simInits = dataFiles(endsWith({dataFiles.name}, 'miSimInits.mat'));
-simHists = dataFiles(endsWith({dataFiles.name}, 'miSimHist.mat'));
-assert(length(simHists) == length(simInits), "input data availability mismatch");
-assert(isscalar(simHists));
-
-init = load(fullfile(simInits(1).folder, simInits(1).name));
-hist = load(fullfile(simHists(1).folder, simHists(1).name));
-hist = hist.out;
-
-%% Plot 3: Per-agent and cumulative normalized performance
-assert(size(init.objectivePos, 1) == 1);
-assert(hist.useDoubleIntegrator);
-
-nAgents = length(hist.agent);
-agentLabels = "Agent " + string(1:nAgents)';
-
-f3 = figure;
-x3 = axes;
-hold(x3, 'on');
-plot(x3, hist.perf ./ init.objectiveIntegral, "LineWidth", 2);
-for ii = 1:nAgents
-    plot(x3, hist.agent(ii).perf ./ init.objectiveIntegral, "LineWidth", 2);
-end
-hold(x3, 'off');
-grid(x3, "on");
-ylabel(x3, "Performance (normalized)");
-xlabel(x3, "Timestep");
-legend(x3, ["Cumulative"; agentLabels], "Location", "northwest");
-title(x3, "$AII\beta$ Performance", "Interpreter", "latex");
-
-savefig(f3, "plot3.fig");
-exportgraphics(f3, "plot3.png");
-
-%% Plot 4: Pairwise distances and barrier functions
-commsRadius = hist.agent(1).commsRadius;
-collisionRadius = hist.agent(1).collisionRadius;
-nPairs = nchoosek(nAgents, 2);
-T = size(hist.agent(1).pos, 1);
-
-% Compute pairwise distances over time
-pairDistMat = NaN(T, nPairs);
-pairLabels = strings(nPairs, 1);
-pp = 0;
-for jj = 1:nAgents-1
-    for kk = jj+1:nAgents
-        pp = pp + 1;
-        pairDistMat(:, pp) = vecnorm(hist.agent(jj).pos - hist.agent(kk).pos, 2, 2);
-        pairLabels(pp) = sprintf("Agents %d-%d Distance", jj, kk);
-    end
-end
-
-f4 = figure;
-x4 = axes;
-
-% Left Y-axis: pairwise distances
-hold(x4, 'on');
-hLeft = gobjects(nPairs, 1);
-for pp = 1:nPairs
-    hLeft(pp) = plot(x4, pairDistMat(:, pp), 'LineWidth', 2);
-end
-yline(x4, collisionRadius, 'r--', "Label", "Collision Radius", ...
-    "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
-yline(x4, commsRadius, 'r--', "Label", "Communications Radius", ...
-    "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
-hold(x4, 'off');
-xlabel(x4, "Timestep");
-ylabel(x4, "Pairwise distance");
-title(x4, "$AII\beta$ Pairwise Agent Distances and Barrier Function Values", "Interpreter", "latex");
-grid(x4, "on");
-
-savefig(f4, "plot4_distanceOnly.fig");
-exportgraphics(f4, "plot4_distanceOnly.png");
-
-% Right Y-axis: barrier function values
-nObs = init.numObstacles;
-nAA = nchoosek(nAgents, 2);
-nAO = nAgents * nObs;
-nAD = nAgents * 6;
-nComms = size(hist.barriers, 1) - nAA - nAO - nAD;
-
-colStart = 1;
-comStart = colStart + nAA + nAO + nAD;
-
-pairColors = lines(nAA);
-
-yyaxis(x4, 'right');
-hold(x4, 'on');
-hRight = gobjects(nAA + nComms, 1);
-rightLabels = strings(nAA + nComms, 1);
-idx = 0;
-for pp = 1:nAA
-    idx = idx + 1;
-    hRight(idx) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', ...
-        'LineWidth', 1.5, 'Color', pairColors(pp, :));
-    rightLabels(idx) = sprintf('h_{col} %d', pp);
-end
-for pp = 1:nComms
-    idx = idx + 1;
-    hRight(idx) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', ...
-        'LineWidth', 1.5, 'Color', pairColors(pp, :));
-    rightLabels(idx) = sprintf('h_{com} %d', pp);
-end
-hold(x4, 'off');
-ylabel(x4, "Barrier function $h$", "Interpreter", "latex");
-
-% Y-axis limits
-yyaxis(x4, 'left');  ylim(x4, [0, 25]);
-yyaxis(x4, 'right'); ylim(x4, [0, 275]);
-x4.YAxis(2).Color = 'k';
-
-legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
-
-savefig(f4, "plot4.fig");
-exportgraphics(f4, "plot4.png");

resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/71xh8lboMaPoa5Dmw3ABwzIozfw/bjgcAyD7dxJIgrUo3yHl5f1-iksd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/71xh8lboMaPoa5Dmw3ABwzIozfw/bjgcAyD7dxJIgrUo3yHl5f1-iksp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8d.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/q1TwgWjEDIn-a93hPi1WGodL3NYp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="results.m" type="File"/>

resources/project/Hp7WImb16Wc_c7gK28oUydAFVts/EhSD3dO3mU2KP9IqbTVz7g60j5Ad.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/Hp7WImb16Wc_c7gK28oUydAFVts/EhSD3dO3mU2KP9IqbTVz7g60j5Ap.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/QVXvsR5cXQzGBi9QgRUFaflegRY/SltMFP7zr3UZsLPdRi9OyWAN7pMd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/QVXvsR5cXQzGBi9QgRUFaflegRY/SltMFP7zr3UZsLPdRi9OyWAN7pMp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/_1CKOWXf9sLpzeC642i06VHUH0Qp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="initializeFromInits.m" type="File"/>

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/abbdKJ8UvIXKWL0sR9NW0pEJSNIp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plotFromSimHist.m" type="File"/>

resources/project/Vq0hyJJCG53DJ8wxA_8WUihyBpg/H9TIrKqi-fFOWfgKFbIj3tQLZ3Yd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/Vq0hyJJCG53DJ8wxA_8WUihyBpg/H9TIrKqi-fFOWfgKFbIj3tQLZ3Yp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQId.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQIp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/boyWn-mt0RFdVNBj1mgEcGMQh_Ed.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <Info>
     <Category UUID="FileClassCategory">
-        <Label UUID="test"/>
+        <Label UUID="design"/>
     </Category>
 </Info>

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/boyWn-mt0RFdVNBj1mgEcGMQh_Ep.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="readControllerLogs.m" type="File"/>

resources/project/ls4G1ReemmlA4lw5Sxijq_iTPE0/YTBPi7yO4zYK5U1mxnqJDcdXNYkd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/ls4G1ReemmlA4lw5Sxijq_iTPE0/YTBPi7yO4zYK5U1mxnqJDcdXNYkp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test4" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/71xh8lboMaPoa5Dmw3ABwzIozfwd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/71xh8lboMaPoa5Dmw3ABwzIozfwp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test13" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGod.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGop.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plot3" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Hp7WImb16Wc_c7gK28oUydAFVtsd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Hp7WImb16Wc_c7gK28oUydAFVtsp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="t1" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/QVXvsR5cXQzGBi9QgRUFaflegRYd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/QVXvsR5cXQzGBi9QgRUFaflegRYp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test14" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Vq0hyJJCG53DJ8wxA_8WUihyBpgd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Vq0hyJJCG53DJ8wxA_8WUihyBpgp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test11" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xod.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xop.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plot1_2" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/ls4G1ReemmlA4lw5Sxijq_iTPE0d.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/ls4G1ReemmlA4lw5Sxijq_iTPE0p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test6" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plot1" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test3" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/soCFsvL91lhqvUw-h8HkHCtfsiwd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/soCFsvL91lhqvUw-h8HkHCtfsiwp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test10" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ed.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/yqgj8VLvTZ-pn09SsztajkpKW1Ep.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test12" type="File"/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plot1_3" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/FAIFc6EGkYrae07xGB2jWoFdM-0p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plots_3_4.m" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/cQhJ0S-hbiRJRVZSMGQe9M3c01Yp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plots_1_2.m" type="File"/>

resources/project/soCFsvL91lhqvUw-h8HkHCtfsiw/1fQ44ZBWSpGWUWzrR6iBNoEkvh4d.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/soCFsvL91lhqvUw-h8HkHCtfsiw/1fQ44ZBWSpGWUWzrR6iBNoEkvh4p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4d.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/yqgj8VLvTZ-pn09SsztajkpKW1E/68s6SYUbIV2lonVX55ZsTcaPe_4p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info/>

resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="1" type="DIR_SIGNIFIER"/>

test/parametricTestSuite.m

@@ -34,7 +34,14 @@ classdef parametricTestSuite < matlab.unittest.TestCase
 
             % Define scenario according to CSV specification
             tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
+            if length(params.objectiveVar) > 4 && length(params.objectivePos) > 2
+                objectiveSigma = permute(reshape(params.objectiveVar, [length(params.objectiveVar)/4 2 2]), [3 1 2]);
+                objectivePos = reshape(params.objectivePos, [length(params.objectivePos)/2, 2])';
+            else
+                objectiveSigma = reshape(params.objectiveVar, [1, 2, 2]);
+                objectivePos = params.objectivePos;
+            end
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(objectivePos, objectiveSigma), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum, objectivePos, objectiveSigma);
             
             agents = cell(size(params.initialPositions, 2) / 3, 1);
             for ii = 1:size(agents, 1)
@@ -81,7 +88,8 @@ classdef parametricTestSuite < matlab.unittest.TestCase
             for ii = 1:size(params.timestep, 1)
                 % Set up square domain
                 tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
-                tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, params.discretizationStep(ii), params.protectedRange(ii), params.sensorPerformanceMinimum(ii));
+                objectiveCenter = [.75 * l, 0.75 * l];
+                tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(objectiveCenter), tc.domain, params.discretizationStep(ii), params.protectedRange(ii), params.sensorPerformanceMinimum(ii), objectiveCenter);
                 
                 % Initialize agents
                 agents = cell(params.numAgents(ii), 1);

test/results.m

@@ -1,338 +0,0 @@
-classdef results < matlab.unittest.TestCase
-    properties (Constant, Access = private)
-        seed = 1;
-        domainSize = [150, 150, 100]; % fixed domain size [X, Y, Z]
-    end
-
-    properties (Access = private)
-        % System under test
-        testClass = miSim;
-
-        %% Diagnostic Parameters
-        % No effect on simulation dynamics
-        makeVideo = false; % disable video writing for big performance increase
-        makePlots = false; % disable plotting for big performance increase (also disables video)
-        plotCommsGeometry = false; % disable plotting communications geometries
-
-        %% Scenario Reinitialization
-        % Number of extra reinitializations per test case (3 n-values x 4 configs = 12).
-        % Order: n3/A_1_alpha, n3/A_1_beta, n3/A_2_alpha, n3/B_1_beta,
-        %        n5/A_1_alpha, ..., n6/B_1_beta
-        % Set inspectScenarios = true to pause after init for manual review.
-        % At the keyboard prompt, type the number of reinits needed into
-        % the variable 'reinitCount', then 'dbcont' to continue.
-        inspectScenarios = false;
-        reinit = zeros(1, 12);
-
-        %% Fixed Test Parameters
-        useFixedTopology = true; % No lesser neighbor, fixed network instead
-        discretizationStep = 0.5;
-        protectedRange = 5;
-        collisionRadius = 5;
-        sensorPerformanceMinimum = 0.005;
-        comRange = 20;
-        maxIter = 400;
-        initialStepSize = 1;
-        % Each row: [minX minY minZ maxX maxY maxZ]
-        obstacleCorners = [results.domainSize(1)/2, results.domainSize(2)*5/8, 0, results.domainSize(1)*5/8, results.domainSize(2), 35;
-                           results.domainSize(1)/3, 0, 0, results.domainSize(1)/2, results.domainSize(2)*3/8, 40];
-        barrierGain = 1;
-        barrierExponent = 1;
-        timestep = 0.5;
-        dampingCoeff = 2;
-    end
-
-    properties (TestParameter)
-        %% Test Iterations
-        % Specific parameter combos to run iterations on
-        n = struct('n3', 3, 'n5', 5, 'n6', 6); % number of agents
-        config = results.makeConfigs();
-    end
-
-    properties (MethodSetupParameter)
-        trials = struct('r1', 1, 'r2', 2, 'r3', 3, 'r4', 4, 'r5', 5);
-    end
-
-    methods (TestMethodSetup)
-        function setSeed(tc, trials)
-            rng(tc.seed + trials);
-        end
-    end
-
-    methods (Static, Access = public)
-        function c = makeConfigs()
-            rng(results.seed);
-            abMin = 6; % alpha*beta >= 6 ensures membership(0) = tanh(3) >= 0.995
-            alphaDist = rand(1, 2) .* [75, 45];
-            betaDist = abMin ./ alphaDist + rand(1, 2) .* [1, 1/8] .* (20 - abMin ./ alphaDist);
-            alphaTilt = 10 + rand(1, 2) .* [20, 20];
-            betaTilt = abMin ./ alphaTilt + rand(1, 2) .* (50 - abMin ./ alphaTilt);
-            sensors = struct('alphaDist', num2cell(alphaDist), 'alphaTilt', num2cell(alphaTilt), 'betaDist', num2cell(betaDist), 'betaTilt', num2cell(betaTilt));
-            sensor1 = sigmoidSensor;
-            sensor2 = sigmoidSensor;
-            sensor1 = sensor1.initialize(sensors(1).alphaDist, sensors(1).betaDist, sensors(1).alphaTilt, sensors(1).betaTilt);
-            sensor2 = sensor2.initialize(sensors(2).alphaDist, sensors(2).betaDist, sensors(2).alphaTilt, sensors(2).betaTilt);
-            % sensor1.plotParameters;
-            % sensor2.plotParameters;
-            c = struct('A_1_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', false), ...
-                        'A_1_beta',  struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', true), ...
-                        'A_2_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(2), 'doubleIntegrator', false), ...
-                        'B_1_beta',  struct('objectivePos', [[3, 1] / 4 .* results.domainSize(1:2); [3, 1] / 4 .* results.domainSize(1:2) + 25 .* [-1, 1] ./ sqrt(2)], 'sensor', sensors(1), 'doubleIntegrator', true));
-        end
-    end
-
-    methods (Test)
-        function plot1_runs(tc, n, config)
-            % if n == 5 && config.doubleIntegrator == true
-            %     tc.makePlots = true;
-            % else
-            %     tc.makePlots = false;
-            % end
-            % Compute test case index for reinit lookup
-            nKeys = fieldnames(tc.n);
-            configKeys = fieldnames(tc.config);
-            nIdx = find(cellfun(@(k) tc.n.(k) == n, nKeys));
-            configIdx = find(cellfun(@(k) isequal(tc.config.(k), config), configKeys));
-            testIdx = (nIdx - 1) * numel(configKeys) + configIdx;
-
-            % Determine number of reinitializations
-            reinitCount = tc.reinit(testIdx);
-
-            for reroll = 0:reinitCount
-
-            % Set up fixed-size domain
-            minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3);
-            % Place sensing objective(s) at fixed positions from config
-            objectiveMu = config.objectivePos;
-            numDist = size(objectiveMu, 1);
-            objectiveSigma = [];
-            for ii = 1:numDist
-                sig = [200, 140; 140, 280];
-                if ~mod(ii, 2)
-                    sig = rot90(sig,2);
-                end
-                sig = reshape(sig, [1, 2, 2]);
-                objectiveSigma = cat(1, objectiveSigma, sig);
-            end
-            tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain");
-            tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
-            
-            % Initialize agents
-            agents = cell(n, 1);
-            [agents{:}] = deal(agent);
-
-            % Initialize sensor model
-            sensorModel = sigmoidSensor;
-            sensorModel = sensorModel.initialize(config.sensor.alphaDist, config.sensor.betaDist, config.sensor.alphaTilt, config.sensor.betaTilt);
-
-            % Initialize fixed obstacles from corner coordinates
-            nObs = size(tc.obstacleCorners, 1);
-            obstacles = cell(nObs, 1);
-            for jj = 1:nObs
-                corners = [tc.obstacleCorners(jj, 1:3); tc.obstacleCorners(jj, 4:6)];
-                obstacles{jj} = rectangularPrism;
-                obstacles{jj} = obstacles{jj}.initialize(corners, REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", jj));
-            end
-
-            % Place agents in small-x, large-y quadrant (opposite objectives)
-            % with chain topology: each agent connected only to its neighbors
-            midXY = (tc.testClass.domain.minCorner(1:2) + tc.testClass.domain.maxCorner(1:2)) / 2;
-            quadrantSize = tc.testClass.domain.maxCorner(1:2) / 2;
-            margin = quadrantSize / 6;
-            agentBounds = [tc.testClass.domain.minCorner(1) + margin(1), ...
-                           midXY(2) + margin(2); ...
-                           midXY(1) - margin(1), ...
-                           tc.testClass.domain.maxCorner(2) - margin(2)];
-            % Find a fixed altitude where sensor performance passes at ALL
-            % corners of the placement bounds (worst-case XY)
-            corners = [agentBounds(1,1), agentBounds(1,2);
-                       agentBounds(2,1), agentBounds(1,2);
-                       agentBounds(1,1), agentBounds(2,2);
-                       agentBounds(2,1), agentBounds(2,2)];
-            agentAlt = tc.testClass.domain.maxCorner(3) - tc.collisionRadius;
-            while agentAlt > minAlt + 2 * tc.collisionRadius
-                worstPerf = inf;
-                for cc = 1:4
-                    p = sensorModel.sensorPerformance([corners(cc,:), agentAlt], [corners(cc,:), 0]);
-                    worstPerf = min(worstPerf, p);
-                end
-                if worstPerf >= tc.sensorPerformanceMinimum * 10
-                    break;
-                end
-                agentAlt = agentAlt - 1;
-            end
-            chainSpacingMin = 0.7 * tc.comRange;
-            chainSpacingMax = 0.9 * tc.comRange;
-            collisionGeometry = spherical;
-            for jj = 1:n
-                retry = true;
-                while retry
-                    retry = false;
-
-                    if jj == 1
-                        % First agent: random XY within bounds, fixed altitude
-                        agentPos = [agentBounds(1, :) + (agentBounds(2, :) - agentBounds(1, :)) .* rand(1, 2), agentAlt];
-                    else
-                        % Place at 0.7-0.9 * comRange in XY from previous agent, same altitude
-                        dir = randn(1, 2);
-                        dir = dir / norm(dir);
-                        r = chainSpacingMin + rand * (chainSpacingMax - chainSpacingMin);
-                        agentPos = [agents{jj-1}.pos(1:2) + r * dir, agentAlt];
-                    end
-
-                    % Check within placement bounds (XY only, Z is fixed)
-                    if any(agentPos(1:2) <= agentBounds(1, :)) || any(agentPos(1:2) >= agentBounds(2, :))
-                        retry = true;
-                        continue;
-                    end
-
-                    % Check sensor performance threshold; lower altitude if it fails
-                    if sensorModel.sensorPerformance(agentPos, [agentPos(1:2), 0]) < tc.sensorPerformanceMinimum * 10
-                        agentAlt = max(agentAlt - tc.collisionRadius, minAlt + 1.1 * tc.collisionRadius);
-                        agentPos(3) = agentAlt;
-                        % If we've hit the floor and still failing, widen XY search
-                        if agentAlt <= minAlt + 2 * tc.collisionRadius
-                            agentBounds = [tc.testClass.domain.minCorner(1) + tc.collisionRadius, ...
-                                           tc.testClass.domain.minCorner(2) + tc.collisionRadius; ...
-                                           tc.testClass.domain.maxCorner(1) - tc.collisionRadius, ...
-                                           tc.testClass.domain.maxCorner(2) - tc.collisionRadius];
-                        end
-                        retry = true;
-                        continue;
-                    end
-
-                    % Must be within comRange of previous agent (chain link)
-                    if jj > 1 && norm(agents{jj-1}.pos - agentPos) >= tc.comRange
-                        retry = true;
-                        continue;
-                    end
-
-                    % Must be BEYOND comRange of all non-adjacent agents (sparsity)
-                    % for kk = 1:(jj - 2)
-                    %     if norm(agents{kk}.pos - agentPos) < tc.comRange
-                    %         retry = true;
-                    %         break;
-                    %     end
-                    % end
-                    % if retry, continue; end
-
-                    % No collision with any existing agent
-                    for kk = 1:(jj - 1)
-                        if norm(agents{kk}.pos - agentPos) < agents{kk}.collisionGeometry.radius + tc.collisionRadius
-                            retry = true;
-                            break;
-                        end
-                    end
-                    if retry, continue; end
-
-                    % No collision with any obstacle
-                    for kk = 1:nObs
-                        P = min(max(agentPos, obstacles{kk}.minCorner), obstacles{kk}.maxCorner);
-                        d = agentPos - P;
-                        if dot(d, d) <= tc.collisionRadius^2
-                            retry = true;
-                            break;
-                        end
-                    end
-                end
-
-                % Initialize agent
-                collisionGeometry = collisionGeometry.initialize(agentPos, tc.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj));
-                agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, tc.comRange, tc.maxIter, tc.initialStepSize, sprintf("Agent %d", jj), tc.plotCommsGeometry);
-            end
-
-            % Randomly shuffle agents to vary index-based topology
-            agents = agents(randperm(numel(agents)));
-
-            end % reroll loop
-
-            % Inspect scenario if enabled
-            if tc.inspectScenarios
-                tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
-                fprintf("Test %d (n=%d, config=%s): reinit=%d. Inspect plot.\n", testIdx, n, configKeys{configIdx}, reinitCount);
-                fprintf("To add reinits, update tc.reinit(%d) and rerun.\n", testIdx);
-                keyboard;
-                tc.testClass = tc.testClass.teardown();
-                return;
-            end
-
-            % Set up simulation
-            tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
-            
-            % Save simulation parameters to output file
-            tc.testClass.writeInits();
-
-            % Run
-            tc.testClass = tc.testClass.run();
-
-            % Cleanup
-            tc.testClass = tc.testClass.teardown();
-            close all;
-        end
-        function AIIbeta_plots_3_4(tc)
-            % test-specific parameters
-            tc.makePlots = false;
-            tc.makeVideo = false;
-            maxIters = 400;
-
-            configs = results.makeConfigs();
-            c = configs.A_2_alpha;
-            c.doubleIntegrator = true; % make a2alpha into a2beta
-
-            % Set up fixed-size domain
-            minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3);
-            tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain");
-
-            % Set objective
-            objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4];
-            objectiveSigma = reshape([215, 100; 100, 175], [1, 2, 2]);
-            tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
-
-            % Set agent initial states (fully connected network of 4)
-            centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4];
-            d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4;
-            agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d;
-            agentAlt = minAlt * 1.5;
-            agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5];
-
-            agents = {agent, agent, agent, agent};
-            cg = spherical;
-            sensorModel = sigmoidSensor;
-            sensorModel = sensorModel.initialize(c.sensor.alphaDist, c.sensor.betaDist, c.sensor.alphaTilt, c.sensor.betaTilt);
-            agents{1} = agents{1}.initialize(agentsPos(1, :), cg.initialize(agentsPos(1, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 1", false);
-            agents{2} = agents{2}.initialize(agentsPos(2, :), cg.initialize(agentsPos(2, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 2 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 2", false);
-            agents{3} = agents{3}.initialize(agentsPos(3, :), cg.initialize(agentsPos(3, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 3 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 3", false);
-            agents{4} = agents{4}.initialize(agentsPos(4, :), cg.initialize(agentsPos(4, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 4 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 4", false);
-
-            obstacles = cell(1, 1);
-            obstacles{1} = rectangularPrism;
-            obstacles{1} = obstacles{1}.initialize([0, tc.domainSize(2)/2, 0; tc.domainSize(1) * 0.4, tc.domainSize(2), 40],REGION_TYPE.OBSTACLE, "Obstacle 1");
-
-            % Set up simulation
-            tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, maxIters, obstacles, tc.makePlots, tc.makeVideo, c.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
-
-            % Save simulation parameters to output file
-            tc.testClass.writeInits();
-
-            % Run
-            tc.testClass = tc.testClass.run();
-
-            % Cleanup
-            tc.testClass = tc.testClass.teardown();
-        end
-    end
-
-    methods
-        function c = obstacleCollisionCheck(~, obstacles, obstacle)
-            % Check if the obstacle intersects with any other obstacles
-            c = false;
-            for ii = 1:size(obstacles, 1)
-                if geometryIntersects(obstacles{ii}, obstacle)
-                    c = true;
-                    return;
-                end
-            end
-        end
-
-    end
-end

test/test_miSim.m

@@ -166,8 +166,8 @@ classdef test_miSim < matlab.unittest.TestCase
                     end
 
                     % Initialize candidate agent collision geometry
-                    candidateGeometry = rectangularPrism;
+                    candidateGeometry = spherical;
-                    candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
+                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
 
                     % Initialize candidate agent sensor model
                     tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
@@ -437,12 +437,12 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [7, 6]);
         
             % Initialize agent collision geometry
             tc.agents = {agent};
-            geometry1 = rectangularPrism;
+            geometry1 = spherical;
-            geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model with fixed parameters
             tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
@@ -466,7 +466,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [3, 7]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent};
@@ -504,7 +504,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5.2195]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent;};
@@ -588,7 +588,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent;};
@@ -633,7 +633,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent; agent; agent; agent;};
@@ -683,7 +683,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
@@ -728,6 +728,128 @@ classdef test_miSim < matlab.unittest.TestCase
                   0, 0, 0, 1, 1, 1, 1;
                   0, 0, 0, 0, 0, 1, 1; ]));
         end
+        function miSim_initializeFromInits(tc)
+            % Build a minimal valid simulation, write it to a matfile, reload
+            % via initializeFromInits, assert round-trip consistency, then
+            % delete the file.  No plotting or video at any stage.
+
+            % Obstacles
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
+            tc.obstacles = cell(nGeom, 1);
+            for ii = 1:nGeom
+                badCandidate = true;
+                while badCandidate
+                    tc.obstacles{ii} = rectangularPrism;
+                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, ...
+                        sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, ...
+                        tc.domain, tc.minAlt);
+                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
+                        badCandidate = false;
+                    end
+                end
+            end
+
+            % Agents
+            nAgents = tc.minAgents;
+            tc.agents = cell(nAgents, 1);
+            tc.collisionRanges = tc.minCollisionRange + rand(nAgents, 1) * (tc.maxCollisionRange - tc.minCollisionRange);
+            tc.commsRanges = tc.minCommsRange + rand(nAgents, 1) * (tc.maxCommsRange - tc.minCommsRange);
+
+            for ii = 1:nAgents
+                initInvalid = true;
+                while initInvalid
+                    if ii == 1
+                        candidatePos = tc.domain.random();
+                        candidatePos(3) = tc.minAlt + rand * 3;
+                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                            candidatePos = tc.domain.random();
+                            candidatePos(3) = tc.minAlt + rand * 3;
+                        end
+                    else
+                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii) / sqrt(2));
+                        candidatePos(3) = tc.minAlt + rand * 3;
+                    end
+
+                    if ~tc.domain.contains(candidatePos), continue; end
+                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2), continue; end
+
+                    % Connectivity check
+                    connections = false(1, ii - 1);
+                    for jj = 1:(ii - 1)
+                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
+                            connections(jj) = true;
+                            for kk = 1:size(tc.obstacles, 1)
+                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
+                                    connections(jj) = false;
+                                end
+                            end
+                        end
+                    end
+                    if ii ~= 1 && ~any(connections), continue; end
+
+                    geom = spherical;
+                    geom = geom.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
+                    tc.sensor = sigmoidSensor;
+                    tc.sensor = tc.sensor.initialize( ...
+                        tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), ...
+                        tc.betaDistMin  + rand * (tc.betaDistMax  - tc.betaDistMin), ...
+                        tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), ...
+                        tc.betaTiltMin  + rand * (tc.betaTiltMax  - tc.betaTiltMin));
+                    newAgent = agent;
+                    newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+
+                    % Domain / obstacle / agent collision checks
+                    violation = false;
+                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
+                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                            violation = true; break;
+                        end
+                    end
+                    if violation, continue; end
+                    for kk = 1:size(tc.obstacles, 1)
+                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
+                            violation = true; break;
+                        end
+                    end
+                    if violation, continue; end
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
+                            violation = true; break;
+                        end
+                    end
+                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
+                        violation = true;
+                    end
+                    if violation, continue; end
+
+                    initInvalid = false;
+                    tc.agents{ii} = newAgent;
+                end
+            end
+
+            % Initialize first sim (no plots / video)
+            sim1 = miSim;
+            sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ...
+                tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ...
+                tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+
+            % Write inits and build file path
+            sim1.writeInits();
+            initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", ...
+                strcat(sim1.artifactName, "_miSimInits.mat"));
+
+            % Load via initializeFromInits
+            sim2 = miSim;
+            sim2 = sim2.initializeFromInits(initsFile);
+
+            % Assertions
+            tc.assertEqual(size(sim2.agents, 1), size(sim1.agents, 1));
+            tc.assertEqual(sim2.maxIter, sim1.maxIter);
+            tc.assertEqual(sim2.barrierGain, sim1.barrierGain);
+
+            % Cleanup
+            delete(initsFile);
+        end
     end
 
     methods

util/objectiveFunctionWrapper.m

@@ -4,12 +4,16 @@ function f = objectiveFunctionWrapper(center, sigma)
     % composite objectives in particular
     arguments (Input)
         center (:, 2) double;
-        sigma (:, 2, 2) double = eye(2);
+        sigma (:, 2, 2) double = reshape(eye(2), 1, 2, 2);
     end
     arguments (Output)
         f (1, 1) {mustBeA(f, "function_handle")};
     end
+    
+    if size(sigma, 1) == 1 && size(center, 1) > 1
+        sigma = repmat(sigma, size(center, 1), 1, 1);
+    end
+
     assert(size(center, 1) == size(sigma, 1));
     f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
-
 end