experiment 1 obstacle collision fixes completed

.gitignore

@@ -48,6 +48,7 @@ sandbox/*
 
 # Figures
 *.fig
+*.png
 
 # Python Virtual Environment
 aerpaw/venv/

@agent/initialize.m

@@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     end
 
     obj.pos = pos;
+    obj.lastPos = pos;
     obj.vel = zeros(1, 3);
     obj.lastVel = zeros(1, 3);
     obj.collisionGeometry = collisionGeometry;

@agent/run.m

@@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
         obj (1, 1) {mustBeA(obj, "agent")};
     end
 
+    % Always update lastPos/lastVel so constrainMotion evaluates barriers at
+    % the correct (most recent) position, even when this agent has no partition.
+    obj.lastPos = obj.pos;
+    if useDoubleIntegrator
+        obj.lastVel = obj.vel;
+    end
+
     % Collect objective function values across partition
     partitionMask = partitioning == index;
     if ~any(partitionMask(:))
@@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
     gradNorm = norm(gradC);
 
     % Compute unconstrained next state
-    obj.lastPos = obj.pos;
     if useDoubleIntegrator
         % Double-integrator: gradient produces desired acceleration with damping
-        obj.lastVel = obj.vel;
         if gradNorm < 1e-100
             a_gradient = zeros(1, 3);
         else

@miSim/constrainMotion.m

@@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj)
     h(logical(eye(nAgents))) = 0; % self value is 0
     for ii = 1:(nAgents - 1)
         for jj = (ii + 1):nAgents
-            h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
+            h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
             h(jj, ii) = h(ii, jj);
 
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
             A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
             % Slack derived from existing params: recovery velocity = max gradient approach velocity.
             % Correction splits between 2 agents, so |A| = 2*r_sum
@@ -61,7 +61,9 @@ function [obj] = constrainMotion(obj)
     end
 
     idx = length(h(triu(true(size(h)), 1)));    
-    obj.barriers(1:idx, obj.timestepIndex) = 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));
@@ -69,11 +71,11 @@ function [obj] = constrainMotion(obj)
     for ii = 1:nAgents
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
+            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
 
-            hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+            hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
 
-            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos);
             % Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
             r_i = obj.agents{ii}.collisionGeometry.radius;
             v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
@@ -84,7 +86,9 @@ function [obj] = constrainMotion(obj)
         end
     end
 
-    obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+    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)
@@ -93,48 +97,45 @@ function [obj] = constrainMotion(obj)
     h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
     for ii = 1:nAgents
         % X minimum
-        h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y minimum
-        h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
-        h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
         b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
         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];
+        if coder.target('MATLAB')
-        idx = idx + 6;
+	    obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-    end
+        end
-
+	idx = idx + 6;
-    if coder.target('MATLAB')
-        % Save off h function values (logging only — not needed in compiled mode)
-        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
 
     % Add communication network constraints
@@ -145,9 +146,9 @@ function [obj] = constrainMotion(obj)
             if obj.constraintAdjacencyMatrix(ii, jj)
                 paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
                 r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
-                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
+                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
 
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
 
                 % One-step forward invariance: b = h/dt ensures h cannot
@@ -164,7 +165,10 @@ function [obj] = constrainMotion(obj)
             end
         end
     end
-    obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
+    
+    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
@@ -138,6 +136,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Initialize variable that will store barrier function values per timestep for analysis purposes
         obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
 
+        % Initialize constraint adjacency history (nAgents x nAgents x nTimesteps)
+        nAgents = size(obj.agents, 1);
+        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

@@ -7,7 +7,6 @@ classdef miSim
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
         domain;
-        objective;
         obstacles; % geometries that define obstacles within the domain
         agents; % agents that move within the domain
         adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -28,6 +27,7 @@ classdef miSim
         spatialPlotIndices = [6, 4, 3, 2];
         numBarriers = 0; % Number of barrier functions needed
         barriers = []; % log barrier function values at each timestep for analysis
+        constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
     end
 
     properties (Access = private)
@@ -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
@@ -67,12 +66,13 @@ classdef miSim
                 obj (1, 1) miSim
             end
             obj.domain = rectangularPrism;
-            obj.objective = sensingObjective;
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
         [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
         [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

@@ -34,6 +34,11 @@ function [obj] = run(obj)
             obj = obj.lesserNeighbor();
         end
 
+        % Log constraint adjacency for this timestep
+        if coder.target('MATLAB')
+            obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
+        end
+
         % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)

@miSim/teardown.m

@@ -20,6 +20,7 @@ function obj = teardown(obj)
     out.dampingCoeff = obj.dampingCoeff;
     out.useDoubleIntegrator = obj.useDoubleIntegrator;
     out.useFixedTopology = obj.useFixedTopology;
+    out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
     for ii = 1:size(obj.agents, 1)
         out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
         out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
@@ -39,11 +40,11 @@ function obj = teardown(obj)
     obj.timestepIndex = NaN;
     obj.maxIter = NaN;
     obj.domain = rectangularPrism;
-    obj.objective = sensingObjective;
     obj.obstacles = cell(0, 1);
     obj.agents = cell(0, 1);
     obj.adjacency = NaN;
     obj.constraintAdjacencyMatrix = NaN;
+    obj.constraintAdjacencyHist = [];
     obj.partitioning = NaN;
     obj.performance = 0;
     obj.barrierGain = NaN;

@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/validate.m

@@ -7,11 +7,11 @@ function validate(obj)
 
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
-        warning("Network is not connected");
+        error("Network is not connected");
     end
 
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
-        warning("Eliminated network connections that were necessary");
+        error("Eliminated network connections that were necessary");
     end
 
     %% Obstacle Validators
@@ -20,10 +20,9 @@ function validate(obj)
         for kk = 1:size(obj.agents, 1)
             P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
             d = obj.agents{kk}.pos - P;
-            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
+            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
-                warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
+                error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, - dot(d, d) + obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
             end
         end
     end
-
 end

@miSim/writeInits.m

@@ -5,26 +5,37 @@ 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, userObstacles, '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); % still needs obstacle states and objective state
+                    "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(:)));
 
     % Save all parameters to output file
     initsFile = strcat(obj.artifactName, "_miSimInits");

@sensingObjective/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
     arguments (Input)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
@@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
         sensorPerformanceMinimum (1, 1) double = 1e-6;
+        objectiveMu (:, 2) double = NaN(1, 2);
+        objectiveSigma (:, 2, 2) double = NaN(1, 2, 2);
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
@@ -37,8 +39,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
 
     % store ground position
     idx = obj.values == 1;    
-    obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    if any(isnan(objectiveMu))
-    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+        obj.groundPos = [obj.X(idx), obj.Y(idx)];
+        obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+    else
+        obj.groundPos = objectiveMu;
+    end
+    obj.objectiveSigma = objectiveSigma;
 
-    assert(domain.distance([obj.groundPos, 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

@@ -11,16 +11,16 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
 
     % Set random objective position
     mu = domain.minCorner;
-    while domain.distance(mu) < protectedRange
+    while domain.distance(mu) < protectedRange * 1.01
         mu = domain.random();
     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

@sensingObjective/sensingObjective.m

@@ -2,7 +2,8 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundPos = [NaN, NaN];
+        groundPos = NaN(1, 2);
+        objectiveSigma = NaN(1, 2, 2);
         discretizationStep = NaN;
         X = [];
         Y = [];

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
-5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "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, 100, 30.0, 0.1, 2.0, 5, 1, 1, "6.0, 6.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

aerpaw/controller.coderprj

@@ -138,6 +138,26 @@
                   <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>
             </coderapp.internal.interface.project.Interface>
           </MF0>
@@ -379,719 +399,761 @@
                 </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/controller.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/controller.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_data.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_data.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_initialize.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_initialize.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_internal_types.h</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_terminate.cpp</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_terminate.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_types.h</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/countsort.cpp</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/countsort.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/deleteColMoveEnd.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/deleteColMoveEnd.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/dot.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/dot.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/driver.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/driver.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/exp.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/exp.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/eye.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/eye.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/factorQR.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/factorQR.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/feasibleX0ForWorkingSet.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/feasibleX0ForWorkingSet.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/feasibleratiotest.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/feasibleratiotest.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/find.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/find.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/fullColLDL2_.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/fullColLDL2_.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/guidance_step.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/guidance_step.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/iterate.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/iterate.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/ixamax.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/ixamax.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/ixfun.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/ixfun.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/lesserNeighbor.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/lesserNeighbor.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/linearForm_.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/linearForm_.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/maxConstraintViolation.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/maxConstraintViolation.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/meshgrid.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/meshgrid.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/miSim.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/miSim.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/minOrMax.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/minOrMax.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/mpower.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/mpower.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/nchoosek.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/nchoosek.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/norm.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/norm.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/partition.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/partition.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/phaseone.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/phaseone.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/quadprog.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/quadprog.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/ratiotest.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/ratiotest.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/rectangularPrism.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/rectangularPrism.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/removeConstr.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/removeConstr.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/repmat.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/repmat.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/rtGetInf.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/rtGetInf.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/rtGetNaN.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/rtGetNaN.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/rt_defines.h</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/rt_nonfinite.cpp</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_nonfinite.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/rtwtypes.h</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/run.cpp</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/run.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/sensingObjective.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/sensingObjective.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/sensorPerformance.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/sensorPerformance.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/setProblemType.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/setProblemType.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/sigmoidSensor.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/sigmoidSensor.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/sort.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/sort.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/spherical.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/spherical.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/sprintf.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/sprintf.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/squareQ_appendCol.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/squareQ_appendCol.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/string1.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/string1.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/sum.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/sum.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/triu.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/triu.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/unique.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/unique.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/vecnorm.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/vecnorm.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/xgemv.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/xgemv.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/xgeqp3.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/xgeqp3.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/xnrm2.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/xnrm2.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/xrotg.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/xrotg.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/xzgeqp3.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/xzgeqp3.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/xzlarf.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/xzlarf.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/xzlarfg.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/xzlarfg.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/matlab/R2025a/extern/include/tmwtypes.h</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/examples/main.cpp</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/Desktop/miSim/aerpaw/codegen/examples/main.h</Path>
                   </File>
@@ -1099,7 +1161,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-03-11T17:11:03</Timestamp>
+                <Timestamp>2026-04-05T13:43:54</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

aerpaw/controller.m

@@ -7,7 +7,8 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-MAX_TARGETS = MAX_CLIENTS;
+% Two waypoints per UAV: altitude-staggered transit + final position
+MAX_TARGETS = MAX_CLIENTS * 2;
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
@@ -33,6 +34,28 @@ else
     numWaypoints = totalLoaded / int32(numClients);
 end
 
+% In the compiled path, inject altitude-staggered transit waypoints so UAVs
+% are vertically separated while flying horizontally to their start positions.
+% ArduPilot reaches target altitude before horizontal movement, so UAV i is at
+% altitude (TRANSIT_ALT_BASE + (i-1)*TRANSIT_ALT_STEP) throughout its transit,
+% preventing collisions regardless of horizontal path geometry.
+% TRANSIT_ALT_STEP must exceed 2 * max(collisionRadius).
+% Waypoint 1: each UAV flies to (finalX, finalY) at its unique transit altitude.
+% Waypoint 2: each UAV adjusts to its actual target altitude.
+if ~coder.target('MATLAB')
+    TRANSIT_ALT_BASE = 25.0;  % must match drone.takeoff() altitude in uav_runner.py
+    TRANSIT_ALT_STEP = 12.1;  % vertical separation per UAV (m); must exceed 2*collisionRadius
+    for ii = double(totalLoaded):-1:1
+        transitRow = (ii - 1) * 2 + 1;
+        finalRow   = (ii - 1) * 2 + 2;
+        finalPos   = targets(ii, :);
+        transitAlt = TRANSIT_ALT_BASE + (ii - 1) * TRANSIT_ALT_STEP;
+        targets(finalRow, :)   = finalPos;
+        targets(transitRow, :) = [finalPos(1), finalPos(2), transitAlt];
+    end
+    numWaypoints = int32(2);
+end
+
 % Load guidance scenario from CSV (parameters for guidance_step)
 NUM_SCENARIO_PARAMS = 45;
 MAX_OBSTACLES_CTRL  = int32(8);

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;
@@ -48,8 +49,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

aerpaw/results/readRadioLogs.m

@@ -31,14 +31,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

@@ -3,7 +3,8 @@ resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_
 
 % 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 +22,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)

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/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/_1CKOWXf9sLpzeC642i06VHUH0Qd.xml

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

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/abbdKJ8UvIXKWL0sR9NW0pEJSNId.xml

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

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/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/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/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/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/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/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"/>

test/parametricTestSuite.m

@@ -34,7 +34,8 @@ 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);
+            objectiveSigma = reshape(params.objectiveVar, [1 2 2]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, objectiveSigma), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum, params.objectivePos, objectiveSigma);
 
             agents = cell(size(params.initialPositions, 2) / 3, 1);
             for ii = 1:size(agents, 1)
@@ -81,7 +82,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/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,12 @@ function f = objectiveFunctionWrapper(center, sigma)
     % composite objectives in particular
     arguments (Input)
         center (:, 2) double;
-        sigma (2, 2) double = eye(2);
+        sigma (:, 2, 2) double = eye(2);
     end
     arguments (Output)
         f (1, 1) {mustBeA(f, "function_handle")};
     end
     
-    f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2);
+    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