test case cleanup

.gitignore

@@ -48,22 +48,3 @@ sandbox/*
 
 # Figures
 *.fig
-*.png
-
-# Python Virtual Environment
-aerpaw/venv/
-aerpaw/venv/*
-
-# Pycache
-__pycache__
-
-# aerpaw stuff
-aerpaw/build/
-aerpaw/build/*
-aerpaw/client/__pycache__/
-aerpaw/client/__pycache__/*
-aerpaw/codegen/
-aerpaw/codegen/*
-
-# results
-*.csv

@agent/agent.m

@@ -6,8 +6,6 @@ classdef agent
         % State
         lastPos = NaN(1, 3); % position from previous timestep
         pos = NaN(1, 3); % current position
-        vel = zeros(1, 3); % velocity (double-integrator mode)
-        lastVel = zeros(1, 3); % pre-step velocity (double-integrator mode)
 
         % Sensor
         sensorModel;
@@ -19,8 +17,8 @@ classdef agent
         fovGeometry;
 
         % Communication
-        commsGeometry;
-        lesserNeighbors = zeros(1, 0);
+        commsGeometry = spherical;
+        lesserNeighbors = [];
 
         % Performance
         performance = 0;
@@ -32,26 +30,13 @@ classdef agent
 
     properties (SetAccess = private, GetAccess = public)
         initialStepSize = NaN;
-        initialMaxAngleStepSize = NaN;
         stepDecayRate = NaN;
-        angleStepDecayRate = NaN;
     end
 
     methods (Access = public)
-        function obj = agent()
-            arguments (Input)
-            end
-            arguments (Output)
-                obj (1, 1) agent
-            end
-            obj.collisionGeometry = spherical;
-            obj.sensorModel = sigmoidSensor;
-            obj.fovGeometry = cone;
-            obj.commsGeometry = spherical;
-        end
         [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents);
-        [partitioning, agents] = partition(obj, agents, objective)
+        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [partitioning] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,42 +1,34 @@
-function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "agent")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
         pos (1, 3) double;
         collisionGeometry (1, 1) {mustBeGeometry};
         sensorModel (1, 1) {mustBeSensor};
         comRange (1, 1) double;
         maxIter (1, 1) double;
         initialStepSize (1, 1) double = 0.2;
-        initialMaxAngleStepSize (1, 1) double = 5.0;
         label (1, 1) string = "";
         plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "agent")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
     obj.pos = pos;
-    obj.lastPos = pos;
-    obj.vel = zeros(1, 3);
-    obj.lastVel = zeros(1, 3);
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
     obj.label = label;
     obj.plotCommsGeometry = plotCommsGeometry;
     obj.initialStepSize = initialStepSize;
-    obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
     obj.stepDecayRate = obj.initialStepSize / maxIter;
-    obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
 
     % Initialize performance vector
-    if coder.target('MATLAB')
-        obj.performance = [0, NaN(1, maxIter), 0];
-    end
+    obj.performance = [0, NaN(1, maxIter), 0];
 
     % Add spherical geometry based on com range
     obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.halfAngle()) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label), obj.sensorModel.tilt, obj.sensorModel.azimuth);
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@agent/partition.m

@@ -1,54 +1,35 @@
-function [partitioning, agents] = partition(obj, agents, objective)
+function [partitioning] = partition(obj, agents, objective)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "agent")};
-        agents (:, 1) {mustBeA(agents, "cell")};
-        objective (1, 1) {mustBeA(objective, "sensingObjective")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        agents (:, 1) {mustBeA(agents, 'cell')};
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
     end
     arguments (Output)
         partitioning (:, :) double;
-        agents (:, 1) cell;
     end
     
-    nAgents = size(agents, 1);
-    gridM   = size(objective.X, 1);
-    gridN   = size(objective.X, 2);
-    nPoints = gridM * gridN;
+    % Assess sensing performance of each agent at each sample point
+    % in the domain
+    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
+    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    agentPerformances = cat(3, agentPerformances{:});
 
-    % Assess sensing performance of each agent at each sample point.
-    % agentPerf is (nPoints x nAgents+1): the extra column is the
-    % minimum threshold that must be exceeded for any assignment.
-    agentPerf = zeros(nPoints, nAgents + 1);
-    for aa = 1:nAgents
-        if isa(agents{aa}.sensorModel, "sigmoidSensor")
-            p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
-                [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
-        elseif isa(agents{aa}.sensorModel, "rfSensor")
-            otherSensorsIdx = [1:(aa - 1), (aa + 1):size(agents, 1)];
-            otherSensors = agents(otherSensorsIdx);
-            otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherSensors, "UniformOutput", false));
-            otherSensors = cellfun(@(x) x.sensorModel, otherSensors, "UniformOutput", false);
-            [p, ~, agents{aa}.sensorModel, otherSensors] = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
-                [objective.X(:), objective.Y(:), zeros(nPoints, 1)], otherSensorsPos, otherSensors);
-            for k = 1:numel(otherSensorsIdx)
-                agents{otherSensorsIdx(k)}.sensorModel = otherSensors{k};
-            end
-        else
-            error("?");
-        end
-        agentPerf(:, aa) = p(:);
+    % Get highest performance value at each point
+    [~, idx] = max(agentPerformances, [], 3);
+
+    % Collect agent indices in the same way as performance
+    indices = 1:size(agents, 1);
+    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+    if size(agentInds, 1) ~= size(agents, 1)
+        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
     end
-    agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;
+    agentInds = num2cell(agentInds, 2:3);
+    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
+    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
+    agentInds = cat(3, agentInds{:});
 
-    % Find which agent has highest performance at each point.
-    % If the threshold column wins (idx == nAgents+1) the point is unassigned (0).
-    [~, idx] = max(agentPerf, [], 2);
-
-    assignedAgent = zeros(nPoints, 1);
-    for pp = 1:nPoints
-        if idx(pp) <= nAgents
-            assignedAgent(pp) = idx(pp);
-        end
-    end
-
-    partitioning = reshape(assignedAgent, gridM, gridN);
+    % Use highest performing agent's index to form partitions
+    [m, n, ~] = size(agentInds);
+    [jj, kk] = ndgrid(1:m, 1:n);
+    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
 end

@agent/plot.m

@@ -1,12 +1,12 @@
 function [obj, f] = plot(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "agent")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "agent")};
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Create axes if they don't already exist
@@ -14,11 +14,11 @@ function [obj, f] = plot(obj, ind, f)
 
     % Plot points representing the agent position
     hold(f.Children(1).Children(end), "on");
-    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), "filled", "ko", "SizeData", 25);
+    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
     hold(f.Children(1).Children(end), "off");
 
     % Check if this is a tiled layout figure
-    if strcmp(f.Children(1).Type, "tiledlayout")
+    if strcmp(f.Children(1).Type, 'tiledlayout')
         % Add to other perspectives
         o = [o; copyobj(o(1), f.Children(1).Children(2))];
         o = [o; copyobj(o(1), f.Children(1).Children(3))];

@agent/run.m

@@ -1,30 +1,19 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "agent")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
         timestepIndex (1, 1) double;
         index (1, 1) double;
-        useDoubleIntegrator (1, 1) logical = false;
-        dampingCoeff (1, 1) double = 2.0;
-        dt (1, 1) double = 1.0;
-        optimizeSensorPointing (1, 1) logical = false;
-        otherAgents (:, 1) cell = cell();
+        agents (:, 1) {mustBeA(agents, 'cell')};
     end
     arguments (Output)
-        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;
+        obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
     % Collect objective function values across partition
     partitionMask = partitioning == index;
-    if ~any(partitionMask(:))
+    if ~unique(partitionMask)
         % This agent has no partition, maintain current state
         return;
     end
@@ -34,62 +23,33 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleInt
     maskedX = domain.objective.X(partitionMask);
     maskedY = domain.objective.Y(partitionMask);
 
-    if isa(obj.sensorModel, "rfSensor")
-        % Extract other agents' sensor models and positions once, outside the delta loop.
-        % Mask the full-grid RSS caches (filled by partition()) down to this agent's
-        % partition subset so sensorPerformance can reuse them for all perturbations.
-        otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherAgents, "UniformOutput", false));
-        otherSensors    = cellfun(@(x) x.sensorModel, otherAgents, "UniformOutput", false);
-        partitionIndices = find(partitionMask);
-        for kk = 1:numel(otherSensors)
-            if ~isempty(otherSensors{kk}.rssCache)
-                otherSensors{kk}.rssCache = otherSensors{kk}.rssCache(partitionIndices);
-            end
-        end
-        % Pre-mask this agent's own full-grid cache to the partition subset.
-        % Used for ii==1 (current position, no perturbation) to avoid recomputing.
-        baseSensorModel = obj.sensorModel;
-        if ~isempty(obj.sensorModel.rssCache)
-            baseSensorModel.rssCache = obj.sensorModel.rssCache(partitionIndices);
-        end
-    end
-
-    if optimizeSensorPointing
-        % Stash actual current sensor model tilt/azimuth before messing with it
-        % in these following hypotheticals
-        tilt = obj.sensorModel.tilt;
-        azimuth = obj.sensorModel.azimuth;
-    end
-
-    % Compute agent performance at the current position and each delta position +/- X, Y, Z, tilt, azimuth
-    deltaPos = domain.objective.discretizationStep; % smallest possible step size that gets different results
-    if optimizeSensorPointing
-        deltaAngle = atan2d(domain.objective.discretizationStep, obj.pos(3)); % smallest possible angle derived from smallest possible step size and current height
-    end
-    deltaApplicator = [0, 0, 0, 0, 0; 1, 0, 0, 0, 0; -1, 0, 0, 0, 0; 0, 1, 0, 0, 0; 0, -1, 0, 0, 0; 0, 0, 1, 0, 0; 0, 0, -1, 0, 0; 0, 0, 0, 1, 0; 0, 0, 0, -1, 0; 0, 0, 0, 0, 1; 0, 0, 0, 0, -1;]; % none, +X, -X, +Y, -Y, +Z, -Z, +tilt, -tilt, +azimuth, -azimuth
-    C_delta = NaN(size(deltaApplicator, 1), 1); % agent performance at delta steps in each direction
-    for ii = 1:size(deltaApplicator, 1)
-        if ~optimizeSensorPointing && ii > 7; break; end
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+    deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
+    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+    for ii = 1:7
         % Apply delta to position
-        pos = obj.pos + deltaPos * deltaApplicator(ii, 1:3);
-        if optimizeSensorPointing
-            % Apply delta to tilt and azimuth
-            obj.sensorModel.tilt = tilt + deltaAngle * deltaApplicator(ii, 4);
-            obj.sensorModel.azimuth = azimuth + deltaAngle * deltaApplicator(ii, 5);
-        end
+        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
         
         % Compute performance values on partition
-        if isa(obj.sensorModel, "sigmoidSensor")
+        if ii < 5
+            % Compute sensing performance
             sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-        elseif isa(obj.sensorModel, "rfSensor")
-            if ii == 1
-                sensorModelForDelta = baseSensorModel; % reuse partition-step cache; no recompute needed
-            else
-                sensorModelForDelta = obj.sensorModel.clearRssCache;
-            end
-            [sensorValues, ~, ~, ~] = sensorModelForDelta.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))], otherSensorsPos, otherSensors);
+            % Objective performance does not change for 0, +/- X, Y steps.
+            % Those values are computed once before the loop and are only
+            % recomputed when +/- Z steps are applied
         else
-            error("?");
+            % Redo partitioning for Z stepping only
+            partitioning = obj.partition(agents, domain.objective);
+
+            % Recompute partiton-derived performance values for objective
+            partitionMask = partitioning == index;
+            objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
+
+            % Recompute partiton-derived performance values for sensing
+            maskedX = domain.objective.X(partitionMask);
+            maskedY = domain.objective.Y(partitionMask);
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
         end
 
         % Rearrange data into image arrays
@@ -103,59 +63,28 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleInt
         C_delta(ii) = sum(C(~isnan(C)));
     end
 
-    if optimizeSensorPointing
-        % Reset sensor model to actual tilt and azimuth angles
-        obj.sensorModel.tilt = tilt;
-        obj.sensorModel.azimuth = azimuth;
-    end
-
     % Store agent performance at current time and place
-    if coder.target('MATLAB')
-        obj.performance(timestepIndex + 1) = C_delta(1);
-    end
+    obj.performance(timestepIndex + 1) = C_delta(1);
 
     % Compute gradient by finite central differences
-    gradC = [(C_delta(2)-C_delta(3))/(2*deltaPos), (C_delta(4)-C_delta(5))/(2*deltaPos), (C_delta(6)-C_delta(7))/(2*deltaPos)];
-    if optimizeSensorPointing
-        gradC(4) = (C_delta(8) -C_delta(9)) /(2*deltaAngle);
-        gradC(5) = (C_delta(10)-C_delta(11))/(2*deltaAngle);
-    end
+    gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
 
     % Compute scaling factor
-    targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
-    gradPosNorm = norm(gradC(1:3));
+    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+    rateFactor = targetRate / norm(gradC);
 
-    % Compute unconstrained next state
-    if useDoubleIntegrator
-        % Double-integrator: gradient produces desired acceleration with damping
-        if gradPosNorm < 1e-100
-            a_gradient = zeros(1, 5);
-        else
-            % Scale so steady-state step ≈ targetRate (matching SI behavior)
-            a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
-        end
-        % Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
-        obj.vel = (obj.vel + a_gradient(1:3) * dt) / (1 + dampingCoeff * dt);
-        obj.pos = obj.lastPos + obj.vel * dt;
-    else
-        % Single-integrator: gradient directly sets position step
-        if gradPosNorm >= 1e-100
-            obj.pos = obj.pos + (targetPosRate / gradPosNorm) * gradC(1:3);
-        end
-    end
+    % Compute unconstrained next position
+    pNext = obj.pos + rateFactor * gradC;
 
-    % Update tilt and azimuth, saturating at the decaying maximum allowed step size
-    if optimizeSensorPointing
-        maxAngleStep = obj.initialMaxAngleStepSize - obj.angleStepDecayRate * timestepIndex;
-        obj.sensorModel.tilt    = obj.sensorModel.tilt    + sign(gradC(4)) * min(abs(gradC(4)), maxAngleStep);
-        obj.sensorModel.azimuth = obj.sensorModel.azimuth + sign(gradC(5)) * min(abs(gradC(5)), maxAngleStep);
-    end
+    % Move to next position
+    obj.lastPos = obj.pos;
+    obj.pos = pNext;
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;
-    if isa(obj.collisionGeometry, "rectangularPrism")
+    if isa(obj.collisionGeometry, 'rectangularPrism')
         obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
-    elseif isa(obj.collisionGeometry, "spherical")
+    elseif isa(obj.collisionGeometry, 'spherical')
         obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
     else
         error("?");

@agent/updatePlots.m

@@ -1,74 +1,51 @@
 function updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "agent")};
+        obj (1, 1) {mustBeA(obj, 'agent')};
     end
     arguments (Output)
     end
 
     % Find change in agent position since last timestep
     deltaPos = obj.pos - obj.lastPos;
-    posChanged    = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
-    orientChanged = obj.sensorModel.tilt    ~= obj.fovGeometry.tilt || ...
-                    obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
-
-    if ~posChanged && ~orientChanged
+    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+        % Agent did not move, so nothing has to move on the plots
         return;
     end
 
-    if posChanged
-        % Scatterplot point positions
-        for ii = 1:size(obj.scatterPoints, 1)
-            obj.scatterPoints(ii).XData = obj.pos(1);
-            obj.scatterPoints(ii).YData = obj.pos(2);
-            obj.scatterPoints(ii).ZData = obj.pos(3);
-        end
+    % Scatterplot point positions
+    for ii = 1:size(obj.scatterPoints, 1)
+        obj.scatterPoints(ii).XData = obj.pos(1);
+        obj.scatterPoints(ii).YData = obj.pos(2);
+        obj.scatterPoints(ii).ZData = obj.pos(3);
+    end
 
-        % Collision geometry edges
-        for jj = 1:size(obj.collisionGeometry.lines, 2)
+    % Collision geometry edges
+    for jj = 1:size(obj.collisionGeometry.lines, 2)
+        % Update plotting
+        for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
+            obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
+            obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
+            obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
+        end
+    end
+
+    % Communications geometry edges
+    if obj.plotCommsGeometry
+        for jj = 1:size(obj.commsGeometry.lines, 2)
             for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
                 obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
                 obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
                 obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
             end
         end
-
-        % Communications geometry edges
-        if obj.plotCommsGeometry
-            for jj = 1:size(obj.commsGeometry.lines, 2)
-                for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
-                    obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
-                    obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
-                    obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
-                end
-            end
-        end
     end
 
-    % FOV cone: recompute full mesh whenever position or orientation changes
-    if ~isempty(obj.fovGeometry.surface)
-        % Sync fovGeometry state to current agent position and sensor orientation
-        obj.fovGeometry = obj.fovGeometry.initialize( ...
-            obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
-            obj.fovGeometry.tag, obj.fovGeometry.label, ...
-            obj.sensorModel.tilt, obj.sensorModel.azimuth);
-
-        % Recompute cone mesh (mirrors cone.plot logic)
-        maxAlt = obj.fovGeometry.surface(1).Parent.ZLim(2);
-        scalingFactor = maxAlt / obj.fovGeometry.height;
-        [X, Y, Z] = cylinder([scalingFactor * obj.fovGeometry.radius, 0], obj.fovGeometry.n);
-        Z = Z * maxAlt;
-        Ry = [cosd(obj.fovGeometry.tilt), 0, -sind(obj.fovGeometry.tilt); 0, 1, 0; sind(obj.fovGeometry.tilt), 0, cosd(obj.fovGeometry.tilt)];
-        Rz = [sind(obj.fovGeometry.azimuth), -cosd(obj.fovGeometry.azimuth), 0; cosd(obj.fovGeometry.azimuth), sind(obj.fovGeometry.azimuth), 0; 0, 0, 1];
-        R  = Rz * Ry;
-        pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
-        X = reshape(pts(1, :), size(X)) + obj.pos(1);
-        Y = reshape(pts(2, :), size(Y)) + obj.pos(2);
-        Z = reshape(pts(3, :) + maxAlt, size(Z)) + obj.pos(3) - maxAlt;
-
-        for jj = 1:size(obj.fovGeometry.surface, 2)
-            obj.fovGeometry.surface(jj).XData = X;
-            obj.fovGeometry.surface(jj).YData = Y;
-            obj.fovGeometry.surface(jj).ZData = Z;
-        end
+    % Update FOV geometry surfaces
+    for jj = 1:size(obj.fovGeometry.surface, 2)
+        % Update each plot
+        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
+        obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
+        obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
+        obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
     end
 end

@miSim/constrainMotion.m

@@ -1,226 +1,149 @@
 function [obj] = constrainMotion(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    nAgents = size(obj.agents, 1);
+    if size(obj.agents, 1) < 2
+        nAAPairs = 0;
+    else
+        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
+    end
 
-    % Compute current velocity and desired control input
-    v = zeros(nAgents, 3);        % current velocity (for drift term in DI mode)
-    u_desired = zeros(nAgents, 3); % desired control: velocity (SI) or acceleration (DI)
-    for ii = 1:nAgents
-        if obj.useDoubleIntegrator
-            v(ii, :) = obj.agents{ii}.lastVel;
-            u_desired(ii, :) = (obj.agents{ii}.vel - obj.agents{ii}.lastVel) / obj.timestep;
-        else
-            v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
-            u_desired(ii, :) = v(ii, :);
-        end
-    end
-    if ~obj.useDoubleIntegrator && (all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all"))
-        % Single-integrator: agents are not attempting to move
-        return;
-    end
-    if obj.useDoubleIntegrator && all(u_desired == 0, "all") && all(v == 0, "all")
-        % Double-integrator: no desired acceleration and no existing velocity
+    agents = [obj.agents{:}];
+    v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
+    if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all')
+        % Agents are not attempting to move, so there is no motion to be
+        % constrained
         return;
     end
 
     % Initialize QP based on number of agents and obstacles
+    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
+    nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
+    nLNAPairs = sum(obj.constraintAdjacencyMatrix, 'all') - size(obj.agents, 1);
+    total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
     kk = 1;
-    A = zeros(obj.numBarriers, 3 * nAgents);
-    b = zeros(obj.numBarriers, 1);
+    A = zeros(total, 3 * size(obj.agents, 1));
+    b = zeros(total, 1);
 
     % Set up collision avoidance constraints
-    h = NaN(nAgents, nAgents);
-    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}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
+    h = NaN(size(obj.agents, 1));
+    h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
+    for ii = 1:(size(obj.agents, 1) - 1)
+        for jj = (ii + 1):size(obj.agents, 1)
+            h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
             h(jj, ii) = h(ii, jj);
             
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (agents(ii).pos - agents(jj).pos);
             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
-            r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius;
-            v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
-            hMin = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent);
-            if norm(A(kk, :)) < 1e-9
-                % Agents are coincident: A-row is zero, so b < 0 would make
-                % 0 ≤ b unsatisfiable. Fall back to b = 0 (no correction possible).
-                b(kk) = 0;
-            else
-                b(kk) = obj.barrierGain * max(hMin, h(ii, jj))^obj.barrierExponent;
-            end
+            b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent;
             kk = kk + 1;
         end
     end
 
-    idx = length(h(triu(true(size(h)), 1)));    
-    if coder.target('MATLAB')
-	obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
-    end
-    idx = idx + 1;
-
-    hObs = NaN(nAgents, size(obj.obstacles, 1));
+    hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
     % Set up obstacle avoidance constraints
-    for ii = 1:nAgents
+    for ii = 1:size(obj.agents, 1)
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
+            cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
 
-            hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+            hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
 
-            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;
-            hMin = -(2 * r_i * v_max_i / obj.barrierGain)^(1 / obj.barrierExponent);
-            b(kk) = obj.barrierGain * max(hMin, hObs(ii, jj))^obj.barrierExponent;
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
+            b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
            
             kk = kk + 1;
         end
     end
     
-    if coder.target('MATLAB')
-	obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
-    end
-    idx = idx + numel(hObs);
-
     % Set up domain constraints (walls and ceiling only)
     % Floor constraint is implicit with an obstacle corresponding to the
     % minimum allowed altitude, but I included it anyways
-    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
+    for ii = 1:size(obj.agents, 1)
         % X minimum
-        h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
-        b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent;
         kk = kk + 1;
     
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
-        b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent;
         kk = kk + 1;
     
         % Y minimum
-        h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
-        b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent;
         kk = kk + 1;
     
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
-        b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent;
         kk = kk + 1;
     
-        % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
-        h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
+        % Z minimum
+        h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
-        b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
         kk = kk + 1;
     
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
-        b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
+        b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent;
         kk = kk + 1;
-
-        if coder.target('MATLAB')
-	    obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-        end
-	idx = idx + 6;
     end
 
+    % Save off h function values (ignoring network constraints which may evolve in time)
+    obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; 
+
     % Add communication network constraints
-    hComms = NaN(nAgents, nAgents);
-    hComms(logical(eye(nAgents))) = 0;
-    for ii = 1:(nAgents - 1)
-        for jj = (ii + 1):nAgents
+    hComms = NaN(size(obj.agents, 1));
+    hComms(logical(eye(size(obj.agents, 1)))) = 0;
+    for ii = 1:(size(obj.agents, 1) - 1)
+        for jj = (ii + 1):size(obj.agents, 1)
             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}.lastPos - obj.agents{jj}.lastPos)^2;
+                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
 
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-
-                % One-step forward invariance: b = h/dt ensures h cannot
-                % go negative in a single timestep (linear approximation)
-                v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
-                hMin = -4 * r_comms * v_max_ij * obj.timestep;
-                if norm(A(kk, :)) < 1e-9
-                    b(kk) = 0;
-                else
-                    b(kk) = max(hMin, hComms(ii, jj)) / obj.timestep;
-                end
+                b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent;
 
                 kk = kk + 1; 
             end
         end
     end
 
-    if coder.target('MATLAB')
-	obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
-    end
-
-    % Double-integrator: transform QP from velocity to acceleration space.
-    % Single-integrator constraint: A * v <= b
-    % Double-integrator: A * a <= (b - A * v_current) / dt
-    if obj.useDoubleIntegrator
-        v_flat = reshape(v', 3 * nAgents, 1);
-        b = (b - A * v_flat) / obj.timestep;
-    end
-
-    % Solve QP: minimize ||u - u_desired||²
-    uhat = reshape(u_desired', 3 * nAgents, 1);
-    H = 2 * eye(3 * nAgents);
-    f = -2 * uhat;
+    % Solve QP program generated earlier
+    vhat = reshape(v', 3 * size(obj.agents, 1), 1);
+    H = 2 * eye(3 * size(obj.agents, 1));
+    f = -2 * vhat;
     
     % Update solution based on constraints
-    if coder.target('MATLAB')
-        assert(size(A,2) == size(H,1))
-        assert(size(A,1) == size(b,1))
-        assert(size(H,1) == length(f))
-    end
-    opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
-    x0 = zeros(size(H, 1), 1);
-    [uNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
-    uNew = reshape(uNew, 3, nAgents)';
+    assert(size(A,2) == size(H,1))
+    assert(size(A,1) == size(b,1))
+    assert(size(H,1) == length(f))
+    opt = optimoptions('quadprog', 'Display', 'off');
+    [vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
+    assert(exitflag == 1, sprintf('quadprog failure... %s%s', newline, m.message));
+    vNew = reshape(vNew, 3, size(obj.agents, 1))';
 
-    if exitflag < 0
-        % Infeasible or other hard failure: hold all agents at current positions
-        if coder.target('MATLAB')
-            warning("QP infeasible (exitflag=%d), holding positions.", int16(exitflag));
-        else
-            fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
-        end
-        uNew = zeros(nAgents, 3);
-    elseif exitflag == 0
-        % Max iterations exceeded: use suboptimal solution already in uNew
-        if coder.target('MATLAB')
-            warning("QP max iterations exceeded, using suboptimal solution.");
-        else
-            fprintf("[constrainMotion] QP max iterations exceeded, using suboptimal solution\n");
-        end
+    if exitflag <= 0
+        warning("QP failed, continuing with unconstrained solution...")
+        vNew = v;
     end
 
-    % Update agent state using the constrained control input
-    for ii = 1:size(uNew, 1)
-        if obj.useDoubleIntegrator
-            % uNew is constrained acceleration
-            obj.agents{ii}.vel = obj.agents{ii}.lastVel + uNew(ii, :) * obj.timestep;
-            obj.agents{ii}.pos = obj.agents{ii}.lastPos + obj.agents{ii}.vel * obj.timestep;
-        else
-            % uNew is constrained velocity
-            obj.agents{ii}.pos = obj.agents{ii}.lastPos + uNew(ii, :) * obj.timestep;
-        end
+    % Update the "next position" that was previously set by unconstrained
+    % GA using the constrained solution produced here
+    for ii = 1:size(vNew, 1)
+        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
     end
 
     % Here we run this at the simulation level, but in reality there is no

@miSim/initialize.m

@@ -1,6 +1,6 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
         agents (:, 1) cell;
         barrierGain (1, 1) double = 100;
@@ -11,33 +11,23 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
         makePlots(1, 1) logical = true;
         makeVideo (1, 1) logical = true;
-        useDoubleIntegrator (1, 1) logical = false;
-        dampingCoeff (1, 1) double = 2.0;
-        useFixedTopology (1, 1) logical = false;
-        optimizeSensorPointing (1, 1) logical = false;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % enable/disable plotting and video writer
     obj.makePlots = makePlots;
     if ~obj.makePlots
         if makeVideo
-            if coder.target('MATLAB')
-                warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
-            end
+            warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
             makeVideo = false;
         end
     end
     obj.makeVideo = makeVideo;
 
     % Generate artifact(s) name
-    if coder.target('MATLAB')
-        obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
-    else
-        obj.artifactName = ""; % Generate no artifacts from simulation in codegen
-    end
+    obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
 
     % Define simulation time parameters
     obj.timestep = timestep;
@@ -47,24 +37,14 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Define domain
     obj.domain = domain;
 
-    % Add geometries representing obstacles within the domain, pre-allocating
-    % one extra slot for the minimum altitude floor obstacle if needed
-    numInputObs = size(obstacles, 1);
-    if minAlt > 0
-        obj.obstacles = repmat({rectangularPrism}, numInputObs + 1, 1);
-    else
-        obj.obstacles = repmat({rectangularPrism}, numInputObs, 1);
-    end
-    for kk = 1:numInputObs
-        obj.obstacles{kk} = obstacles{kk};
-    end
+    % Add geometries representing obstacles within the domain
+    obj.obstacles = obstacles;
 
     % Add an additional obstacle spanning the domain's footprint to 
     % represent the minimum allowable altitude
     if minAlt > 0
-        minAltObstacle = rectangularPrism;
-        minAltObstacle = minAltObstacle.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
-        obj.obstacles{numInputObs + 1} = minAltObstacle;
+        obj.obstacles{end + 1, 1} = rectangularPrism;
+        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
     end
 
     % Define agents
@@ -76,78 +56,42 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     for ii = 1:size(obj.agents, 1)
         % Agent
         if isempty(char(obj.agents{ii}.label))
-            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
+            obj.agents{ii}.label = sprintf("Agent %d", ii);
         end
 
         % Collision geometry
         if isempty(char(obj.agents{ii}.collisionGeometry.label))
-            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", ii);
         end
     end
 
     % Set CBF parameters
     obj.barrierGain = barrierGain;
     obj.barrierExponent = barrierExponent;
-    obj.minAlt = minAlt;
 
-    % Set dynamics model
-    obj.useDoubleIntegrator = useDoubleIntegrator;
-    obj.dampingCoeff = dampingCoeff;
-    obj.useFixedTopology = useFixedTopology;
-    obj.optimizeSensorPointing = optimizeSensorPointing;
-
-    % Compute adjacency matrix and network topology
+    % Compute adjacency matrix and lesser neighbors
     obj = obj.updateAdjacency();
-    if obj.useFixedTopology
-        obj.constraintAdjacencyMatrix = obj.adjacency;
-    else
-        obj = obj.lesserNeighbor();
-    end
+    obj = obj.lesserNeighbor();
 
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
 
-    if coder.target('MATLAB')
-        % 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 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)];
 
-    end
+    % 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));
 
     % Create initial partitioning
     obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
-    % Determine number of barrier functions that will be necessary
-    if size(obj.agents, 1) < 2
-        nAAPairs = 0;
-    else
-        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
-    end
-    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
-    nADPairs = size(obj.agents, 1) * 6; % agents x (4 walls + 1 floor + 1 ceiling)
-    nLNAPairs = sum(triu(obj.constraintAdjacencyMatrix, 1), "all");
-    obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
+    % Initialize variable that will store agent positions for trail plots
+    obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
+    obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
 
-    if coder.target('MATLAB')
-        % Initialize variable that will store agent positions for trail plots
-        obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
-        obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
+    % Set up plots showing initialized state
+    obj = obj.plot();
 
-        % Initialize velocity history (zeros at t=0, all agents start at rest)
-        obj.velHist = zeros(size(obj.agents, 1), obj.maxIter + 1, 3);
-
-        % 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();
-
-        % Run validations
-        obj.validate();
-    end
+    % Run validations
+    obj.validate();
 end

@miSim/initializeFromCsv.m

@@ -1,158 +0,0 @@
-function obj = initializeFromCsv(obj, csvPath)
-% INITIALIZEFROMCSV  Initialize miSim from an AERPAW scenario CSV file.
-%
-% Reads all guidance parameters, domain geometry, initial UAV positions,
-% and obstacle definitions from the CSV, then builds and initialises the
-% simulation. Ends by calling the standard obj.initialize(...) method.
-%
-% This is the MATLAB-path counterpart to the compiled path that unpacks a
-% flat scenarioParams array in guidance_step.m.  It is only ever called
-% from within a coder.target('MATLAB') guard and is never compiled.
-%
-% Usage (inside guidance_step.m on MATLAB path):
-%   sim = sim.initializeFromCsv('aerpaw/config/scenario.csv');
-%
-% Expected CSV columns (see scenario.csv):
-%   timestep, maxIter, minAlt, discretizationStep, protectedRange,
-%   initialStepSize, barrierGain, barrierExponent,
-%   collisionRadius (per-UAV), comRange (per-UAV),
-%   alphaDist (per-UAV), betaDist (per-UAV),
-%   alphaTilt (per-UAV), betaTilt (per-UAV),
-%   domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"),
-%   objectiveVar ("v11,v12,v21,v22"), sensorPerformanceMinimum,
-%   initialPositions (flat "x1,y1,z1, x2,y2,z2,..."),
-%   numObstacles, obstacleMin (flat), obstacleMax (flat)
-
-arguments (Input)
-    obj     (1, 1) {mustBeA(obj, 'miSim')};
-    csvPath (1, 1) string;
-end
-arguments (Output)
-    obj (1, 1) {mustBeA(obj, 'miSim')};
-end
-
-% ---- Parse CSV via readScenarioCsv ---------------------------------------
-scenario = obj.readScenarioCsv(csvPath);
-
-TIMESTEP            = scenario.timestep;
-MAX_ITER            = scenario.maxIter;
-MIN_ALT             = scenario.minAlt;
-DISCRETIZATION_STEP = scenario.discretizationStep;
-PROTECTED_RANGE     = scenario.protectedRange;
-INITIAL_STEP_SIZE   = scenario.initialStepSize;
-BARRIER_GAIN        = scenario.barrierGain;
-BARRIER_EXPONENT    = scenario.barrierExponent;
-% Per-UAV parameters (vectors with one element per UAV)
-COLLISION_RADIUS_VEC = scenario.collisionRadius;   % 1×N
-COMMS_RANGE_VEC      = scenario.comRange;           % 1×N
-ALPHA_DIST_VEC       = scenario.alphaDist;          % 1×N
-BETA_DIST_VEC        = scenario.betaDist;           % 1×N
-ALPHA_TILT_VEC       = scenario.alphaTilt;          % 1×N
-BETA_TILT_VEC        = scenario.betaTilt;           % 1×N
-
-DOMAIN_MIN                 = scenario.domainMin;                % 1×3
-DOMAIN_MAX                 = scenario.domainMax;                % 1×3
-
-% objectivePos: 2 values per Gaussian component (1 or 2 components supported)
-nObjComponents = numel(scenario.objectivePos) / 2;
-assert(mod(numel(scenario.objectivePos), 2) == 0, ...
-    'objectivePos must have an even number of values (2 per Gaussian component)');
-assert(nObjComponents >= 1 && nObjComponents <= 2, ...
-    'At most 2 objective Gaussian components supported; got %d', nObjComponents);
-assert(numel(scenario.objectiveVar) == nObjComponents * 4, ...
-    'objectiveVar must have %d values for %d component(s); got %d', ...
-    nObjComponents * 4, nObjComponents, numel(scenario.objectiveVar));
-OBJECTIVE_GROUND_POS = reshape(scenario.objectivePos, 2, nObjComponents)';           % nObj×2
-OBJECTIVE_VAR        = permute(reshape(scenario.objectiveVar, 2, 2, nObjComponents), [3, 1, 2]); % nObj×2×2
-
-SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
-
-% Initial UAV positions: flat vector reshaped to N×3
-flatPos = scenario.initialPositions;          % 1×(3*N)
-assert(mod(numel(flatPos), 3) == 0, ...
-    "initialPositions must have a multiple of 3 values; got %d", numel(flatPos));
-positions  = reshape(flatPos, 3, [])';        % N×3
-numAgents  = size(positions, 1);
-
-% Validate per-UAV parameter lengths match numAgents
-assert(numel(COLLISION_RADIUS_VEC) == numAgents, ...
-    "collisionRadius has %d values but expected %d (one per UAV)", numel(COLLISION_RADIUS_VEC), numAgents);
-assert(numel(COMMS_RANGE_VEC) == numAgents, ...
-    "comRange has %d values but expected %d (one per UAV)", numel(COMMS_RANGE_VEC), numAgents);
-assert(numel(ALPHA_DIST_VEC) == numAgents, ...
-    "alphaDist has %d values but expected %d (one per UAV)", numel(ALPHA_DIST_VEC), numAgents);
-assert(numel(BETA_DIST_VEC) == numAgents, ...
-    "betaDist has %d values but expected %d (one per UAV)", numel(BETA_DIST_VEC), numAgents);
-assert(numel(ALPHA_TILT_VEC) == numAgents, ...
-    "alphaTilt has %d values but expected %d (one per UAV)", numel(ALPHA_TILT_VEC), numAgents);
-assert(numel(BETA_TILT_VEC) == numAgents, ...
-    "betaTilt has %d values but expected %d (one per UAV)", numel(BETA_TILT_VEC), numAgents);
-
-numObstacles = scenario.numObstacles;
-
-% Dynamics model (optional columns — backward compatible with older CSVs)
-if isfield(scenario, 'useDoubleIntegrator')
-    USE_DOUBLE_INTEGRATOR = logical(scenario.useDoubleIntegrator);
-else
-    USE_DOUBLE_INTEGRATOR = false;
-end
-if isfield(scenario, 'dampingCoeff')
-    DAMPING_COEFF = scenario.dampingCoeff;
-else
-    DAMPING_COEFF = 2.0;
-end
-if isfield(scenario, 'useFixedTopology')
-    USE_FIXED_TOPOLOGY = logical(scenario.useFixedTopology);
-else
-    USE_FIXED_TOPOLOGY = false;
-end
-
-% ---- Build domain --------------------------------------------------------
-dom = rectangularPrism;
-dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
-
-% ---- Build sensing objective (MATLAB path: objectiveFunctionWrapper) -----
-dom.objective = sensingObjective;
-objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR);
-dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
-
-% ---- Initialise agents from scenario positions ---------------------------
-% Each agent gets its own sensor model and collision/comms radii from the
-% per-UAV parameter vectors.
-agentList = cell(numAgents, 1);
-for ii = 1:numAgents
-    pos  = positions(ii, :);
-
-    % Per-UAV sensor model
-    sensor = sigmoidSensor;
-    sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
-                               ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
-
-    geom = spherical;
-    geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
-                           sprintf("UAV %d Collision", ii));
-    ag = agent;
-    ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
-                       INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
-    agentList{ii} = ag;
-end
-
-% ---- Build obstacles from CSV --------------------------------------------
-obstacleList = cell(numObstacles, 1);
-if numObstacles > 0
-    obsMin = reshape(scenario.obstacleMin, 3, numObstacles)';  % N×3
-    obsMax = reshape(scenario.obstacleMax, 3, numObstacles)';
-    for ii = 1:numObstacles
-        obs = rectangularPrism;
-        obs = obs.initialize([obsMin(ii, :); obsMax(ii, :)], ...
-                             REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
-        obstacleList{ii} = obs;
-    end
-end
-
-% ---- Initialise simulation (plots and video disabled) --------------------
-obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
-                     MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
-
-end

@miSim/initializeFromInits.m

@@ -1,87 +0,0 @@
-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/lesserNeighbor.m

@@ -1,56 +1,50 @@
 function obj = lesserNeighbor(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % initialize solution with self-connections only
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
-    nAgents = size(obj.agents, 1);
-    for ii = 1:nAgents
+    for ii = 1:size(obj.agents, 1)
         % Find lesser neighbors of each agent
         % Lesser neighbors of ii are jj < ii in range of ii
-        lnBuf = zeros(1, nAgents);
-        lnCount = 0;
+        lesserNeighbors = [];
         for jj = 1:(ii - 1)
             if obj.adjacency(ii, jj)
-                lnCount = lnCount + 1;
-                lnBuf(lnCount) = jj;
+                lesserNeighbors = [lesserNeighbors, jj];
             end
         end
-        obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
+        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
         
         % Early exit for isolated agents
-        if lnCount == 0
+        if isempty(obj.agents{ii}.lesserNeighbors)
             continue
         end
 
         % Focus on subgraph defined by lesser neighbors
         subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
 
-        % Find connected components; store only the max global index per
-        % component (the only value needed below) to avoid a cell array
-        visited = false(1, lnCount);
-        maxInComponent = zeros(1, lnCount);
-        numComponents = 0;
-        for jj = 1:lnCount
+        % Find connected components in each agent's subgraph
+        % TODO: rewrite this using matlab "conncomp" function?
+        visited = false(size(subgraphAdjacency, 1), 1);
+        components = {};
+        for jj = 1:size(subgraphAdjacency, 1)
             if ~visited(jj)
                 reachable = bfs(subgraphAdjacency, jj);
                 visited(reachable) = true;
-                numComponents = numComponents + 1;
-                maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable));
+                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
             end
         end
 
         % Connect to the greatest index in each connected component in the
         % lesser neighborhood of this agent
-        for jj = 1:numComponents
-            maxIdx = maxInComponent(jj);
-            constraintAdjacencyMatrix(ii, maxIdx) = true;
-            constraintAdjacencyMatrix(maxIdx, ii) = true;
+        for jj = 1:size(components, 2)
+            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
+            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
         end
     end
     obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
@@ -59,34 +53,24 @@ end
 function cComp = bfs(subgraphAdjacency, startIdx)
     n = size(subgraphAdjacency, 1);
     visited = false(1, n);
-
-    % Pre-allocated queue and component buffer with head/tail pointers
-    % to avoid element deletion and dynamic array growth
-    queue    = zeros(1, n);
-    cCompBuf = zeros(1, n);
-    qHead = 1;
-    qTail = 2;
-    queue(1)    = startIdx;
-    cCompBuf(1) = startIdx;
-    cSize = 1;
+    queue = startIdx;
+    cComp = startIdx;
     visited(startIdx) = true;
     
-    while qHead < qTail
-        current = queue(qHead);
-        qHead = qHead + 1;
+    while ~isempty(queue)
+        current = queue(1);
+        queue(1) = [];
         
         % Find all neighbors of current node in the subgraph
         neighbors = find(subgraphAdjacency(current, :));
-        for kk = 1:numel(neighbors)
-            neighbor = neighbors(kk);
+        
+        for neighbor = neighbors
             if ~visited(neighbor)
                 visited(neighbor) = true;
-                cCompBuf(cSize + 1) = neighbor;
-                cSize = cSize + 1;
-                queue(qTail) = neighbor;
-                qTail = qTail + 1;
+                cComp = [cComp, neighbor];
+                queue = [queue, neighbor];
             end
         end
     end
-    cComp = sort(cCompBuf(1:cSize));
+    cComp = sort(cComp);
 end

@miSim/miSim.m

@@ -2,33 +2,23 @@ classdef miSim
     % multiagent interconnection simulation
 
     % Simulation parameters
-    properties (SetAccess = public, GetAccess = public)
+    properties (SetAccess = private, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
-        domain;
-        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
-        constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections
-        partitioning = zeros(0, 0);
+        domain = rectangularPrism;
+        objective = sensingObjective;
+        obstacles = cell(0, 1); % geometries that define obstacles within the domain
+        agents = cell(0, 1); % agents that move within the domain
+        adjacency = NaN; % Adjacency matrix representing communications network graph
+        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
+        partitioning = NaN;
         perf; % sensor performance timeseries array
         performance = 0; % simulation performance timeseries vector
-        barrierGain = NaN; % CBF gain parameter
-        barrierExponent = NaN; % CBF exponent parameter
-        minAlt = 0; % minimum allowable altitude (m)
-        useDoubleIntegrator = false; % false = single-integrator, true = double-integrator dynamics
-        dampingCoeff = 2.0; % velocity-proportional damping for double-integrator mode
-        useFixedTopology = false; % false = lesser neighbor (dynamic), true = fixed initial topology
-        optimizeSensorPointing = false; % false = fixed sensor tilt/azimuth, true = optimize tilt/azimuth via gradient ascent
+        barrierGain = 100; % CBF gain parameter
+        barrierExponent = 3; % CBF exponent parameter
         artifactName = "";
-        f; % main plotting tiled layout figure
         fPerf; % performance plot figure
-        % Indicies for various plot types in the main tiled layout figure
-        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)
@@ -40,21 +30,23 @@ classdef miSim
         % Plot objects
         makePlots = true; % enable/disable simulation plotting (performance implications)
         makeVideo = true; % enable/disable VideoWriter (performance implications)
+        f; % main plotting tiled layout figure
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
         partitionPlot; % objects for partition plot
         performancePlot; % objects for sensor performance plot
 
         posHist; % data for trail plot
-        velHist; % velocity history (double-integrator mode)
         trailPlot; % objects for agent trail plot
 
         % Indicies for various plot types in the main tiled layout figure
+        spatialPlotIndices = [6, 4, 3, 2];
         objectivePlotIndices = [6, 4];
         networkGraphIndex = 5;
         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
@@ -62,18 +54,7 @@ classdef miSim
     end
 
     methods (Access = public)
-        function obj = miSim()
-            arguments (Output)
-                obj (1, 1) miSim
-            end
-            obj.domain = rectangularPrism;
-            obj.obstacles = {rectangularPrism};
-            obj.agents = {agent};
-        end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology);
-        [obj] = initializeFromCsv(obj, csvPath);
-        [obj] = initializeFromInits(obj, initsPath);
-        [obj] = plotFromSimHist(obj, initsPath, histPath);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
         [obj] = constrainMotion(obj);
@@ -86,9 +67,8 @@ classdef miSim
         [obj] = plotTrails(obj);
         [obj] = plotH(obj);
         [obj] = updatePlots(obj);
-        [obj] = teardown(obj);
-        writeInits(obj);
         validate(obj);
+        teardown(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/plot.m

@@ -1,9 +1,9 @@
 function obj = plot(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     
     % fast exit when plotting is disabled

@miSim/plotConnections.m

@@ -1,9 +1,9 @@
 function obj = plotConnections(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % Iterate over lower triangle off-diagonal region of the 
@@ -23,11 +23,11 @@ function obj = plotConnections(obj)
     % Plot the connections
     if isnan(obj.spatialPlotIndices)
         hold(obj.f.CurrentAxes, "on");
-        o = plot3(obj.f.CurrentAxes, X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
+        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
         hold(obj.f.CurrentAxes, "off");
     else
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
+        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
     end
 

@miSim/plotFromSimHist.m

@@ -1,93 +0,0 @@
-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/plotGraph.m

@@ -1,23 +1,23 @@
 function obj = plotGraph(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % Form graph from adjacency matrix
-    G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
+    G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
 
     % Plot graph object
     if isnan(obj.networkGraphIndex)
-        hold(obj.f.CurrentAxes, "on");
-        o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
-        hold(obj.f.CurrentAxes, "off");
+        hold(obj.f.CurrentAxes, 'on');
+        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.CurrentAxes, 'off');
     else
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "on");
-        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
+        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
         if size(obj.networkGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];

@miSim/plotH.m

@@ -1,69 +1,60 @@
 function obj = plotH(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        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");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.barriers(1:nCA, :)');
+    obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
     legendStrings = [];
     for ii = 2:size(obj.agents, 1)
         for jj = 1:(ii - 1)
             legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
         end
     end
-    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
+    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(2), "");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.barriers((nCA + 1):(nCA + nObs), :)');
+    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)), :)');
     legendStrings = [];
     for ii = 1:size(obj.obstacles, 1)
         for jj = 1:size(obj.agents, 1)
             legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
         end
     end
-    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
+    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.barriers((nCA + nObs + 1):(nCA + nObs + nDom), :)');
-    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");
+    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)');
+    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], 'Location', 'bestoutside');
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
     title(obj.hf.Children(1).Children(1), "Communications");
     % skipped this for now because it is very complicated
 

@miSim/plotPartitions.m

@@ -1,19 +1,19 @@
 function obj = plotPartitions(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     if isnan(obj.partitionGraphIndex)
-        hold(obj.f.CurrentAxes, "on");
+        hold(obj.f.CurrentAxes, 'on');
         o = imagesc(obj.f.CurrentAxes, obj.partitioning);
-        hold(obj.f.CurrentAxes, "off");
+        hold(obj.f.CurrentAxes, 'off');
     else
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "on");
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
         o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "off");
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
         if size(obj.partitionGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];

@miSim/plotPerformance.m

@@ -1,9 +1,9 @@
 function obj = plotPerformance(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % fast exit when plotting is disabled
@@ -15,31 +15,31 @@ function obj = plotPerformance(obj)
 
     axes(obj.fPerf);
     title(obj.fPerf.Children(1), "Sensor Performance");
-    xlabel(obj.fPerf.Children(1), "Time (s)");
-    ylabel(obj.fPerf.Children(1), "Sensor Performance");
-    grid(obj.fPerf.Children(1), "on");
+    xlabel(obj.fPerf.Children(1), 'Time (s)');
+    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
+    grid(obj.fPerf.Children(1), 'on');
 
     % Plot current cumulative performance
-    hold(obj.fPerf.Children(1), "on");
+    hold(obj.fPerf.Children(1), 'on');
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    warning("off", "MATLAB:gui:array:InvalidArrayShape"); % suppress this warning to avoid polluting output
+    warning('off', 'MATLAB:gui:array:InvalidArrayShape'); % suppress this warning to avoid polluting output
     o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
     o.YData = [0, NaN(1, obj.maxIter - 1)];
-    hold(obj.fPerf.Children(1), "off");
+    hold(obj.fPerf.Children(1), 'off');
 
     % Plot current agent performance
     for ii = 1:(size(obj.perf, 1) - 1)
-        hold(obj.fPerf.Children(1), "on");
+        hold(obj.fPerf.Children(1), 'on');
         o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
         o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
         o(end).YData = [0, NaN(1, obj.maxIter - 1)];
-        hold(obj.fPerf.Children(1), "off");
+        hold(obj.fPerf.Children(1), 'off');
     end
 
     % Add legend
-    agentStrings = string(cellfun(@(x) x.label, obj.agents, "UniformOutput", false));
+    agentStrings = string(cellfun(@(x) x.label, obj.agents, 'UniformOutput', false));
     agentStrings = ["Total"; agentStrings];
-    legend(obj.fPerf.Children(1), agentStrings, "Location", "northwest");
+    legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
 
     obj.performancePlot = o;
 end

@miSim/plotTrails.m

@@ -1,9 +1,9 @@
 function obj = plotTrails(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")}
+        obj (1, 1) {mustBeA(obj, 'miSim')}
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")}
+        obj (1, 1) {mustBeA(obj, 'miSim')}
     end
     
     % fast exit when plotting is disabled
@@ -14,9 +14,9 @@ function obj = plotTrails(obj)
     % Plot full range of position history on each spatial plot axes
     o = [];
     for ii = 1:(size(obj.posHist, 1))
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), "Color", "k", "LineWidth", 1)];
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'on');
+        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), 'Color', 'k', 'LineWidth', 1)];
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
     end
 
     % Copy to other plots

@miSim/run.m

@@ -1,98 +1,66 @@
 function [obj] = run(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    if coder.target('MATLAB')
-        % Start video writer
-        if obj.makeVideo
-            v = obj.setupVideoWriter();
-            drawnow;
-            v.open();
-            % Capture reference frame size; used to resize frames that deviate
-            % due to figure reflow during plot updates (e.g. in headless mode).
-            I_ref = getframe(obj.f);
-            v.writeVideo(I_ref);
-            videoFrameSize = [size(I_ref.cdata, 2), size(I_ref.cdata, 1)];
-        end
+    % Start video writer
+    if obj.makeVideo
+        v = obj.setupVideoWriter();
+        v.open();
     end
     
     for ii = 1:size(obj.times, 1)
         % Display current sim time
         obj.t = obj.times(ii);
         obj.timestepIndex = ii;
-        if coder.target('MATLAB')
-            fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
+        fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
-            % Validate current simulation configuration
-            obj.validate();
-        end
-
-        % Clear RF sensor caches
-        if isa(obj.agents{1}.sensorModel, "rfSensor")
-            for ss = 1:size(obj.agents, 1)
-                obj.agents{ss}.sensorModel = obj.agents{ss}.sensorModel.clearRssCache;
-            end
-        end
+        % Before moving
+        % Validate current simulation configuration
+        obj.validate();
 
         % Update partitioning before moving (this one is strictly for
         % plotting purposes, the real partitioning is done by the agents)
-        [obj.partitioning, obj.agents] = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+        obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
         % Determine desired communications links
-        if ~obj.useFixedTopology
-            obj = obj.lesserNeighbor();
-        end
-
-        % Log constraint adjacency for this timestep
-        if coder.target('MATLAB')
-            obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
-        end
+        obj = obj.lesserNeighbor();
 
         % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep, obj.optimizeSensorPointing, obj.agents([1:(jj - 1), (jj + 1):size(obj.agents, 1)]));
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
         end
 
         % Adjust motion determined by unconstrained gradient ascent using
         % CBF constraints solved by QP
         obj = constrainMotion(obj);
 
-        if coder.target('MATLAB')
-            % Update agent position and velocity history arrays
-            obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
-            obj.velHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.vel, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
+        % After moving
+        % Update agent position history array
+        obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
 
-            % Update total performance
-            obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
+        % Update total performance
+        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
 
-            % Update adjacency matrix
-            obj = obj.updateAdjacency();
+        % Update adjacency matrix
+        obj = obj.updateAdjacency();
 
-            % Update plots
-            obj = obj.updatePlots();
+        % Update plots
+        obj = obj.updatePlots();
 
-            % Write frame in to video
-            if obj.makeVideo
-                I = getframe(obj.f);
-                if size(I.cdata, 2) ~= videoFrameSize(1) || size(I.cdata, 1) ~= videoFrameSize(2)
-                    I.cdata = imresize(I.cdata, [videoFrameSize(2), videoFrameSize(1)]);
-                end
-                v.writeVideo(I);
-            end
-        end
-    end
-
-    % Close video
-    if coder.target('MATLAB')
+        % Write frame in to video
         if obj.makeVideo
-            % Close video file
-            v.close();
+            I = getframe(obj.f);
+            v.writeVideo(I);
         end
     end
 
+    if obj.makeVideo
+        % Close video file
+        v.close();
+    end
 end

@miSim/setupVideoWriter.m

@@ -1,15 +1,15 @@
 function v = setupVideoWriter(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        v (1, 1) {mustBeA(v, "VideoWriter")};
+        v (1, 1) {mustBeA(v, 'VideoWriter')};
     end
 
     if ispc || ismac
-        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "MPEG-4");
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
     elseif isunix
-        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "Motion JPEG AVI");
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
     end
     
     v.FrameRate = 1 / obj.timestep;

@miSim/teardown.m

@@ -1,57 +1,13 @@
-function obj = teardown(obj)
+function teardown(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % % Close plots
-    % close(obj.hf);
-    % close(obj.fPerf);
-    % close(obj.f);
-
-    % Log results into matfile
-    histPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist.mat"));
-    out = struct("agent", repmat(struct("pos", [], "vel", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", [], "useDoubleIntegrator", [], "dampingCoeff", [], "useFixedTopology", []);
-
-    out.perf = obj.performance(1:(end - 1));
-    out.barriers = [zeros(size(obj.barriers(1:end, 1), 1), 1), obj.barriers(1:end, 1:(end - 1))];
-    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));
-        out.agent(ii).perf = obj.agents{ii}.performance(1:(end - 2));
-        out.agent(ii).sensor.alphaDist = obj.agents{ii}.sensorModel.alphaDist;
-        out.agent(ii).sensor.betaDist = obj.agents{ii}.sensorModel.betaDist;
-        out.agent(ii).sensor.alphaTilt = obj.agents{ii}.sensorModel.alphaTilt;
-        out.agent(ii).sensor.betaTilt = obj.agents{ii}.sensorModel.betaTilt;
-        out.agent(ii).collisionRadius = obj.agents{ii}.collisionGeometry.radius;
-        out.agent(ii).commsRadius = obj.agents{ii}.commsGeometry.radius;
-    end
-
-    save(histPath, "out");
-
-    % reset parameters
-    obj.timestep = NaN;
-    obj.timestepIndex = NaN;
-    obj.maxIter = NaN;
-    obj.domain = rectangularPrism;
-    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;
-    obj.barrierExponent = NaN;
-    obj.useDoubleIntegrator = false;
-    obj.dampingCoeff = 2.0;
-    obj.useFixedTopology = false;
-    obj.artifactName = "";
+    % Close plots
+    close(obj.hf);
+    close(obj.fPerf);
+    close(obj.f);
 
 end

@miSim/updateAdjacency.m

@@ -1,9 +1,9 @@
 function obj = updateAdjacency(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % Initialize assuming only self-connections

@miSim/updatePlots.m

@@ -1,9 +1,9 @@
 function [obj] = updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
     % Fast exit when plotting is disabled
@@ -61,15 +61,13 @@ function [obj] = updatePlots(obj)
     end
 
     % Update h function plots
-    nCA  = size(obj.caPlot, 1);
-    nObs = size(obj.obsPlot, 1);
-    for ii = 1:nCA
-        obj.caPlot(ii).YData(obj.timestepIndex) = obj.barriers(ii, obj.timestepIndex);
+    for ii = 1:size(obj.caPlot, 1)
+        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
     end
-    for ii = 1:nObs
-        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + ii, obj.timestepIndex);
+    for ii = 1:size(obj.obsPlot, 1)
+        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
     end
     for ii = 1:size(obj.domPlot, 1)
-        obj.domPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + nObs + ii, obj.timestepIndex);
+        obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
     end
 end

@miSim/validate.m

@@ -1,28 +1,27 @@
 function validate(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
+        obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
     end
 
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
-        error("Network is not connected");
+        warning("Network is not connected");
     end
 
-    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
-        error("Eliminated network connections that were necessary");
+    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
+        warning("Eliminated network connections that were necessary");
     end
 
     %% Obstacle Validators
-    % Agent-Obstacle Collision Detection
-    for jj = 1:size(obj.obstacles, 1)
-        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 - 1e-3
-                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
+    AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
+    AO_collisions = vertcat(AO_collisions{:});
+    if any(AO_collisions)
+        [idx, idy] = find(AO_collisions);
+        for ii = 1:size(idx, 1)
+            error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
         end
     end
+
 end

@miSim/writeInits.m

@@ -1,75 +0,0 @@
-function writeInits(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
-    end
-    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);
-    if isprop(obj.agents{1}.sensorModel, "alphaDist")
-        % sigmoidSensor parameters
-        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.betaTilt, obj.agents);
-
-        % others to zero
-        lossExponent = zeros(size(obj.agents));
-        P_TX = zeros(size(obj.agents));
-        BW = zeros(size(obj.agents));
-        f_c = zeros(size(obj.agents));
-        G_RX_dBi = zeros(size(obj.agents));
-        beamwidthExponent = zeros(size(obj.agents));
-        
-    elseif isprop(obj.agents{1}.sensorModel, "P_TX")
-        % rfSensor parameters
-        lossExponent = cellfun(@(x) x.sensorModel.lossExponent, obj.agents);
-        P_TX = cellfun(@(x) x.sensorModel.P_TX, obj.agents);
-        BW = cellfun(@(x) x.sensorModel.BW, obj.agents);
-        f_c = cellfun(@(x) x.sensorModel.f_c, obj.agents);
-        G_RX_dBi = cellfun(@(x) x.sensorModel.G_RX_dBi, obj.agents);
-        beamwidthExponent = cellfun(@(x) x.sensorModel.beamwidthExponent, obj.agents);
-
-        % others to zero
-        alphaDist = zeros(size(obj.agents));
-        betaDist = zeros(size(obj.agents));
-        alphaTilt = zeros(size(obj.agents));
-        betaTilt = zeros(size(obj.agents));
-    end
-    % joint parameters
-    tilt = cellfun(@(x) x.sensorModel.tilt, obj.agents);
-    azimuth = cellfun(@(x) x.sensorModel.azimuth, 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 + 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", numInputObs, ...
-                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
-                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
-                    "tilt", tilt, "azimuth", azimuth, ... % joint sensor parameters
-                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... % sigmoid sensor parameters
-                    "lossExponent", lossExponent, "P_TX", P_TX, "BW", BW, "f_c", f_c, "G_RX_dBi", G_RX_dBi, "beamwidthExponent", beamwidthExponent, ... % RF sensor parameters
-                    ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
-                    "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
-                    "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...
-                    "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
-                    "objectiveIntegral", sum(obj.domain.objective.values(:)));
-
-    % Save all parameters to output file
-    initsFile = strcat(obj.artifactName, "_miSimInits");
-    initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile);
-    save(initsFile, "-struct", "inits");
-end

@miSim/writeParams.m

@@ -0,0 +1,25 @@
+function writeParams(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    % 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);
+    comRange = cellfun(@(x) x.commsGeometry.radius, obj.agents);
+
+    % Combine with simulation parameters
+    params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ...
+                    'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ...
+                    'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange);
+
+    % Save all parameters to output file
+    paramsFile = strcat(obj.artifactName, "_miSimParams");
+    paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile);
+    save(paramsFile, "-struct", "params");
+end

@rfSensor/RSS.m

@@ -1,24 +0,0 @@
-function value = RSS(obj, d, dx, dy, dz)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        d  (:, 1) double;
-        dx (:, 1) double;
-        dy (:, 1) double;
-        dz (:, 1) double;
-    end
-    arguments (Output)
-        value (:, 1) double
-    end
-    % Boresight unit vector: [st*sa, st*ca, -ct]
-    % Target direction unit vector: [dx, dy, dz] / d
-    % cos_theta = dot product of the two, computed without per-point trig.
-    st = sind(obj.tilt);
-    ct = cosd(obj.tilt);
-    sa = sind(obj.azimuth);
-    ca = cosd(obj.azimuth);
-    cos_theta = (st .* (dx .* sa + dy .* ca) - ct .* dz) ./ max(d, eps);
-    cos_theta = max(-1, min(1, cos_theta));
-    theta = acosd(cos_theta);
-    gain  = 10 .* obj.beamwidthExponent .* log10((1 + cosd(theta)) ./ 2);
-    value = obj.P_TX_dBm + gain + obj.G_RX_dBi - obj.pathLoss(d);
-end

@rfSensor/clearRssCache.m

@@ -1,11 +0,0 @@
-function obj = clearRssCache(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-    end
-
-    obj.rssCache = double.empty(0, 1);
-
-end

@rfSensor/computePointToPoints.m

@@ -1,6 +0,0 @@
-function [d, dx, dy, dz] = computePointToPoints(~, agentPos, targetPos)
-    dx = targetPos(:,1) - agentPos(1);
-    dy = targetPos(:,2) - agentPos(2);
-    dz = targetPos(:,3) - agentPos(3);
-    d  = sqrt(dx.^2 + dy.^2 + dz.^2);
-end

@rfSensor/halfAngle.m

@@ -1,23 +0,0 @@
-function value = halfAngle(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-    end
-    arguments (Output)
-        value (1, 1) double;
-    end
-    % Sweep angular offset from boresight by evaluating transmitterGain at
-    % (obj.tilt + dtheta, obj.azimuth). The cosine difference identity guarantees
-    % the resulting angular offset from boresight equals dtheta exactly,
-    % independent of the actual pointing direction.
-    dtheta = (0:0.1:179.9)';
-    gain = obj.transmitterGain(obj.tilt + dtheta, obj.azimuth * ones(size(dtheta)));
-    target = gain(1) - 3;
-    idx = find(gain <= target, 1);
-    if isempty(idx) || idx == 1
-        value = dtheta(end);
-        return;
-    end
-    % Linear interpolation between bracketing samples
-    value = dtheta(idx-1) + (target - gain(idx-1)) * ...
-            (dtheta(idx) - dtheta(idx-1)) / (gain(idx) - gain(idx-1));
-end

@rfSensor/initialize.m

@@ -1,32 +0,0 @@
-function obj = initialize(obj, txPower, bandwidth, centerFreq, rxGain_dBi, beamwidthExponent, tilt, azimuth, lossExponent)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")}
-        txPower (1, 1) double;
-        bandwidth (1, 1) double;
-        centerFreq (1, 1) double;
-        rxGain_dBi (1, 1) double;
-        beamwidthExponent (1, 1) double;
-        tilt (1, 1) double = 0;
-        azimuth (1, 1) double = 0;
-        lossExponent (1, 1) double = NaN;
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, "rfSensor")}
-    end
-
-    %% Provided values
-    obj.P_TX = txPower; % Transmit power (W)
-    obj.BW = bandwidth; % Bandwidth (Hz)
-    obj.f_c = centerFreq; % Center frequency (Hz)
-    obj.G_RX_dBi = rxGain_dBi; % Receiving Antenna Gain (dBi)
-    obj.beamwidthExponent = beamwidthExponent; % Defines how focused the antenna beam is
-    obj.lossExponent = lossExponent;
-
-    % Define initial antenna pointing
-    obj.tilt = tilt;
-    obj.azimuth = azimuth;
-
-    %% Computed values
-    obj.P_TX_dBm = 10*log10(obj.P_TX/1e-3); % Transmit power in dBm
-    obj.N = obj.k_B * obj.T_0 * obj.BW; % Thermal noise
-end

@rfSensor/pathLoss.m

@@ -1,13 +0,0 @@
-function L_FSPL_dB = pathLoss(obj, d)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        d (:, 1) double; % distance from TX to RX
-    end
-    arguments (Output)
-        L_FSPL_dB (:, 1) double
-    end
-
-    % Free Space Path Loss (dB); d clamped away from zero (log undefined at d=0)
-    L_FSPL_dB = obj.lossExponent * 10 * log10(max(d, eps)) + 20 * log10(obj.f_c) + 20 * log10((4*pi)/obj.c);
-
-end

@rfSensor/plot.m

@@ -1,125 +0,0 @@
-function f = plot(obj, altitude, otherSensorsPos, otherSensors)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        altitude (1, 1) double;
-        otherSensorsPos (:, 3) double = NaN(0, 3);
-        otherSensors (:, 1) cell = cell(0, 1);
-    end
-    arguments (Output)
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
-    end
-
-    % Clear local caches so this visualization always uses its own grid
-    obj.rssCache = [];
-    for ii = 1:numel(otherSensors)
-        otherSensors{ii}.rssCache = [];
-    end
-
-    % bias other sensors altitudes appropriately
-    otherSensorsPos = otherSensorsPos + [0, 0, altitude];
-
-    % Create grid on which to evalute SINR, SNR
-    agentPos = [0, 0, altitude];
-    d = 10;
-    if ~isempty(otherSensorsPos)
-        d = max(otherSensorsPos(:, 3) * 0.55);
-        d = max(d, max(vecnorm(otherSensorsPos(:, 1:2), 2, 2)) * 1.25);
-    end
-    c = 0.1;
-    d = ceil(d / c) * c;
-    distances = -d:c:d;
-    [targetPosX, targetPosY] = meshgrid(distances, distances);
-
-    % Compute SINR, SNR
-    [SINR, ~] = obj.sensorPerformance(agentPos, [targetPosX(:), targetPosY(:), zeros(size(targetPosX(:)))], otherSensorsPos, otherSensors);
-    SINR = reshape(SINR, size(targetPosX));
-
-    % normalize in linear scale
-    % SINR = 10.^(SINR/10); SINR = SINR ./ max(SINR(:)); SINR = 10 * log10(SINR);
-
-    % Collect sensor positions and boresight parameters for overlay
-    sensorTilts   = [obj.tilt;    cellfun(@(s) s.tilt,    otherSensors)];
-    sensorAzimuths = [obj.azimuth; cellfun(@(s) s.azimuth, otherSensors)];
-    tailScale = 0.5 * d;
-
-    f = figure;
-    surf(targetPosX, targetPosY, zeros(size(targetPosX)), SINR, "EdgeColor", "none");
-    axis(f.Children(1), "image");
-    colormap(f.Children(1), "hot");
-    title("Ground User SINR and -3 dB antenna gain regions");
-    subtitle(sprintf("%d interfering source(s)", size(otherSensorsPos, 1)));
-    c = colorbar;
-    ylabel(c, "SINR (dB)");
-    xlabel("X (m)");
-    ylabel("Y (m)");
-    hold(f.Children(2), "on");
-    scatter3(0, 0, altitude, 100, 'ko', "LineWidth", 2);
-    scatter3(otherSensorsPos(:, 1), otherSensorsPos(:, 2), otherSensorsPos(:, 3), 100, "bx", "LineWidth", 2);
-    qSelf = quiver3(0, 0, altitude, ...
-        tailScale * sind(obj.tilt) * sind(obj.azimuth), ...
-        tailScale * sind(obj.tilt) * cosd(obj.azimuth), ...
-        -tailScale * cosd(obj.tilt), ...
-        0, 'k', 'LineWidth', 1.5);
-    qSelf.MaxHeadSize = 0.75;
-    if ~isempty(otherSensors)
-        qOthers = quiver3(otherSensorsPos(:,1), otherSensorsPos(:,2), otherSensorsPos(:,3), ...
-            tailScale .* sind(sensorTilts(2:end)) .* sind(sensorAzimuths(2:end)), ...
-            tailScale .* sind(sensorTilts(2:end)) .* cosd(sensorAzimuths(2:end)), ...
-            -tailScale .* cosd(sensorTilts(2:end)), ...
-            0, 'b', 'LineWidth', 1.5);
-        qOthers.MaxHeadSize = 0.75;
-    end
-    % Draw half-angle cones co-boresighted with each quiver arrow
-    N = 48;
-    phi = linspace(0, 2*pi, N);
-    [PHI, S] = meshgrid(phi, [0; 1]);   % row 1 = apex (s=0), row 2 = base (s=1)
-    allSensors = [{obj}; otherSensors];
-    allPos     = [[0, 0, altitude]; otherSensorsPos];
-    for ii = 1:numel(allSensors)
-        ha  = allSensors{ii}.halfAngle();
-        tlt = sensorTilts(ii);
-        az  = sensorAzimuths(ii);
-        pos = allPos(ii, :);
-        % Cone length: enough that the axis tip is guaranteed below z=0
-        coneLength = 1.1 * pos(3) / max(cosd(tlt), 0.1);
-        % Nadir cone mesh: apex at origin, base at z = -coneLength
-        cX = S .* coneLength .* tand(ha) .* cos(PHI);
-        cY = S .* coneLength .* tand(ha) .* sin(PHI);
-        cZ = -S .* coneLength;
-        % Rotate nadir → boresight (same convention as quiver arrows)
-        Ry = [cosd(tlt), 0, -sind(tlt); 0, 1, 0; sind(tlt), 0, cosd(tlt)];
-        Rz = [sind(az), -cosd(az), 0; cosd(az), sind(az), 0; 0, 0, 1];
-        R  = Rz * Ry;
-        pts = R * [cX(:)'; cY(:)'; cZ(:)'];
-        cX  = reshape(pts(1,:), size(cX)) + pos(1);
-        cY  = reshape(pts(2,:), size(cY)) + pos(2);
-        cZ  = reshape(pts(3,:), size(cZ)) + pos(3);
-        if ii == 1
-            fc = [0, 0, 0];
-        else
-            fc = [0, 0, 1];
-        end
-        surf(cX, cY, cZ, "FaceColor", fc, "FaceAlpha", 0.15, "EdgeColor", "none");
-
-        % Conic section: intersect each cone generator with z=0
-        b_vec = R * [0; 0; -1];
-        u_vec = R * [1; 0; 0];
-        v_vec = R * [0; 1; 0];
-        phi_sec = linspace(0, 2*pi, 720)';
-        dirs = cosd(ha) .* b_vec' + sind(ha) .* (cos(phi_sec) .* u_vec' + sin(phi_sec) .* v_vec');
-        t_sec = -pos(3) ./ dirs(:, 3);
-        t_sec(t_sec <= 0) = NaN;
-        sx = pos(1) + t_sec .* dirs(:, 1);
-        sy = pos(2) + t_sec .* dirs(:, 2);
-        plot3(sx, sy, zeros(size(sx)), "Color", fc, "LineWidth", 2);
-    end
-    clim(f.Children(2), [min(SINR(:)), max(SINR(:))]);
-    xlim(f.Children(2), [-d, d]);
-    ylim(f.Children(2), [-d, d]);
-    hold(f.Children(2), "off");
-    zlim([0, Inf]);
-    
-
-
-
-end

@rfSensor/plotParameters.m

@@ -1,52 +0,0 @@
-function f = plotParameters(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-    end
-    arguments (Output)
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
-    end
-
-    % Agent altitude layers and angle sample points
-    alt_values = 10.^[1, 2, 3, 4];
-    t_values = 0:2.5:87.5;   % 0=nadir (center), <90=near horizon (edge)
-    a_values = 0:2.5:360;
-
-    [T, A] = meshgrid(t_values, a_values);   % Naz x Nel
-    Ar = deg2rad(A);
-
-    f = figure;
-    hold("on");
-
-    for ii = 1:numel(alt_values)
-        alt = alt_values(ii);
-
-        % For agent at altitude alt, ground target at tilt T has slant distance:
-        D = alt ./ cosd(T);
-
-        % Compute RSS for each (d, t, a) triple
-        rss = obj.RSS(D(:), T(:), A(:));
-        Fslice = reshape(rss, size(D));
-
-        % Disc geometry: t=0 (nadir) -> center, t~90 (horizon) -> edge
-        r = log10(alt) .* T ./ 90;
-        X = r .* cos(Ar);
-        Y = r .* sin(Ar);
-        Z = log10(alt) * ones(size(X));
-
-        hs = surf(X, Y, Z, Fslice);
-        hs.EdgeColor = 'none';
-        hs.FaceColor = 'interp';
-        hs.FaceAlpha = 0.25;
-    end
-
-    colormap(turbo);
-    c = colorbar; c.Label.String = "Received Signal Strength (dB)";
-    daspect([1 1 0.2]);
-    xlabel('X (log_{10} units)'); ylabel('Y (log_{10} units)'); zlabel('log_{10} Altitude (m)');
-    set(gca, 'ZDir', 'reverse');
-    view(3);
-    axis("vis3d");
-    grid("on");
-    scatter3(0, 0, 0, 'rx');
-    hold("off");
-end

@rfSensor/plotPerformance.m

@@ -1,91 +0,0 @@
-function f = plotPerformance(obj, altitude, otherSensorsPos, otherSensors)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        altitude (1, 1) double;
-        otherSensorsPos (:, 3) double = NaN(0, 3);
-        otherSensors (:, 1) cell = cell(0, 1);
-    end
-    arguments (Output)
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
-    end
-
-    % Clear local caches so this visualization always uses its own grid
-    obj.rssCache = [];
-    for ii = 1:numel(otherSensors)
-        otherSensors{ii}.rssCache = [];
-    end
-
-    % bias other sensors altitudes appropriately
-    otherSensorsPos = otherSensorsPos + [0, 0, altitude];
-
-    % Create grid on which to evalute SINR, SNR
-    agentPos = [0, 0, altitude];
-    d = 10;
-    if ~isempty(otherSensorsPos)
-        d = max(d, max(vecnorm(otherSensorsPos(:, 1:2), 2, 2)) * 1.25);
-    end
-    c = 0.1;
-    d = ceil(d / c) * c;
-    distances = -d:c:d;
-    [targetPosX, targetPosY] = meshgrid(distances, distances);
-
-    % Compute SINR, SNR
-    [SINR, SNR] = obj.sensorPerformance(agentPos, [targetPosX(:), targetPosY(:), zeros(size(targetPosX(:)))], otherSensorsPos, otherSensors);
-    SINR = reshape(SINR, size(targetPosX));
-    SNR = reshape(SNR, size(targetPosX));
-
-    % normalize in linear scale
-    SINR = 10.^(SINR/10); SINR = SINR ./ max(SINR(:)); SINR = 10 * log10(SINR);
-    SNR  = 10.^(SNR/10);  SNR  = SNR  ./ max(SNR(:));  SNR  = 10 * log10(SNR);
-
-    % Collect sensor positions and boresight parameters for overlay
-    sensorXY      = [0, 0; otherSensorsPos(:, 1:2)];
-    sensorTilts   = [obj.tilt;    cellfun(@(s) s.tilt,    otherSensors)];
-    sensorAzimuths = [obj.azimuth; cellfun(@(s) s.azimuth, otherSensors)];
-    tailScale = 0.5 * d;
-
-    f = figure;
-    tiledlayout(1, 2, TileSpacing="compact", Padding="compact");
-
-    nexttile;
-    imagesc(distances, distances, SNR);
-    axis("image"); set(gca, 'YDir', 'normal');
-    colorbar; xlabel("X (m)"); ylabel("Y (m)");
-    title("Linearly Normalized SNR (dB)");
-    subtitle("No interfering sources");
-    addSensorOverlay(gca, sensorXY(1, 1:2), sensorTilts(1, 1), sensorAzimuths(1, 1), tailScale);
-
-    nexttile;
-    imagesc(distances, distances, SINR);
-    axis("image"); set(gca, 'YDir', 'normal');
-    colorbar; xlabel("X (m)"); ylabel("Y (m)");
-    title("Linearly Normalized SINR (dB)");
-    subtitle(sprintf("%d interfering source(s)", size(otherSensorsPos, 1)));
-    addSensorOverlay(gca, sensorXY, sensorTilts, sensorAzimuths, tailScale);
-end
-
-function addSensorOverlay(ax, sensorXY, tilts, azimuths, tailScale)
-    % Draw a marker + boresight arrow for each sensor.
-    % Tail direction follows azimuth convention (0=+Y, 90=+X, clockwise).
-    % Tail length = tailScale * sind(tilt), so nadir (0°) has no tail and
-    % horizon (90°) has the full tailScale length.
-    hold(ax, 'on');
-    for ii = 1:size(sensorXY, 1)
-        x = sensorXY(ii, 1);
-        y = sensorXY(ii, 2);
-        if ii == 1
-            c = [0, 0, 0];
-            mk = 'o';
-        else
-            c = [0.9, 0.2, 0.2];
-            mk = 'x';
-        end
-        scatter(ax, x, y, 80, c, mk, LineWidth=2);
-        if tilts(ii) > 0
-            u = tailScale * sind(tilts(ii)) * sind(azimuths(ii));
-            v = tailScale * sind(tilts(ii)) * cosd(azimuths(ii));
-            quiver(ax, x, y, u, v, 0, Color=c, LineWidth=2, MaxHeadSize=1.0);
-        end
-    end
-    hold(ax, 'off');
-end

@rfSensor/rfSensor.m

@@ -1,40 +0,0 @@
-classdef rfSensor
-    properties (SetAccess = private, GetAccess = public)
-        % Physical parameters
-        c = 3e8; % Speed of light (m/s)
-        k_B = 1.38e-23 % Boltzmann constant (W/Hz/K) for thermal noise model
-        T_0 = 300; % Ambient temperature (Kelvin) for thermal noise model
-        lossExponent = NaN; % Path loss exponent (2 for free space, up to 6 for the lossiest environments)
-        % Sensor parameters
-        P_TX = NaN; % Transmit power (Watts)
-        BW = NaN; % Bandwidth (Hz)
-        f_c = NaN; % Center frequency (Hz)
-        G_RX_dBi = NaN; % Receiver antenna gain
-        beamwidthExponent = NaN; % Antenna beamwidth exponent for cosine radiation pattern, larger exponent -> narrower beam
-        % Values computed at initialization
-        P_TX_dBm = NaN; % Transmit power (dBm)
-        N = NaN; % Thermal noise
-        % Cached state (per timestep)
-    end
-    properties (Access = public)
-        tilt = NaN;     % Antenna boresight tilt (deg): 0=nadir, 90=horizon
-        azimuth = NaN;  % Antenna boresight azimuth (deg): 0=+y, 90=+x, 180=-y, 270=-x
-        rssCache (:,1) double = double.empty(0,1); % linear-scale RSS to last ground targets grid
-    end
-
-    methods (Access = public)
-        [obj] = initialize(obj, txPower, bandwidth, centerFreq, rxGain, beamwidthExponent, tilt, azimuth); % initialize sensor, define parameters
-        [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targetPos, otherSensorsPos, otherSensors); % determine sensor performance for a given single sensor and target geometry
-        [d, dx, dy, dz] = computePointToPoints(obj, agentPos, targetPos);
-        [value] = halfAngle(obj); % tilt angle (deg) at which sensor performance is halved
-        [f] = plotParameters(obj); % debug, plot sensor response as a function of distance and tilt angle
-        [f] = plotPerformance(obj, altitude, otherSensorsPos, otherSensors); % debug, plot SNR or SINR ground heatmap for a given geometry
-        [f] = plot(obj, altitude, otherSensorsPos, otherSensors);
-        obj = clearRssCache(obj);
-    end
-    methods (Access = private)
-        x = RSS(obj, d, dx, dy, dz); % Received signal strength (function of distance and tilt angle)
-        G_TX_dB = transmitterGain(obj, t, a); % Antenna gain for a given TX/RX pair 
-        L_FSPL_dB = pathLoss(obj, d); % Free space path loss for a given TX/RX pair
-    end
-end

@rfSensor/sensorPerformance.m

@@ -1,34 +0,0 @@
-function [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targetPos, otherSensorsPos, otherSensors)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        agentPos (1, 3) double;
-        targetPos (:, 3) double;
-        otherSensorsPos (:, 3) double = [];
-        otherSensors (:, 1) cell = {};
-    end
-    arguments (Output)
-        SINR (:, 1) double;
-        SNR (:, 1) double;
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        otherSensors (:, 1) cell;
-    end
-    assert(size(otherSensorsPos, 1) == size(otherSensors, 1), "Mismatch in number of other sensor positions (%d) and number of other sensors (%d) provided", size(otherSensorsPos, 1), size(otherSensors, 1));
-
-    if isempty(obj.rssCache)
-        [d, dx, dy, dz] = obj.computePointToPoints(agentPos, targetPos);
-        obj.rssCache = 1e-3 .* 10 .^ (0.1 .* obj.RSS(d, dx, dy, dz));  % dBm → W
-    end
-    S = obj.rssCache;
-
-    I = zeros(size(S));
-    for ii = 1:size(otherSensors, 1)
-        if isempty(otherSensors{ii}.rssCache)
-            [d_o, dx_o, dy_o, dz_o] = otherSensors{ii}.computePointToPoints(otherSensorsPos(ii, 1:3), targetPos);
-            otherSensors{ii}.rssCache = 1e-3 .* 10 .^ (0.1 .* otherSensors{ii}.RSS(d_o, dx_o, dy_o, dz_o));  % dBm → W
-        end
-        I = I + otherSensors{ii}.rssCache;
-    end
-
-    SINR = 10*log10(S ./ (I + obj.N));
-    SNR  = 10*log10(S ./ obj.N);
-end

@rfSensor/transmitterGain.m

@@ -1,23 +0,0 @@
-function value = transmitterGain(obj, t, a)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-        t (:, 1) double; % LOS tilt angle
-        a (:, 1) double; % LOS azimuth angle
-    end
-    arguments (Output)
-        value (:, 1) double
-    end
-    if ~isequal(size(t), size(a))
-        error("t and a must be the same size");
-    end
-
-    % Angular offset from boresight via spherical law of cosines
-    % Convention: t=0° nadir, t=90° horizon; a=0° +y, a=90° +x
-    cos_theta = sind(obj.tilt) .* sind(t) .* cosd(a - obj.azimuth) + ...
-                cosd(obj.tilt) .* cosd(t);
-    cos_theta = max(-1, min(1, cos_theta));  % clamp for numerical safety
-    theta = acosd(cos_theta);
-
-    % Cardioid family: peak at boresight (theta=0), null opposite (theta=180°)
-    value = 10 .* obj.beamwidthExponent .* log10((1 + cosd(theta)) ./ 2);
-end

@sensingObjective/initialize.m

@@ -1,22 +1,21 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, "sensingObjective")};
-        objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
+        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         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")};
+        obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
     obj.discretizationStep = discretizationStep;
 
     obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
 
+    obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 
     % Extract footprint limits
@@ -32,20 +31,16 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
 
     % Evaluate function over grid points
-    obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X));
+    obj.objectiveFunction = objectiveFunction;
+    obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
     
     % Normalize
     obj.values = obj.values ./ max(obj.values, [], "all");
 
     % store ground position
     idx = obj.values == 1;
-    if any(isnan(objectiveMu))
-        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;
+    obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
 
-    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective");
+    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -1,26 +1,26 @@
 function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sensingObjective")};
+        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "sensingObjective")};
+        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
     end
 
     % Set random objective position
     mu = domain.minCorner;
-    while domain.distance(mu) < protectedRange * 1.01
+    while domain.distance(mu) < protectedRange
         mu = domain.random();
     end
 
     % Set random distribution parameters
-    sig = reshape([2 + rand * 2, 1; 1, 2 + rand * 2], [1 2 2]);
+    sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
 
     % Set up random bivariate normal distribution function
     objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
-    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange, 1e-6, mu(1:2), sig);
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
 end

@sensingObjective/initializeWithValues.m

@@ -1,42 +0,0 @@
-function obj = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
-    arguments (Input)
-        obj (1,1) {mustBeA(obj, "sensingObjective")};
-        values (:,:) double;
-        domain (1, 1) {mustBeGeometry};
-        discretizationStep (1, 1) double = 1;
-        protectedRange (1, 1) double = 1;
-        sensorPerformanceMinimum (1, 1) double = 1e-6;
-    end
-    arguments (Output)
-        obj (1,1) {mustBeA(obj, "sensingObjective")};
-    end
-
-    obj.discretizationStep = discretizationStep;
-    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
-    obj.protectedRange = protectedRange;
-
-    % Extract footprint limits
-    xMin = min(domain.footprint(:, 1));
-    xMax = max(domain.footprint(:, 1));
-    yMin = min(domain.footprint(:, 2));
-    yMax = max(domain.footprint(:, 2));
-
-    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
-    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
-
-    % Store grid points
-    [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
-
-    % Use pre-computed values (caller must evaluate on same grid)
-    obj.values = reshape(values, size(obj.X));
-
-    % Normalize
-    obj.values = obj.values ./ max(obj.values, [], "all");
-
-    % Store ground position (peak of objective)
-    idx = obj.values == 1;
-    obj.groundPos = [obj.X(idx), obj.Y(idx)];
-    obj.groundPos = obj.groundPos(1, 1:2);
-
-    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
-end

@sensingObjective/plot.m

@@ -1,36 +1,29 @@
 function f = plot(obj, ind, f)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, "sensingObjective")};
+        obj (1,1) {mustBeA(obj, 'sensingObjective')};
         ind (1, :) double = NaN;
-        f (1,1) {mustBeA(f, "matlab.ui.Figure")} = figure;
+        f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
-        f (1,1) {mustBeA(f, "matlab.ui.Figure")};
+        f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Create axes if they don't already exist
     f = firstPlotSetup(f);
 
-    normalized = obj.values ./ sum(obj.values, "all");
-    cRange = [min(normalized, [], "all"), max(normalized, [], "all")];
-
     % Plot gradient on the "floor" of the domain
     if isnan(ind)
-        ax = f.CurrentAxes;
-        hold(ax, "on");
-        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
-        o.HitTest = "off";
-        o.PickableParts = "none";
-        clim(ax, cRange);
-        hold(ax, "off");
+        hold(f.CurrentAxes, "on");
+        o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o.HitTest = 'off';
+        o.PickableParts = 'none';
+        hold(f.CurrentAxes, "off");
     else
-        ax = f.Children(1).Children(ind(1));
-        hold(ax, "on");
-        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
-        o.HitTest = "off";
-        o.PickableParts = "none";
-        clim(ax, cRange);
-        hold(ax, "off");
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o.HitTest = 'off';
+        o.PickableParts = 'none';
+        hold(f.Children(1).Children(ind(1)), "off");
     end
 
     % Add to other perspectives

@sensingObjective/sensingObjective.m

@@ -2,9 +2,10 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundPos = NaN(1, 2);
-        objectiveSigma = NaN(1, 2, 2);
+        groundAlt = NaN;
+        groundPos = [NaN, NaN];
         discretizationStep = NaN;
+        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
         X = [];
         Y = [];
         values = [];
@@ -14,8 +15,7 @@ classdef sensingObjective
 
     methods (Access = public)
         [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
-        [obj] = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
-        [obj] = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange);
+        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end
 end

@sigmoidSensor/distanceMembership.m

@@ -1,6 +1,6 @@
 function x = distanceMembership(obj, d)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
         d (:, 1) double;
     end
     arguments (Output)

@sigmoidSensor/halfAngle.m

@@ -1,9 +0,0 @@
-function value = halfAngle(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
-    end
-    arguments (Output)
-        value (1, 1) double;
-    end
-    value = obj.alphaTilt;
-end

@sigmoidSensor/initialize.m

@@ -1,24 +1,17 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
-        tilt (1, 1) double = 0;
-        azimuth (1, 1) double = 0;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
     end
     
-    % Sensor performance parameters
     obj.alphaDist = alphaDist;
     obj.betaDist = betaDist;
     obj.alphaTilt = alphaTilt;
     obj.betaTilt = betaTilt;
-
-    % Sensor pointing parameters
-    obj.tilt = tilt;
-    obj.azimuth = azimuth;
 end

@sigmoidSensor/plotParameters.m

@@ -1,9 +1,9 @@
 function f = plotParameters(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
     end
     arguments (Output)
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Distance and tilt sample points
@@ -24,9 +24,9 @@ function f = plotParameters(obj)
     title("Distance Membership Sigmoid");
     xlabel("Distance (m)");
     ylabel("Membership");
-    hold("on");
-    plot(d, d_x, "LineWidth", 2);
-    hold("off");
+    hold('on');
+    plot(d, d_x, 'LineWidth', 2);
+    hold('off');
     ylim([0, 1]);
 
     % Tilt
@@ -35,8 +35,8 @@ function f = plotParameters(obj)
     title("Tilt Membership Sigmoid");
     xlabel("Tilt (deg)");
     ylabel("Membership");
-    hold("on");
-    plot(t, t_x, "LineWidth", 2);
-    hold("off");
+    hold('on');
+    plot(t, t_x, 'LineWidth', 2);
+    hold('off');
     ylim([0, 1]);
 end

@sigmoidSensor/sensorPerformance.m

@@ -1,6 +1,6 @@
 function value = sensorPerformance(obj, agentPos, targetPos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
         agentPos (1, 3) double;
         targetPos (:, 3) double;
     end
@@ -8,20 +8,16 @@ function value = sensorPerformance(obj, agentPos, targetPos)
         value (:, 1) double;
     end
 
-    % Unit vectors from agent to each target
-    diffs = targetPos - agentPos;
-    d = vecnorm(diffs, 2, 2);
-    dirs = diffs ./ d;
+    % compute direct distance and distance projected onto the ground
+    d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
+    x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
 
-    % Boresight unit vector: tilt=0 → nadir [0,0,-1]; azimuth 0=+Y, 90=+X clockwise
-    boresight = [sind(obj.tilt)*sind(obj.azimuth), sind(obj.tilt)*cosd(obj.azimuth), -cosd(obj.tilt)];
-
-    % Angular offset from boresight to each target direction
-    angularOffset = acosd(dirs * boresight');
+    % compute tilt angle
+    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
 
     % Membership functions
     mu_d = obj.distanceMembership(d);
-    mu_t = obj.tiltMembership(angularOffset);
+    mu_t = obj.tiltMembership(tiltAngle);
 
     value = mu_d .* mu_t; % assume pan membership is always 1
 end

@sigmoidSensor/sigmoidSensor.m

@@ -6,20 +6,14 @@ classdef sigmoidSensor
         alphaTilt = NaN; % degrees
         betaTilt = NaN;
     end
-    properties (Access = public)
-        % pointing states
-        tilt = 0;
-        azimuth = 0;
-    end
 
     methods (Access = public)
-        [obj]   = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth); % initialize sensor, define parameters
-        [value] = sensorPerformance(obj, agentPos, targetPos); % determine sensor performance for a given single sensor and target geometry
-        [value] = halfAngle(obj); % tilt angle (deg) at which sensor performance is halved
-        [f]     = plotParameters(obj); % debug, plot sensor response as a function of distance and tilt angle
+        [obj]               = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt);
+        [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
+        [f] = plotParameters(obj);
     end
     methods (Access = private)
-        x = distanceMembership(obj, d); % used in computing distance factor of sensor performance
-        x = tiltMembership(obj, t); % used in computing tilt factor of sensor performance
+        x = distanceMembership(obj, d);
+        x = tiltMembership(obj, t);
     end
 end

@sigmoidSensor/tiltMembership.m

@@ -1,6 +1,6 @@
 function x = tiltMembership(obj, t)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
         t (:, 1) double; % degrees
     end
     arguments (Output)

aerpaw/MESSAGE_TYPE.m

@@ -1,12 +0,0 @@
-classdef MESSAGE_TYPE < uint8
-    enumeration
-        TARGET  (1)  % Server->Client: target coordinates follow (3 doubles)
-        ACK     (2)  % Client->Server: command received
-        READY   (3)  % Both: ready for next command / mission complete
-        RTL              (4)  % Server->Client: return to launch
-        LAND             (5)  % Server->Client: land now
-        GUIDANCE_TOGGLE  (6)  % Server->Client: toggle guidance mode on/off
-        REQUEST_POSITION (7)  % Server->Client: respond with current ENU position
-        POSITION         (8)  % Client->Server: current ENU position (3 doubles)
-    end
-end

aerpaw/client/uav_runner.py

@@ -1,300 +0,0 @@
-#!/usr/bin/env python3
-"""
-UAV Runner for AERPAW - aerpawlib BasicRunner implementation.
-
-Run via:
-    python -m aerpawlib --script client.uav_runner --conn <conn> --vehicle drone
-
-Or use the auto-detecting wrapper:
-    ./run_uav.sh
-
-Binary Protocol:
-    For each waypoint:
-        Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
-        Client sends: ACK (1 byte)
-        Client (after moving): READY (1 byte)
-    After all waypoints:
-        Server sends: RTL (1 byte) → Client: ACK, return home, READY
-        Server sends: LAND (1 byte) → Client: ACK, land, READY
-        Server sends: READY (1 byte) - mission complete, disconnect
-"""
-from enum import IntEnum
-from pathlib import Path
-import asyncio
-import csv
-import datetime
-import os
-import platform
-import struct
-import time
-import yaml
-
-from aerpawlib.runner import BasicRunner, entrypoint
-from aerpawlib.util import Coordinate, VectorNED
-from aerpawlib.vehicle import Drone
-
-
-# Message types - must match MESSAGE_TYPE.m enum
-class MessageType(IntEnum):
-    TARGET           = 1
-    ACK              = 2
-    READY            = 3
-    RTL              = 4
-    LAND             = 5
-    GUIDANCE_TOGGLE  = 6
-    REQUEST_POSITION = 7
-    POSITION         = 8
-
-
-AERPAW_DIR = Path('/root/miSim/aerpaw')
-CONFIG_FILE = Path(os.environ.get('AERPAW_CLIENT_CONFIG',
-                                  AERPAW_DIR / "config" / "client.yaml"))
-
-
-def load_config():
-    """Load configuration from YAML file."""
-    with open(CONFIG_FILE, 'r') as f:
-        return yaml.safe_load(f)
-
-
-def get_environment():
-    """Get environment from AERPAW_ENV variable. Fails if not set."""
-    env = os.environ.get('AERPAW_ENV')
-    if env is None:
-        raise RuntimeError(
-            "AERPAW_ENV environment variable not set. "
-            "Set to 'local' or 'testbed', or use: ./run_uav.sh [local|testbed]"
-        )
-    if env not in ('local', 'testbed'):
-        raise RuntimeError(
-            f"Invalid AERPAW_ENV '{env}'. Must be 'local' or 'testbed'."
-        )
-    return env
-
-
-async def recv_exactly(reader: asyncio.StreamReader, n: int) -> bytes:
-    """Receive exactly n bytes from async stream."""
-    data = await reader.readexactly(n)
-    return data
-
-
-async def recv_message_type(reader: asyncio.StreamReader) -> MessageType:
-    """Receive a single-byte message type."""
-    data = await recv_exactly(reader, 1)
-    return MessageType(data[0])
-
-
-async def send_message_type(writer: asyncio.StreamWriter, msg_type: MessageType):
-    """Send a single-byte message type."""
-    writer.write(bytes([msg_type]))
-    await writer.drain()
-
-
-def _gps_log_row(vehicle, line_num, writer):
-    """Sample vehicle state and write one CSV row (matches gps_logger.py format)."""
-    pos = vehicle.position
-    lat, lon = pos.lat, pos.lon
-    alt = round(float(str(pos.alt)), 3)
-
-    batt = str(vehicle.battery)
-    volt = float(batt[16:batt.find(",")])
-
-    timestamp = datetime.datetime.now()
-    fix, num_sat = vehicle.gps.fix_type, vehicle.gps.satellites_visible
-
-    if fix < 2:
-        lat, lon, alt = -999, -999, -999
-
-    vel = vehicle.velocity
-    attitude = vehicle.attitude
-    attitude_str = (
-        "(" + ",".join(map(str, [attitude.pitch, attitude.yaw, attitude.roll])) + ")"
-    )
-
-    writer.writerow([line_num, lon, lat, alt, attitude_str, vel, volt, timestamp, fix, num_sat])
-
-
-async def _gps_log_loop(drone):
-    """Background async task that logs GPS data at 1Hz."""
-    results_dir = os.environ.get('RESULTS_DIR', '/root/Results')
-    log_prefix = os.environ.get('LOG_PREFIX', datetime.datetime.now().strftime('%Y-%m-%d_%H_%M_%S'))
-    filename = os.path.join(results_dir, f"{log_prefix}_gps_log.csv")
-    print(f"[UAV] GPS logging to {filename}")
-    line_num = 1
-    try:
-        with open(filename, "w+") as f:
-            writer = csv.writer(f)
-            while True:
-                if drone.connected:
-                    _gps_log_row(drone, line_num, writer)
-                    f.flush()
-                    os.fsync(f)
-                    line_num += 1
-                else:
-                    print("[UAV] [GPS] No vehicle heartbeat")
-                await asyncio.sleep(1.0)
-    except asyncio.CancelledError:
-        print(f"[UAV] GPS logging stopped ({line_num - 1} rows written)")
-    except Exception as e:
-        print(f"[UAV] GPS logging error: {e}")
-
-
-class UAVRunner(BasicRunner):
-    def initialize_args(self, extra_args):
-        """Load configuration from YAML config file."""
-        config = load_config()
-        env = get_environment()
-        print(f"[UAV] Environment: {env}")
-
-        # Load origin
-        origin = config['origin']
-        self.origin = Coordinate(origin['lat'], origin['lon'], origin['alt'])
-        print(f"[UAV] Origin: {origin['lat']}, {origin['lon']}, {origin['alt']}")
-
-        # Load controller address for this environment
-        env_config = config['environments'][env]
-        self.server_ip = env_config['controller']['ip']
-        self.server_port = env_config['controller']['port']
-        print(f"[UAV] Controller: {self.server_ip}:{self.server_port}")
-
-    @entrypoint
-    async def run_mission(self, drone: Drone):
-        """Main mission entry point."""
-        print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
-
-        # Retry connection up to 10 times (~30 seconds total)
-        reader, writer = None, None
-        for attempt in range(100):
-            try:
-                reader, writer = await asyncio.wait_for(
-                    asyncio.open_connection(self.server_ip, self.server_port),
-                    timeout=5,
-                )
-                print(f"[UAV] Connected to controller")
-                break
-            except (ConnectionRefusedError, asyncio.TimeoutError, OSError) as e:
-                print(f"[UAV] Connection attempt {attempt + 1}/10 failed: {e}")
-                if attempt < 9:
-                    await asyncio.sleep(3)
-
-        if reader is None:
-            print("[UAV] Failed to connect to controller after 10 attempts")
-            return
-
-        log_task = None
-        nav_task = None
-        try:
-            # Takeoff to above AERPAW minimum altitude
-            print("[UAV] Taking off...")
-            await drone.takeoff(25)
-            print("[UAV] Takeoff complete, waiting for commands...")
-
-            # Start GPS logging in background
-            log_task = asyncio.create_task(_gps_log_loop(drone))
-
-            # Command loop - handle all messages from controller
-            waypoint_num = 0
-            in_guidance = False
-            while True:
-                msg_type = await recv_message_type(reader)
-                print(f"[UAV] Received: {msg_type.name}")
-
-                if msg_type == MessageType.GUIDANCE_TOGGLE:
-                    in_guidance = not in_guidance
-                    print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}")
-                    if not in_guidance:
-                        # Exiting guidance: wait for current navigation to finish
-                        # before resuming sequential (ACK/READY) mode
-                        if nav_task and not nav_task.done():
-                            print("[UAV] Waiting for current navigation to complete...")
-                            await nav_task
-                            nav_task = None
-                        # Acknowledge that we are ready for sequential commands
-                        await send_message_type(writer, MessageType.ACK)
-                        print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
-
-                elif msg_type == MessageType.REQUEST_POSITION:
-                    # Respond immediately with current ENU position relative to origin
-                    pos = drone.position
-                    enu = pos - self.origin  # VectorNED(north, east, down)
-                    await send_message_type(writer, MessageType.POSITION)
-                    writer.write(struct.pack('<ddd', enu.east, enu.north, -enu.down))
-                    await writer.drain()
-                    print(f"[UAV] Sent POSITION: E={enu.east:.1f} N={enu.north:.1f} U={-enu.down:.1f}")
-
-                elif msg_type == MessageType.TARGET:
-                    # Read 24 bytes of coordinates (3 little-endian doubles)
-                    data = await recv_exactly(reader, 24)
-                    enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
-
-                    # Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
-                    target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
-
-                    if in_guidance:
-                        # Guidance mode (event-triggered): navigate to target,
-                        # then send ACK once arrived so the controller knows
-                        # all UAVs have reached their targets before it
-                        # requests positions and computes the next step.
-                        print(f"[UAV] Guidance TARGET: E={enu_x:.1f} N={enu_y:.1f} U={enu_z:.1f}")
-                        if nav_task and not nav_task.done():
-                            nav_task.cancel()
-                            await asyncio.gather(nav_task, return_exceptions=True)
-                        await drone.goto_coordinates(target)
-                        await send_message_type(writer, MessageType.ACK)
-                        print("[UAV] Sent ACK (arrived at guidance target)")
-                        nav_task = None
-                    else:
-                        # Sequential mode: ACK → navigate → READY
-                        waypoint_num += 1
-                        print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x:.1f}, y={enu_y:.1f}, z={enu_z:.1f}")
-                        print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
-                        await send_message_type(writer, MessageType.ACK)
-                        print("[UAV] Sent ACK")
-                        print(f"[UAV] Moving to waypoint {waypoint_num}...")
-                        await drone.goto_coordinates(target)
-                        print(f"[UAV] Arrived at waypoint {waypoint_num}")
-                        await send_message_type(writer, MessageType.READY)
-                        print("[UAV] Sent READY")
-
-                elif msg_type == MessageType.RTL:
-                    await send_message_type(writer, MessageType.ACK)
-                    print(f"[UAV] Sent ACK")
-                    print("[UAV] Returning to home...")
-                    home = drone.home_coords
-                    safe_alt = 25
-                    rtl_target = Coordinate(home.lat, home.lon, safe_alt)
-                    print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
-                    await drone.goto_coordinates(rtl_target)
-                    print("[UAV] Arrived at home position")
-                    await send_message_type(writer, MessageType.READY)
-                    print(f"[UAV] Sent READY")
-
-                elif msg_type == MessageType.LAND:
-                    await send_message_type(writer, MessageType.ACK)
-                    print(f"[UAV] Sent ACK")
-                    print("[UAV] Landing...")
-                    await drone.land()
-                    print("[UAV] Landed and disarmed")
-                    await send_message_type(writer, MessageType.READY)
-                    print(f"[UAV] Sent READY")
-
-                elif msg_type == MessageType.READY:
-                    print("[UAV] Mission complete")
-                    break
-
-                else:
-                    print(f"[UAV] Unknown command: {msg_type}")
-
-        except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
-            print(f"[UAV] Error: {e}")
-
-        finally:
-            if nav_task is not None and not nav_task.done():
-                nav_task.cancel()
-                await asyncio.gather(nav_task, return_exceptions=True)
-            if log_task is not None:
-                log_task.cancel()
-                await asyncio.gather(log_task, return_exceptions=True)
-            writer.close()
-            await writer.wait_closed()
-            print("[UAV] Connection closed")

aerpaw/compile.sh

@@ -1,24 +0,0 @@
-#!/bin/bash
-# Build controller_app from codegen + impl sources
-
-AERPAW_DIR="$(cd "$(dirname "$0")" && pwd)"
-CODEGEN="$AERPAW_DIR/codegen"
-IMPL="$AERPAW_DIR/impl"
-BUILD="$AERPAW_DIR/build"
-
-mkdir -p "$BUILD"
-
-echo "Compiling controller executable..."
-
-# Compile all codegen sources (handles any new generated files)
-g++ -I/home/kdee/matlab/R2025a/extern/include \
-    -I"$CODEGEN" \
-    -I"$IMPL" \
-    "$IMPL/controller_main.cpp" \
-    "$IMPL/controller_impl.cpp" \
-    "$CODEGEN"/*.cpp \
-    -o "$BUILD/controller_app" \
-    -fopenmp \
-    -lpthread
-
-echo "Built: $BUILD/controller_app"

aerpaw/config/client1.yaml

@@ -1,38 +0,0 @@
-# AERPAW UAV (Client) Configuration
-
-# Unique 0-indexed UAV identifier (each UAV must have a distinct value)
-uav_id: 0
-
-# TDM (Time-Division Multiplexing) radio settings
-# All UAVs share a common frequency; each transmits only during its time slot.
-# Slot assignment uses wall-clock modular arithmetic (GPS/NTP synced).
-tdm:
-  slot_duration: 0.5      # seconds per slot
-  guard_interval: 0.05    # seconds of silence at slot boundaries
-
-# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
-origin:
-  lat: 35.72595214250436
-  lon: -78.69917609299937
-  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
-# Environment-specific settings
-environments:
-  local:
-    # MAVLink connection for SITL simulation (UDP)
-    mavlink:
-      ip: "127.0.0.1"
-      port: 14550
-    # Controller server address
-    controller:
-      ip: "127.0.0.1"
-      port: 5000
-
-  testbed:
-    # 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.X.1, generally)
-    controller:
-      ip: "192.168.112.1"
-      port: 5000

aerpaw/config/client2.yaml

@@ -1,38 +0,0 @@
-# AERPAW UAV (Client) Configuration
-
-# Unique 0-indexed UAV identifier (each UAV must have a distinct value)
-uav_id: 1
-
-# TDM (Time-Division Multiplexing) radio settings
-# All UAVs share a common frequency; each transmits only during its time slot.
-# Slot assignment uses wall-clock modular arithmetic (GPS/NTP synced).
-tdm:
-  slot_duration: 0.5      # seconds per slot
-  guard_interval: 0.05    # seconds of silence at slot boundaries
-
-# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
-origin:
-  lat: 35.72595214250436
-  lon: -78.69917609299937
-  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
-# Environment-specific settings
-environments:
-  local:
-    # MAVLink connection for SITL simulation (UDP)
-    mavlink:
-      ip: "127.0.0.1"
-      port: 14550
-    # Controller server address
-    controller:
-      ip: "127.0.0.1"
-      port: 5000
-
-  testbed:
-    # 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.X.1, generally)
-    controller:
-      ip: "192.168.112.1"
-      port: 5000

aerpaw/config/scenario.csv

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

aerpaw/config/scenario_columns.csv

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

aerpaw/config/scenario_lock.csv

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

aerpaw/config/scenario_two_around_wall.csv

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

aerpaw/config/scenario_walls.csv

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

aerpaw/controller.m

@@ -1,281 +0,0 @@
-function controller(numClients)
-arguments (Input)
-    numClients (1, 1) int32;
-end
-
-coder.extrinsic('disp', 'readScenarioCsv');
-
-% Maximum clients supported (one initial position per UAV)
-MAX_CLIENTS = 4;
-% 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);
-numWaypoints = int32(0);
-totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
-
-% Load initial UAV positions from scenario CSV
-if coder.target('MATLAB')
-    disp('Loading initial positions from scenario.csv (simulation)...');
-    tmpSim = miSim;
-    sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
-    flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
-    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
-    totalLoaded = int32(size(posMatrix, 1));
-    targets(1:totalLoaded, :) = posMatrix;
-    numWaypoints = int32(1);
-    disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
-else
-    coder.cinclude('controller_impl.h');
-    filename = ['config/scenario.csv', char(0)];
-    totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
-                coder.ref(targets), int32(MAX_TARGETS));
-    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.
-% These constants are also used for the altitude-staggered return before RTL.
-TRANSIT_ALT_BASE = 25.0;  % must match drone.takeoff() altitude in uav_runner.py
-TRANSIT_ALT_STEP = 25;    % vertical separation per UAV (m); must exceed 2*collisionRadius
-if ~coder.target('MATLAB')
-    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 = 55;
-MAX_OBSTACLES_CTRL  = int32(8);
-scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
-obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
-obstacleMax    = zeros(MAX_OBSTACLES_CTRL, 3);
-numObstacles   = int32(0);
-if ~coder.target('MATLAB')
-    coder.cinclude('controller_impl.h');
-    scenarioFilename = ['config/scenario.csv', char(0)];
-    coder.ceval('loadScenario', coder.ref(scenarioFilename), coder.ref(scenarioParams));
-    numObstacles = coder.ceval('loadObstacles', coder.ref(scenarioFilename), ...
-                         coder.ref(obstacleMin), coder.ref(obstacleMax), ...
-                         int32(MAX_OBSTACLES_CTRL));
-end
-% On MATLAB path, scenarioParams and obstacle arrays are left as zeros.
-% guidance_step's MATLAB path loads parameters directly from scenario.csv
-% via sim.initializeFromCsv and does not use these arrays.
-
-% Initialize server
-if coder.target('MATLAB')
-    disp('Initializing server (simulation)...');
-else
-    coder.ceval('initServer');
-end
-
-% Accept clients
-for i = 1:numClients
-    if coder.target('MATLAB')
-        disp(['Accepting client ', num2str(i)]);
-    else
-        coder.ceval('acceptClient', int32(i));
-    end
-end
-
-% Waypoint loop: send each waypoint to all clients, wait for all to arrive
-for w = 1:numWaypoints
-    % Send TARGET for waypoint w to each client
-    for i = 1:numClients
-        % Targets are grouped by client: client i's waypoints are at rows
-        % (i-1)*numWaypoints+1 through i*numWaypoints
-        targetIdx = (i - 1) * numWaypoints + w;
-        target = targets(targetIdx, :);
-
-        if coder.target('MATLAB')
-            disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
-                  num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
-        else
-            coder.ceval('sendTarget', int32(i), coder.ref(target));
-        end
-    end
-
-    % Wait for ACK from all clients
-    if coder.target('MATLAB')
-        disp('Waiting for ACK from all clients...');
-    else
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
-                    int32(MESSAGE_TYPE.ACK));
-    end
-
-    % Wait for READY from all clients (all arrived at waypoint w)
-    if coder.target('MATLAB')
-        disp(['All UAVs arrived at waypoint ', num2str(w)]);
-    else
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
-                    int32(MESSAGE_TYPE.READY));
-    end
-end
-
-% ---- Phase 2: miSim guidance loop ----------------------------------------
-% Guidance parameters (adjust here and recompile as needed)
-MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
-
-% Enter guidance mode on all clients
-if ~coder.target('MATLAB')
-    coder.ceval('sendGuidanceToggle', int32(numClients));
-end
-
-% Request initial GPS positions and initialise guidance algorithm
-positions = zeros(MAX_CLIENTS, 3);
-if ~coder.target('MATLAB')
-    coder.ceval('sendRequestPositions', int32(numClients));
-    coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
-else
-    % Simulation: seed positions from CSV waypoints so agents don't start at origin
-    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
-end
-guidance_step(positions(1:numClients, :), true, ...
-              scenarioParams, obstacleMin, obstacleMax, numObstacles);
-
-% Main guidance loop (event-triggered)
-for step = 1:MAX_GUIDANCE_STEPS
-    if ~coder.target('MATLAB')
-        coder.ceval('setGuidanceStep', int32(step), int32(MAX_GUIDANCE_STEPS));
-    end
-
-    % Run one guidance step: feed current GPS positions in, get targets out
-    nextPositions = guidance_step(positions(1:numClients, :), false, ...
-                                  scenarioParams, obstacleMin, obstacleMax, numObstacles);
-
-    % Send target to each client
-    for i = 1:numClients
-        target = nextPositions(i, :);
-        if ~coder.target('MATLAB')
-            coder.ceval('sendTarget', int32(i), coder.ref(target));
-        else
-            disp(['[step ', num2str(step), '] target UAV ', num2str(i), ': ', num2str(target)]);
-        end
-    end
-
-    % Wait for ACK from all clients (each UAV ACKs when it arrives at its target)
-    if ~coder.target('MATLAB')
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
-                    int32(MESSAGE_TYPE.ACK));
-    else
-        disp(['[guidance] step ', num2str(step), ': all UAVs arrived']);
-    end
-
-    % Request current GPS positions from all clients
-    if ~coder.target('MATLAB')
-        coder.ceval('sendRequestPositions', int32(numClients));
-        coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
-    else
-        % Simulation: advance positions to guidance outputs for closed-loop feedback
-        positions(1:numClients, :) = nextPositions(1:numClients, :);
-    end
-end
-
-% Exit guidance mode on all clients (second toggle)
-if ~coder.target('MATLAB')
-    coder.ceval('sendGuidanceToggle', int32(numClients));
-    % Wait for ACK from all clients: confirms each client has finished its
-    % last guidance navigation and is back in sequential (ACK/READY) mode.
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.ACK));
-    % Reset step counter so post-guidance logging carries no step prefix.
-    coder.ceval('setGuidanceStep', int32(0), int32(MAX_GUIDANCE_STEPS));
-end
-% --------------------------------------------------------------------------
-
-% Altitude-staggered return: separate UAVs vertically before issuing RTL,
-% mirroring the initial positioning stagger so UAVs transit laterally at
-% unique altitudes and cannot collide during the return flight.
-if ~coder.target('MATLAB')
-    for i = 1:numClients
-        transitAlt = TRANSIT_ALT_BASE + (double(i) - 1) * TRANSIT_ALT_STEP;
-        target = [positions(i, 1), positions(i, 2), transitAlt];
-        coder.ceval('sendTarget', int32(i), coder.ref(target));
-    end
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
-else
-    disp('Altitude-staggered return (simulation): UAVs commanded to transit altitudes.');
-end
-
-% Send RTL command to all clients
-for i = 1:numClients
-    if coder.target('MATLAB')
-        disp(['Sending RTL to client ', num2str(i)]);
-    else
-        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL));
-    end
-end
-
-% Wait for ACK from all clients
-if coder.target('MATLAB')
-    disp('Waiting for ACK from all clients...');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.ACK));
-end
-
-% Wait for READY from all clients (returned to home)
-if coder.target('MATLAB')
-    disp('All UAVs returned to home.');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.READY));
-end
-
-% Send LAND command to all clients
-for i = 1:numClients
-    if coder.target('MATLAB')
-        disp(['Sending LAND to client ', num2str(i)]);
-    else
-        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND));
-    end
-end
-
-% Wait for ACK from all clients
-if coder.target('MATLAB')
-    disp('Waiting for ACK from all clients...');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.ACK));
-end
-
-% Wait for READY from all clients (landed and disarmed)
-if coder.target('MATLAB')
-    disp('All UAVs landed and disarmed.');
-else
-    coder.ceval('waitForAllMessageType', int32(numClients), ...
-                int32(MESSAGE_TYPE.READY));
-end
-
-% Send READY to all clients to signal mission complete
-for i = 1:numClients
-    if coder.target('MATLAB')
-        disp(['Sending READY to client ', num2str(i)]);
-    else
-        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY));
-    end
-end
-
-% Close server
-if ~coder.target('MATLAB')
-    coder.ceval('closeServer');
-end
-
-disp('Experiment complete.');
-
-end

aerpaw/guidance_step.m

@@ -1,215 +0,0 @@
-function nextPositions = guidance_step(currentPositions, isInit, ...
-                                       scenarioParams, ...
-                                       obstacleMin, obstacleMax, numObstacles)
-% guidance_step  One step of the miSim sensing coverage guidance algorithm.
-%
-% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW.
-% Holds full simulation state in a persistent variable between calls.
-%
-% Usage (from controller.m):
-%   guidance_step(initPositions, true,  scenarioParams, obstacleMin, obstacleMax, numObstacles)
-%   nextPos = guidance_step(gpsPos, false, scenarioParams, obstacleMin, obstacleMax, numObstacles)
-%
-% Inputs:
-%   currentPositions  (MAX_CLIENTS × 3) double  ENU [east north up] metres
-%   isInit            (1,1)             logical  true on first call only
-%   scenarioParams    (1 × NUM_SCENARIO_PARAMS) double
-%                       Flat array of guidance parameters (compiled path).
-%                       On MATLAB path this is ignored; parameters are loaded
-%                       from scenario.csv via initializeFromCsv instead.
-%                       Index mapping (1-based):
-%                         1  timestep            9-12  collisionRadius[1:4]
-%                         2  maxIter            13-16  comRange[1:4]
-%                         3  minAlt             17-20  alphaDist[1:4]
-%                         4  discretizationStep 21-24  betaDist[1:4]
-%                         5  protectedRange     25-28  alphaTilt[1:4]
-%                         6  initialStepSize    29-32  betaTilt[1:4]
-%                         7  barrierGain        33-35  domainMin
-%                         8  barrierExponent    36-38  domainMax
-%                                               39-40  objectivePos
-%                                               41-44  objectiveVar (2x2, col-major)
-%                                               45     sensorPerformanceMinimum
-%                                               46     useDoubleIntegrator
-%                                               47     dampingCoeff
-%                                               48     useFixedTopology
-%   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
-%   obstacleMax       (MAX_OBSTACLES × 3) double
-%   numObstacles      (1,1) int32                 actual obstacle count
-%
-% Output:
-%   nextPositions     (MAX_CLIENTS × 3) double  guidance targets, ENU metres
-%
-% Codegen notes:
-%   - Persistent variable 'sim' holds the miSim object between calls.
-%   - On MATLAB path isInit uses sim.initializeFromCsv (file I/O, not compiled).
-%   - On compiled path isInit uses scenarioParams + obstacle arrays directly.
-%   - Plotting/video are disabled (makePlots=false, makeVideo=false).
-
-coder.extrinsic('disp', 'objectiveFunctionWrapper', 'initializeFromCsv');
-
-MAX_CLIENTS = int32(4);      % must match MAX_CLIENTS in controller.m
-
-% Path to scenario CSV — used on MATLAB path only (not compiled)
-SCENARIO_CSV = 'aerpaw/config/scenario.csv';
-
-persistent sim;
-if isempty(sim)
-    sim = miSim;
-end
-
-% Pre-allocate output with known static size (required for codegen)
-nextPositions = zeros(MAX_CLIENTS, 3);
-
-numAgents = int32(size(currentPositions, 1));
-
-if isInit
-    if coder.target('MATLAB')
-        disp('[guidance_step] Initialising simulation...');
-
-        % MATLAB path: load all parameters and obstacles from scenario CSV.
-        % initializeFromCsv reads the file, builds domain/agents/obstacles,
-        % and calls sim.initialize internally.
-        sim = sim.initializeFromCsv(SCENARIO_CSV);
-
-        disp('[guidance_step] Initialisation complete.');
-    else
-        % ================================================================
-        % Compiled path: unpack scenarioParams array and obstacle arrays.
-        % Per-UAV parameters are stored as MAX_CLIENTS-wide blocks; only
-        % the first numAgents entries of each block are used.
-        % ================================================================
-        TIMESTEP            = scenarioParams(1);
-        MAX_ITER            = int32(scenarioParams(2));
-        MIN_ALT             = scenarioParams(3);
-        DISCRETIZATION_STEP = scenarioParams(4);
-        PROTECTED_RANGE     = scenarioParams(5);
-        INITIAL_STEP_SIZE   = scenarioParams(6);
-        BARRIER_GAIN        = scenarioParams(7);
-        BARRIER_EXPONENT    = scenarioParams(8);
-        COLLISION_RADIUS_VEC = scenarioParams(9:12);   % per-UAV [1:MAX_CLIENTS]
-        COMMS_RANGE_VEC      = scenarioParams(13:16);
-        ALPHA_DIST_VEC       = scenarioParams(17:20);
-        BETA_DIST_VEC        = scenarioParams(21:24);
-        ALPHA_TILT_VEC       = scenarioParams(25:28);
-        BETA_TILT_VEC        = scenarioParams(29:32);
-        DOMAIN_MIN                  = scenarioParams(33:35);
-        DOMAIN_MAX                  = scenarioParams(36:38);
-        NUM_OBJ_COMPONENTS          = int32(scenarioParams(39));
-        OBJECTIVE_POS_FLAT          = scenarioParams(40:43); % [x1,y1,x2,y2]; zero-padded if N=1
-        OBJECTIVE_VAR_FLAT          = scenarioParams(44:51); % [v11,v12,v21,v22 per component]
-        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(52);
-        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(53));
-        DAMPING_COEFF               = scenarioParams(54);
-        USE_FIXED_TOPOLOGY          = logical(scenarioParams(55));
-
-        % --- Build domain geometry ---
-        dom = rectangularPrism;
-        dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
-
-        % --- Build sensing objective: sum of N bivariate Gaussians (codegen-compatible) ---
-        dom.objective = sensingObjective;
-        xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]);
-        yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]);
-        [gridX, gridY] = meshgrid(xGrid, yGrid);
-        objValues = zeros(size(gridX));
-        for kk = 1:NUM_OBJ_COMPONENTS
-            pos_k = OBJECTIVE_POS_FLAT((kk-1)*2+1 : (kk-1)*2+2);
-            var_k = reshape(OBJECTIVE_VAR_FLAT((kk-1)*4+1 : (kk-1)*4+4), 2, 2);
-            dx = gridX - pos_k(1);
-            dy = gridY - pos_k(2);
-            ov_a = var_k(1,1); ov_b = var_k(1,2);
-            ov_c = var_k(2,1); ov_d = var_k(2,2);
-            ov_det = ov_a * ov_d - ov_b * ov_c;
-            objValues = objValues + exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
-        end
-        dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
-            DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
-
-        % --- Initialise agents from GPS positions (per-UAV parameters) ----
-        agentList = cell(numAgents, 1);
-        for ii = 1:numAgents
-            pos  = currentPositions(ii, :);
-
-            % Per-UAV sensor model
-            sensor = sigmoidSensor;
-            sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
-                                       ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
-
-            geom = spherical;
-            geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
-                                   sprintf("UAV %d Collision", ii));
-            ag = agent;
-            ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
-                               INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
-            agentList{ii} = ag;
-        end
-
-        % --- Build obstacle list from flat arrays ---
-        coder.varsize('obstacleList', [8, 1], [1, 0]);
-        obstacleList = repmat({rectangularPrism}, numObstacles, 1);
-        for ii = 1:numObstacles
-            obs = rectangularPrism;
-            obs = obs.initialize([obstacleMin(ii, :); obstacleMax(ii, :)], ...
-                                 REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
-            obstacleList{ii} = obs;
-        end
-
-        % --- Initialise simulation (plots and video disabled) ---
-        sim = miSim;
-        sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
-                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                             USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
-    end
-
-    % On the init call return current positions unchanged
-    for ii = 1:numAgents
-        nextPositions(ii, :) = currentPositions(ii, :);
-    end
-
-else
-    % =====================================================================
-    % One guidance step
-    % =====================================================================
-
-    % 1. Inject actual GPS positions (closed-loop feedback)
-    for ii = 1:size(sim.agents, 1)
-        sim.agents{ii}.lastPos = sim.agents{ii}.pos;
-        sim.agents{ii}.pos     = currentPositions(ii, :);
-
-        % Re-centre collision geometry at new position
-        d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center;
-        sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ...
-            sim.agents{ii}.collisionGeometry.center + d, ...
-            sim.agents{ii}.collisionGeometry.radius, ...
-            REGION_TYPE.COLLISION);
-    end
-
-    % 2. Advance timestep counter
-    sim.timestepIndex = sim.timestepIndex + 1;
-
-    % 3. Update communications topology (Lesser Neighbour Assignment)
-    if ~sim.useFixedTopology
-        sim = sim.lesserNeighbor();
-    end
-
-    % 4. Compute Voronoi partitioning
-    sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
-
-    % 5. Unconstrained gradient-ascent step for each agent
-    for ii = 1:size(sim.agents, 1)
-        sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, ...
-                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
-    end
-
-    % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
-    sim = sim.constrainMotion();
-
-    % 7. Return constrained next positions
-    for ii = 1:size(sim.agents, 1)
-        nextPositions(ii, :) = sim.agents{ii}.pos;
-    end
-
-end
-
-end

aerpaw/impl/controller_impl.cpp

@@ -1,677 +0,0 @@
-#include "controller_impl.h"
-#include <iostream>
-#include <vector>
-#include <string>
-#include <cstring>
-#include <cstdio>
-#include <limits>
-#include <sys/socket.h>
-#include <sys/select.h>
-#include <arpa/inet.h>
-#include <unistd.h>
-#include <time.h>
-
-#define SERVER_PORT 5000
-#define SERVER_IP "127.0.0.1"
-
-static int serverSocket = -1;
-static std::vector<int> clientSockets;
-static int guidanceStep = 0;
-static int guidanceTotalSteps = 0;
-static struct timespec lastStepTime = {0, 0};
-
-// During guidance returns "(%d/%d) "; outside guidance returns "HH:MM:SS ".
-static std::string logPrefix() {
-    if (guidanceStep > 0) {
-        char buf[32];
-        snprintf(buf, sizeof(buf), "(%d/%d) ", guidanceStep, guidanceTotalSteps);
-        return std::string(buf);
-    }
-    time_t now = time(nullptr);
-    struct tm* lt = localtime(&now);
-    char ts[16];
-    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
-    return std::string(ts) + " ";
-}
-
-void setGuidanceStep(int step, int totalSteps) {
-    struct timespec now;
-    clock_gettime(CLOCK_MONOTONIC, &now);
-
-    // From step 2 onward, elapsed = setGuidanceStep(N-1) → setGuidanceStep(N),
-    // which spans the full prior iteration: guidance computation + target send
-    // + flight + position request/receive.
-    if (step > 1 && lastStepTime.tv_sec != 0) {
-        double elapsed = (now.tv_sec - lastStepTime.tv_sec)
-                       + (now.tv_nsec - lastStepTime.tv_nsec) * 1e-9;
-        guidanceStep = step;
-        guidanceTotalSteps = totalSteps;
-        std::cout << logPrefix() << "Iteration duration: " << elapsed << " s\n";
-    } else {
-        guidanceStep = step;
-        guidanceTotalSteps = totalSteps;
-    }
-
-    lastStepTime = now;
-}
-
-void initSockets() {}
-void cleanupSockets() {}
-
-void initServer() {
-    initSockets();
-    serverSocket = socket(AF_INET, SOCK_STREAM, 0);
-    if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; }
-
-    sockaddr_in serverAddr;
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_addr.s_addr = INADDR_ANY;
-    serverAddr.sin_port = htons(SERVER_PORT);
-
-    int opt = 1;
-    setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt));
-
-    if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
-        std::cerr << "Bind failed\n"; return;
-    }
-    if(listen(serverSocket, 5) < 0) {
-        std::cerr << "Listen failed\n"; return;
-    }
-
-    std::cout << "Server initialized\n";
-}
-
-void acceptClient(int clientId) {
-    sockaddr_in clientAddr;
-    socklen_t addrLen = sizeof(clientAddr);
-    int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen);
-    if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; }
-    clientSockets.push_back(clientSock);
-    std::cout << "Client " << clientId << " connected\n";
-}
-
-void closeServer() {
-    for(auto sock : clientSockets) {
-        close(sock);
-    }
-    close(serverSocket);
-    cleanupSockets();
-}
-
-// Load target coordinates from file
-// File format: one line per UAV with "x,y,z" coordinates
-// Returns number of targets loaded
-int loadTargets(const char* filename, double* targets, int maxClients) {
-    FILE* file = fopen(filename, "r");
-    if (!file) {
-        std::cerr << "Failed to open config file: " << filename << "\n";
-        return 0;
-    }
-
-    int count = 0;
-    char line[256];
-    bool inTargets = false;
-
-    // Simple YAML parser for targets section
-    // Expects format:
-    //   targets:
-    //     - [x, y, z]
-    //     - [x, y, z]
-    while (fgets(line, sizeof(line), file) && count < maxClients) {
-        // Check if we've entered the targets section
-        if (strstr(line, "targets:") != nullptr) {
-            inTargets = true;
-            continue;
-        }
-
-        // If we hit another top-level key (no leading whitespace), exit targets section
-        if (inTargets && line[0] != ' ' && line[0] != '\t' && line[0] != '\n' && line[0] != '#') {
-            break;
-        }
-
-        // Parse target entries: "  - [x, y, z]"
-        if (inTargets) {
-            double x, y, z;
-            // Try to match the array format
-            if (sscanf(line, " - [%lf, %lf, %lf]", &x, &y, &z) == 3) {
-                // MATLAB uses column-major order, so for a maxClients x 3 matrix:
-                // Column 1 (x): indices 0, 1, 2, ...
-                // Column 2 (y): indices maxClients, maxClients+1, ...
-                // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ...
-                targets[count + 0 * maxClients] = x;
-                targets[count + 1 * maxClients] = y;
-                targets[count + 2 * maxClients] = z;
-                std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n";
-                count++;
-            }
-        }
-    }
-
-    fclose(file);
-    return count;
-}
-
-// ---------------------------------------------------------------------------
-// Scenario CSV loading
-// ---------------------------------------------------------------------------
-
-// Split a CSV data row into fields, respecting quoted commas.
-// Inserts NUL terminators into `line` in place.
-// Returns the number of fields found.
-static int splitCSVRow(char* line, char** fields, int maxFields) {
-    int n = 0;
-    bool inQuote = false;
-    if (n < maxFields) fields[n++] = line;
-    for (char* p = line; *p && *p != '\n' && *p != '\r'; ++p) {
-        if (*p == '"') {
-            inQuote = !inQuote;
-        } else if (*p == ',' && !inQuote && n < maxFields) {
-            *p = '\0';
-            fields[n++] = p + 1;
-        }
-    }
-    // NUL-terminate the last field (strip trailing newline)
-    for (char* p = fields[n - 1]; *p; ++p) {
-        if (*p == '\n' || *p == '\r') { *p = '\0'; break; }
-    }
-    return n;
-}
-
-// Trim leading/trailing ASCII whitespace and remove enclosing double-quotes.
-// Modifies the string in place and returns the start of the trimmed content.
-static char* trimField(char* s) {
-    // Trim leading whitespace
-    while (*s == ' ' || *s == '\t') ++s;
-    // Remove enclosing quotes
-    size_t len = strlen(s);
-    if (len >= 2 && s[0] == '"' && s[len - 1] == '"') {
-        s[len - 1] = '\0';
-        ++s;
-    }
-    // Trim trailing whitespace
-    char* end = s + strlen(s) - 1;
-    while (end > s && (*end == ' ' || *end == '\t')) { *end-- = '\0'; }
-    return s;
-}
-
-// Open scenario.csv, skip the header row, return the data row in `line`.
-// Returns 1 on success, 0 on failure.
-static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
-    FILE* f = fopen(filename, "r");
-    if (!f) {
-        std::cerr << "loadScenario: cannot open " << filename << "\n";
-        return 0;
-    }
-    // Skip header row
-    if (!fgets(line, lineSize, f)) { fclose(f); return 0; }
-    // Read data row
-    int ok = (fgets(line, lineSize, f) != NULL);
-    fclose(f);
-    return ok ? 1 : 0;
-}
-
-// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
-// Index mapping (0-based):
-//   0-7  : timestep, maxIter, minAlt, discretizationStep, protectedRange,
-//           initialStepSize, barrierGain, barrierExponent
-//   8-11 : collisionRadius[1:4]  (per-UAV, MAX_CLIENTS_PER_PARAM slots)
-//   12-15: comRange[1:4]
-//   16-19: alphaDist[1:4]
-//   20-23: betaDist[1:4]
-//   24-27: alphaTilt[1:4]
-//   28-31: betaTilt[1:4]
-//   32-34: domainMin  (east, north, up)
-//   35-37: domainMax  (east, north, up)
-//   38   : numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   39-42: objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   43-50: objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
-//   51   : sensorPerformanceMinimum  (CSV column 18)
-//   52   : useDoubleIntegrator       (CSV column 23)
-//   53   : dampingCoeff              (CSV column 24)
-//   54   : useFixedTopology          (CSV column 25)
-// Returns 1 on success, 0 on failure.
-int loadScenario(const char* filename, double* params) {
-    char line[4096];
-    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
-
-    char copy[4096];
-    strncpy(copy, line, sizeof(copy) - 1);
-    copy[sizeof(copy) - 1] = '\0';
-
-    char* fields[32];
-    int nf = splitCSVRow(copy, fields, 32);
-    if (nf < 26) {
-        fprintf(stderr, "loadScenario: expected >=26 columns, got %d\n", nf);
-        return 0;
-    }
-
-    // Zero-initialise all params so unused per-UAV slots default to 0
-    for (int i = 0; i < NUM_SCENARIO_PARAMS; i++) params[i] = 0.0;
-
-    // Scalar fields (columns 0–7): timestep..barrierExponent
-    for (int i = 0; i < 8; i++) {
-        params[i] = atof(trimField(fields[i]));
-    }
-
-    // Per-UAV fields (columns 8–13): collisionRadius, comRange,
-    // alphaDist, betaDist, alphaTilt, betaTilt.
-    // Each is a quoted comma-separated list with up to MAX_CLIENTS_PER_PARAM values.
-    // Stored in params as consecutive blocks of MAX_CLIENTS_PER_PARAM (4) slots.
-    {
-        const int perUavCols[] = {8, 9, 10, 11, 12, 13};
-        const int perUavOffsets[] = {8, 12, 16, 20, 24, 28};  // 0-based param indices
-        const char* perUavNames[] = {"collisionRadius", "comRange",
-                                     "alphaDist", "betaDist",
-                                     "alphaTilt", "betaTilt"};
-        for (int p = 0; p < 6; p++) {
-            char tmp[256];
-            strncpy(tmp, fields[perUavCols[p]], sizeof(tmp) - 1);
-            tmp[sizeof(tmp) - 1] = '\0';
-            char* t = trimField(tmp);
-            // Parse comma-separated values
-            double vals[4] = {0, 0, 0, 0};
-            int count = 0;
-            char* tok = strtok(t, ",");
-            while (tok && count < MAX_CLIENTS_PER_PARAM) {
-                vals[count++] = atof(tok);
-                tok = strtok(nullptr, ",");
-            }
-            if (count == 0) {
-                fprintf(stderr, "loadScenario: failed to parse %s\n", perUavNames[p]);
-                return 0;
-            }
-            for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) {
-                params[perUavOffsets[p] + k] = vals[k];
-            }
-        }
-    }
-
-    // domainMin: column 14, format "east, north, up"
-    {
-        char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf , %lf", &params[32], &params[33], &params[34]) != 3) {
-            fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t);
-            return 0;
-        }
-    }
-
-    // domainMax: column 15
-    {
-        char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf , %lf", &params[35], &params[36], &params[37]) != 3) {
-            fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t);
-            return 0;
-        }
-    }
-
-    // objectivePos: column 16 — 2 values per component (up to 2 components).
-    // Infer numObjectiveComponents from the number of values parsed.
-    {
-        char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        double posVals[4] = {0, 0, 0, 0};
-        int posCount = 0;
-        char* tok = strtok(t, ",");
-        while (tok && posCount < 4) {
-            posVals[posCount++] = atof(tok);
-            tok = strtok(nullptr, ",");
-        }
-        // Check for a 5th token — would mean > 2 components
-        if (tok) {
-            fprintf(stderr, "loadScenario: at most 2 objective Gaussian components supported (objectivePos has >4 values)\n");
-            return 0;
-        }
-        if (posCount == 0 || posCount % 2 != 0) {
-            fprintf(stderr, "loadScenario: objectivePos must have 2 or 4 values, got %d\n", posCount);
-            return 0;
-        }
-        int nObj = posCount / 2;
-        params[38] = (double)nObj;
-        for (int k = 0; k < 4; k++) params[39 + k] = posVals[k];  // zero-padded for nObj=1
-    }
-
-    // objectiveVar: column 17 — 4 values per component (v11,v12,v21,v22).
-    {
-        char tmp[512]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        int nObj = (int)params[38];
-        double varVals[8] = {0, 0, 0, 0, 0, 0, 0, 0};
-        int varCount = 0;
-        char* tok = strtok(t, ",");
-        while (tok && varCount < 8) {
-            varVals[varCount++] = atof(tok);
-            tok = strtok(nullptr, ",");
-        }
-        if (varCount != nObj * 4) {
-            fprintf(stderr, "loadScenario: objectiveVar has %d values but expected %d (4 per component)\n",
-                    varCount, nObj * 4);
-            return 0;
-        }
-        for (int k = 0; k < 8; k++) params[43 + k] = varVals[k];  // zero-padded for nObj=1
-    }
-
-    // sensorPerformanceMinimum: column 18
-    {
-        char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[51] = atof(trimField(tmp));
-    }
-
-    // useDoubleIntegrator: column 23
-    {
-        char tmp[64]; strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[52] = atof(trimField(tmp));
-    }
-
-    // dampingCoeff: column 24
-    {
-        char tmp[64]; strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[53] = atof(trimField(tmp));
-    }
-
-    // useFixedTopology: column 25
-    {
-        char tmp[64]; strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[54] = atof(trimField(tmp));
-    }
-
-    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g], %d objective component(s)\n",
-           params[32], params[33], params[34], params[35], params[36], params[37], (int)params[38]);
-    return 1;
-}
-
-// Load initial UAV positions from scenario.csv (column 19: initialPositions).
-// targets is a column-major [maxClients x 3] array (same layout as loadTargets):
-//   targets[i + 0*maxClients] = east
-//   targets[i + 1*maxClients] = north
-//   targets[i + 2*maxClients] = up
-// Returns number of positions loaded.
-int loadInitialPositions(const char* filename, double* targets, int maxClients) {
-    char line[4096];
-    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
-
-    char copy[4096];
-    strncpy(copy, line, sizeof(copy) - 1);
-    copy[sizeof(copy) - 1] = '\0';
-
-    char* fields[32];
-    int nf = splitCSVRow(copy, fields, 32);
-    if (nf < 20) {
-        fprintf(stderr, "loadInitialPositions: expected >=20 columns, got %d\n", nf);
-        return 0;
-    }
-
-    // Column 19: initialPositions flat "x1,y1,z1, x2,y2,z2, ..."
-    char tmp[1024]; strncpy(tmp, fields[19], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-    char* t = trimField(tmp);
-
-    double vals[3 * 4];  // up to MAX_CLIENTS triples
-    int parsed = 0;
-    char* tok = strtok(t, ",");
-    while (tok && parsed < 3 * maxClients) {
-        vals[parsed++] = atof(tok);
-        tok = strtok(nullptr, ",");
-    }
-
-    int count = parsed / 3;
-    for (int i = 0; i < count; i++) {
-        targets[i + 0 * maxClients] = vals[i * 3 + 0];  // east
-        targets[i + 1 * maxClients] = vals[i * 3 + 1];  // north
-        targets[i + 2 * maxClients] = vals[i * 3 + 2];  // up
-    }
-
-    printf("Loaded %d initial position(s) from scenario\n", count);
-    return count;
-}
-
-// Load obstacle bounding-box corners from scenario.csv.
-// Columns used: 20 (numObstacles), 21 (obstacleMin flat), 22 (obstacleMax flat).
-// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays:
-//   obstacleMin[obs + 0*maxObstacles] = east_min
-//   obstacleMin[obs + 1*maxObstacles] = north_min
-//   obstacleMin[obs + 2*maxObstacles] = up_min
-// Returns number of obstacles loaded (0 if none or on error).
-int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax,
-                  int maxObstacles) {
-    char line[4096];
-    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
-
-    char copy[4096];
-    strncpy(copy, line, sizeof(copy) - 1);
-    copy[sizeof(copy) - 1] = '\0';
-
-    char* fields[32];
-    int nf = splitCSVRow(copy, fields, 32);
-    if (nf < 23) return 0;
-
-    // Column 20: numObstacles
-    int n = (int)atof(trimField(fields[20]));
-    if (n <= 0) return 0;
-    if (n > maxObstacles) {
-        fprintf(stderr, "loadObstacles: %d obstacles exceeds MAX_OBSTACLES=%d\n", n, maxObstacles);
-        n = maxObstacles;
-    }
-
-    // Column 21: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..."
-    {
-        char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        double vals[3 * 8];  // up to MAX_OBSTACLES triples
-        int parsed = 0;
-        char* tok = strtok(t, ",");
-        while (tok && parsed < 3 * n) {
-            vals[parsed++] = atof(tok);
-            tok = strtok(nullptr, ",");
-        }
-        if (parsed < 3 * n) {
-            fprintf(stderr, "loadObstacles: obstacleMin has fewer values than expected\n");
-            return 0;
-        }
-        for (int obs = 0; obs < n; obs++) {
-            obstacleMin[obs + 0 * maxObstacles] = vals[obs * 3 + 0];  // east
-            obstacleMin[obs + 1 * maxObstacles] = vals[obs * 3 + 1];  // north
-            obstacleMin[obs + 2 * maxObstacles] = vals[obs * 3 + 2];  // up
-        }
-    }
-
-    // Column 22: obstacleMax flat
-    {
-        char tmp[1024]; strncpy(tmp, fields[22], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        double vals[3 * 8];
-        int parsed = 0;
-        char* tok = strtok(t, ",");
-        while (tok && parsed < 3 * n) {
-            vals[parsed++] = atof(tok);
-            tok = strtok(nullptr, ",");
-        }
-        if (parsed < 3 * n) {
-            fprintf(stderr, "loadObstacles: obstacleMax has fewer values than expected\n");
-            return 0;
-        }
-        for (int obs = 0; obs < n; obs++) {
-            obstacleMax[obs + 0 * maxObstacles] = vals[obs * 3 + 0];
-            obstacleMax[obs + 1 * maxObstacles] = vals[obs * 3 + 1];
-            obstacleMax[obs + 2 * maxObstacles] = vals[obs * 3 + 2];
-        }
-    }
-
-    printf("Loaded %d obstacle(s) from scenario\n", n);
-    return n;
-}
-
-// Message type names for logging
-static const char* messageTypeName(uint8_t msgType) {
-    switch (msgType) {
-        case 1: return "TARGET";
-        case 2: return "ACK";
-        case 3: return "READY";
-        case 4: return "RTL";
-        case 5: return "LAND";
-        case 6: return "GUIDANCE_TOGGLE";
-        case 7: return "REQUEST_POSITION";
-        case 8: return "POSITION";
-        default: return "UNKNOWN";
-    }
-}
-
-// Send a single-byte message type to a client (no logging)
-static int sendMessageTypeRaw(int clientId, int msgType) {
-    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-    uint8_t msg = (uint8_t)msgType;
-    ssize_t sent = send(clientSockets[clientId - 1], &msg, 1, 0);
-    if (sent != 1) {
-        std::cerr << "Send failed for client " << clientId << "\n";
-        return 0;
-    }
-    return 1;
-}
-
-// Send a single-byte message type to a client
-int sendMessageType(int clientId, int msgType) {
-    if (!sendMessageTypeRaw(clientId, msgType)) return 0;
-    std::cout << logPrefix() << "Sent " << messageTypeName((uint8_t)msgType) << " to client " << clientId << "\n";
-    return 1;
-}
-
-// Send TARGET message with coordinates (1 byte type + 24 bytes coords)
-int sendTarget(int clientId, const double* coords) {
-    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-
-    // Build message: 1 byte type + 3 doubles (little-endian)
-    uint8_t buffer[1 + 3 * sizeof(double)];
-    buffer[0] = 1;  // TARGET = 1
-    memcpy(buffer + 1, coords, 3 * sizeof(double));
-
-    ssize_t sent = send(clientSockets[clientId - 1], buffer, sizeof(buffer), 0);
-    if (sent != sizeof(buffer)) {
-        std::cerr << "Send target failed for client " << clientId << "\n";
-        return 0;
-    }
-
-    std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
-              << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
-    return 1;
-}
-
-// Wait for a specific message type from ALL clients simultaneously using select()
-// Returns 1 if all clients responded with expected message type, 0 on failure
-int waitForAllMessageType(int numClients, int expectedType) {
-    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
-
-    uint8_t expected = (uint8_t)expectedType;
-    std::vector<bool> completed(numClients, false);
-    int completedCount = 0;
-
-    while (completedCount < numClients) {
-        // Build fd_set for select()
-        fd_set readfds;
-        FD_ZERO(&readfds);
-        int maxfd = -1;
-
-        for (int i = 0; i < numClients; i++) {
-            if (!completed[i]) {
-                FD_SET(clientSockets[i], &readfds);
-                if (clientSockets[i] > maxfd) maxfd = clientSockets[i];
-            }
-        }
-
-        // Wait for any socket to have data
-        int ready = select(maxfd + 1, &readfds, nullptr, nullptr, nullptr);
-        if (ready <= 0) return 0;
-
-        // Check each socket
-        for (int i = 0; i < numClients; i++) {
-            if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
-                uint8_t msgType;
-                int len = recv(clientSockets[i], &msgType, 1, 0);
-                if (len == 0) {
-                    std::cerr << "waitForAllMessageType: client " << (i + 1)
-                              << " disconnected while waiting for "
-                              << messageTypeName(expected) << "\n";
-                    return 0;
-                }
-                if (len < 0) {
-                    std::cerr << "waitForAllMessageType: recv error for client " << (i + 1)
-                              << " while waiting for " << messageTypeName(expected) << "\n";
-                    return 0;
-                }
-
-                if (msgType == expected) {
-                    completed[i] = true;
-                    completedCount++;
-                } else {
-                    std::cerr << logPrefix() << "Unexpected " << messageTypeName(msgType)
-                              << " from client " << (i + 1)
-                              << " (expected " << messageTypeName(expected) << ")\n";
-                }
-            }
-        }
-    }
-
-    std::cout << logPrefix() << "Received " << messageTypeName(expected) << " from all clients\n";
-    return 1;
-}
-
-// Broadcast GUIDANCE_TOGGLE to all clients
-void sendGuidanceToggle(int numClients) {
-    for (int i = 1; i <= numClients; i++) {
-        sendMessageTypeRaw(i, 6);  // GUIDANCE_TOGGLE = 6
-    }
-    std::cout << logPrefix() << "Sent GUIDANCE_TOGGLE to clients\n";
-}
-
-// Send REQUEST_POSITION to all clients
-int sendRequestPositions(int numClients) {
-    for (int i = 1; i <= numClients; i++) {
-        if (!sendMessageTypeRaw(i, 7)) return 0;  // REQUEST_POSITION = 7
-    }
-    std::cout << logPrefix() << "Sent REQUEST_POSITION to clients\n";
-    return 1;
-}
-
-// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients.
-// Stores results in column-major order: positions[client + 0*maxClients] = x (east),
-//                                       positions[client + 1*maxClients] = y (north),
-//                                       positions[client + 2*maxClients] = z (up)
-int recvPositions(int numClients, double* positions, int maxClients) {
-    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
-
-    for (int i = 0; i < numClients; i++) {
-        // Expect: POSITION byte (1) + 3 doubles (24)
-        uint8_t msgType;
-        if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) {
-            std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n";
-            return 0;
-        }
-        if (msgType != 8) {  // POSITION = 8
-            std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType
-                      << " from client " << (i + 1) << "\n";
-            return 0;
-        }
-
-        double coords[3];
-        if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) {
-            std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n";
-            return 0;
-        }
-
-        // Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up
-        positions[i + 0 * maxClients] = coords[0];  // east  (x)
-        positions[i + 1 * maxClients] = coords[1];  // north (y)
-        positions[i + 2 * maxClients] = coords[2];  // up    (z)
-
-        std::cout << logPrefix() << "Position from client " << (i + 1) << ": "
-                  << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
-    }
-    return 1;
-}
-
-// Sleep for the given number of milliseconds
-void sleepMs(int ms) {
-    struct timespec ts;
-    ts.tv_sec  = ms / 1000;
-    ts.tv_nsec = (ms % 1000) * 1000000L;
-    nanosleep(&ts, nullptr);
-}

aerpaw/impl/controller_impl.h

@@ -1,76 +0,0 @@
-#ifndef CONTROLLER_IMPL_H
-#define CONTROLLER_IMPL_H
-
-#include <stdint.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// Server lifecycle
-void initServer();
-void acceptClient(int clientId);
-void closeServer();
-
-// Configuration
-int loadTargets(const char* filename, double* targets, int maxClients);
-
-// Scenario loading (scenario.csv)
-// Number of elements in the flat params array passed to guidance_step.
-// Indices (0-based):
-//   0-7   scalars (timestep..barrierExponent)
-//   8-11  collisionRadius[1:4]  (per-UAV, MAX_CLIENTS slots)
-//   12-15 comRange[1:4]
-//   16-19 alphaDist[1:4]
-//   20-23 betaDist[1:4]
-//   24-27 alphaTilt[1:4]
-//   28-31 betaTilt[1:4]
-//   32-34 domainMin
-//   35-37 domainMax
-//   38    numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   39-42 objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   43-50 objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
-//   51    sensorPerformanceMinimum
-//   52    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
-//   53    dampingCoeff
-//   54    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
-#define NUM_SCENARIO_PARAMS 55
-#define MAX_CLIENTS_PER_PARAM 4
-// Maximum number of obstacles (upper bound for pre-allocated arrays).
-#define MAX_OBSTACLES 8
-
-// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
-// Returns 1 on success, 0 on failure.
-int loadScenario(const char* filename, double* params);
-
-// Load initial UAV positions from scenario.csv (initialPositions column).
-// targets is a column-major [maxClients x 3] array (same layout as loadTargets).
-// Returns number of positions loaded.
-int loadInitialPositions(const char* filename, double* targets, int maxClients);
-
-// Load obstacle bounding-box corners from scenario.csv.
-// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays
-// (same layout convention as loadTargets / recvPositions).
-// Returns the number of obstacles loaded.
-int loadObstacles(const char* filename,
-                  double* obstacleMin,
-                  double* obstacleMax,
-                  int maxObstacles);
-
-// Binary protocol operations
-int sendMessageType(int clientId, int msgType);
-int sendTarget(int clientId, const double* coords);
-int waitForAllMessageType(int numClients, int expectedType);
-
-// Guidance loop operations
-void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration
-void sendGuidanceToggle(int numClients);
-int  sendRequestPositions(int numClients);
-int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
-void sleepMs(int ms);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // CONTROLLER_IMPL_H

aerpaw/impl/controller_main.cpp

@@ -1,21 +0,0 @@
-#include <iostream>
-#include "controller.h"
-#include "controller_impl.h" // TCP implementation header
-
-int main() {
-    // Derive numClients from initialPositions in scenario.csv
-    double targets[MAX_CLIENTS_PER_PARAM * 3];
-    int numClients = loadInitialPositions("config/scenario.csv",
-                                          targets, MAX_CLIENTS_PER_PARAM);
-    if (numClients < 1) {
-        std::cerr << "Failed to parse numClients from scenario.csv\n";
-        return 1;
-    }
-    std::cout << "Parsed " << numClients << " UAV(s) from scenario.csv\n";
-
-    // Call MATLAB-generated server function
-    controller(numClients);
-
-    std::cout << "Server finished.\n";
-    return 0;
-}

aerpaw/impl/tmwtypes.h

@@ -1,888 +0,0 @@
-/*
- * Copyright 1984-2023 The MathWorks, Inc.
- */
-
-#if defined(_MSC_VER)
-# pragma once
-#endif
-#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3))
-# pragma once
-#endif
-
-#ifndef tmwtypes_h
-#define tmwtypes_h
-
-#ifndef __TMWTYPES__
-#define __TMWTYPES__
-/*
- * File    : tmwtypes.h
- * Abstract:
- *      Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
- *
- *      When compiling stand-alone model code, data types can be overridden
- *      via compiler switches.
- *
- *      Define NO_FLOATS to eliminate reference to real_T, etc.
- */
-
-#ifdef MW_LIBTOOLING
-#include "mwstdint.h"
-#endif
-
-#include <limits.h>
-
-/* __STDC_VERSION__ version check below means "check for a C99 compiler".
-
-   Visual Studio (checked on versions 2015 and 2017) does
-   not define __STDC_VERSION__, however  it has stdbool.h available,
-   thus a separate check for _MSC_VER below.
- */
-#if defined(__APPLE_CC__) \
-    || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \
-    || (defined(_MSC_VER) && (_MSC_VER >= 1900))
-#ifndef tmwtypes_do_not_include_stdbool
-#include <stdbool.h>
-#endif
-#endif
-
-#define LOGICAL_IS_A_TYPE
-#define SPARSE_GENERALIZATION
-
-#ifdef NO_FLOATS
-# define double double_not_allowed
-# define float  float_not_allowed
-#endif /*NO_FLOATS*/
-
-#ifndef NO_FLOATS
-
-#ifndef __MWERKS__
-# ifdef __STDC__
-#  include <float.h>
-# else
-#  ifndef FLT_MANT_DIG
-#   define FLT_MANT_DIG 24
-#  endif
-#  ifndef DBL_MANT_DIG
-#   define DBL_MANT_DIG 53
-#  endif
-# endif
-#endif
-
-#endif /*NO_FLOATS*/
-
-/*
- *      The following data types cannot be overridden when building MEX files.
- */
-#ifdef MATLAB_MEX_FILE
-# undef CHARACTER_T
-# undef INTEGER_T
-# undef BOOLEAN_T
-# undef REAL_T
-# undef TIME_T
-#endif
-
-/*
- * The uchar_T, ushort_T and ulong_T types are needed for compilers which do 
- * not allow defines to be specified, at the command line, with spaces in them.
- */
-
-typedef unsigned char       uchar_T;
-typedef unsigned short      ushort_T;
-typedef unsigned long       ulong_T;
-
-#if (defined(_MSC_VER) && _MSC_VER >= 1500)      \
-    || defined(__x86_64__) || defined(__LP64__)  \
-    || defined(__LCC64__)
-    
-typedef unsigned long long  ulonglong_T;
-#endif
-
-
-
-/*=======================================================================*
- * Fixed width word size data types:                                     *
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
- *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
- *=======================================================================*/
-
-/* When used with Real Time Workshop generated code, this
- * header file can be used with a variety of compilers.
- *
- * The compiler could be for an 8 bit embedded processor that
- * only had 8 bits per integer and 16 bits per long.
- * In that example, a 32 bit integer size is not even available.
- * This header file should be robust to that.   
- *
- * For the case of an 8 bit processor, the preprocessor
- * may be limited to 16 bit math like its target.  That limitation 
- * would mean that 32 bit comparisons can't be done accurately.  
- * To increase robustness to this, comparisons are done against
- * smaller values first.  An inaccurate 32 bit comparison isn't
- * attempted if the 16 bit comparison has already succeeded.
- *
- * Limitations on preprocessor math can also be stricter than
- * for the target.  There are known cases where a compiler
- * targeting processors with 64 bit longs can't do accurate
- * preprocessor comparisons on more than 32 bits.  
- */
-
-/* Determine the number of bits for int, long, short, and char.
- * If one fails to be determined, set the number of bits to -1
- */
-
-#ifndef TMW_BITS_PER_INT
-# if   INT_MAX  == 0x7FL
-#  define TMW_BITS_PER_INT 8
-# elif INT_MAX  == 0x7FFFL
-#  define TMW_BITS_PER_INT 16
-# elif INT_MAX  == 0x7FFFFFFFL
-#  define TMW_BITS_PER_INT 32
-# else
-#  define TMW_BITS_PER_INT -1
-# endif
-#endif
-
-#ifndef TMW_BITS_PER_LONG
-# if   LONG_MAX  == 0x7FL
-#  define TMW_BITS_PER_LONG 8
-# elif LONG_MAX  == 0x7FFFL
-#  define TMW_BITS_PER_LONG 16
-# elif LONG_MAX  == 0x7FFFFFFFL
-#  define TMW_BITS_PER_LONG 32
-# else
-#  define TMW_BITS_PER_LONG -1
-# endif
-#endif
-
-#ifndef TMW_BITS_PER_SHRT
-# if   SHRT_MAX  == 0x7FL
-#  define TMW_BITS_PER_SHRT 8
-# elif SHRT_MAX  == 0x7FFFL
-#  define TMW_BITS_PER_SHRT 16
-# elif SHRT_MAX  == 0x7FFFFFFFL
-#  define TMW_BITS_PER_SHRT 32
-# else
-#  define TMW_BITS_PER_SHRT -1
-# endif
-#endif
-
-#ifndef TMW_BITS_PER_SCHAR
-# if   SCHAR_MAX  == 0x7FL
-#  define TMW_BITS_PER_SCHAR 8
-# elif SCHAR_MAX  == 0x7FFFL
-#  define TMW_BITS_PER_SCHAR 16
-# elif SCHAR_MAX  == 0x7FFFFFFFL
-#  define TMW_BITS_PER_SCHAR 32
-# else
-#  define TMW_BITS_PER_SCHAR -1
-# endif
-#endif
-
-#ifndef TMW_CHAR_SIGNED
-# if SCHAR_MAX == CHAR_MAX
-#  define TMW_CHAR_SIGNED 1
-# else
-#  define TMW_CHAR_SIGNED 0
-# endif
-#endif
-
-/* It is common for one or more of the integer types
- * to be the same size.  For example, on many embedded
- * processors, both shorts and ints are 16 bits.  On
- * processors used for workstations, it is quite common
- * for both int and long to be 32 bits.  
- *   When there is more than one choice for typdef'ing
- * a portable type like int16_T or uint32_T, in
- * concept, it should not matter which choice is made.
- * However, some style guides and some code checking
- * tools do identify and complain about seemingly
- * irrelevant differences.  For example, a code
- * checking tool may complain about an implicit
- * conversion from int to short even though both
- * are 16 bits.  To reduce these types of
- * complaints, it is best to make int the
- * preferred choice when more than one is available.
- */
-
-#ifndef INT8_T
-# if   defined(MW_LIBTOOLING)
-#  define INT8_T  int8_t
-# elif TMW_BITS_PER_INT   == 8
-#  define  INT8_T int
-# elif TMW_BITS_PER_LONG  == 8
-#  define  INT8_T long
-# elif TMW_BITS_PER_SCHAR == 8
-#  define  INT8_T signed char
-# elif TMW_BITS_PER_SHRT  == 8
-#  define  INT8_T short
-# endif
-#endif
-#ifdef INT8_T
- typedef INT8_T int8_T;
-#endif
-
-#ifndef UINT8_T
-# if   defined(MW_LIBTOOLING)
-#  define  UINT8_T uint8_t
-# elif   TMW_BITS_PER_INT   == 8
-#  define  UINT8_T unsigned int
-# elif TMW_BITS_PER_LONG  == 8
-#  define  UINT8_T unsigned long
-# elif TMW_BITS_PER_SCHAR == 8
-#  define  UINT8_T unsigned char
-# elif TMW_BITS_PER_SHRT  == 8
-#  define  UINT8_T unsigned short
-# endif
-#endif
-#ifdef UINT8_T
- typedef UINT8_T uint8_T;
-#endif
-
-
-#ifndef INT16_T
-# if   defined(MW_LIBTOOLING)
-#  define  INT16_T int16_t
-# elif   TMW_BITS_PER_INT   == 16
-#  define  INT16_T int
-# elif TMW_BITS_PER_LONG  == 16
-#  define  INT16_T long
-# elif TMW_BITS_PER_SCHAR == 16
-#  define  INT16_T signed char
-# elif TMW_BITS_PER_SHRT  == 16
-#  define  INT16_T short
-# endif
-#endif
-#ifdef INT16_T
- typedef INT16_T int16_T;
-#endif
-
-
-#ifndef UINT16_T
-# if   defined(MW_LIBTOOLING)
-#  define  UINT16_T uint16_t
-# elif TMW_BITS_PER_INT   == 16
-#  define  UINT16_T unsigned int
-# elif TMW_BITS_PER_LONG  == 16
-#  define  UINT16_T unsigned long
-# elif TMW_BITS_PER_SCHAR == 16
-#  define  UINT16_T unsigned char
-# elif TMW_BITS_PER_SHRT  == 16
-#  define  UINT16_T unsigned short
-# endif
-#endif
-#ifdef UINT16_T
- typedef UINT16_T uint16_T;
-#endif
-
-
-#ifndef INT32_T
-# if   defined(MW_LIBTOOLING)
-#  define  INT32_T int32_t
-# elif TMW_BITS_PER_INT   == 32
-#  define  INT32_T int
-# elif TMW_BITS_PER_LONG  == 32
-#  define  INT32_T long
-# elif TMW_BITS_PER_SCHAR == 32
-#  define  INT32_T signed char
-# elif TMW_BITS_PER_SHRT  == 32
-#  define  INT32_T short
-# endif
-#endif
-#ifdef INT32_T
- typedef INT32_T int32_T;
-#endif
-
-
-#ifndef UINT32_T
-# if   defined(MW_LIBTOOLING)
-#  define  UINT32_T uint32_t
-# elif TMW_BITS_PER_INT   == 32
-#  define  UINT32_T unsigned int
-# elif TMW_BITS_PER_LONG  == 32
-#  define  UINT32_T unsigned long
-# elif TMW_BITS_PER_SCHAR == 32
-#   define UINT32_T unsigned char
-# elif TMW_BITS_PER_SHRT  == 32
-#  define  UINT32_T unsigned short
-# endif
-#endif
-#ifdef UINT32_T
- typedef UINT32_T uint32_T;
-#endif
-
-/* The following is used to emulate smaller integer types when only
- * larger types are available.  For example, compilers for TI C3x/C4x DSPs 
- * define char and short to be 32 bits, so 8 and 16 bits are not directly
- * available.  This target is commonly used with RTW rapid prototyping.
- * Other DSPs define char to be 16 bits, so 8 bits is not directly 
- * available.
- */
-#ifndef INT8_T
-# ifdef INT16_T
-#  define INT8_T INT16_T
-   typedef INT8_T int8_T;
-# else
-#  ifdef INT32_T
-#   define INT8_T INT32_T
-    typedef INT8_T int8_T;
-#  endif
-# endif
-#endif
-
-#ifndef UINT8_T
-# ifdef UINT16_T
-#   define UINT8_T UINT16_T
-    typedef UINT8_T uint8_T;
-# else
-#  ifdef UINT32_T
-#   define UINT8_T UINT32_T
-    typedef UINT8_T uint8_T;
-#  endif
-# endif
-#endif
-
-#ifndef INT16_T
-# ifdef INT32_T
-#   define INT16_T INT32_T
-    typedef INT16_T int16_T;
-# endif
-#endif
-
-#ifndef UINT16_T
-# ifdef UINT32_T
-#   define UINT16_T UINT32_T
-    typedef UINT16_T uint16_T;
-# endif
-#endif
-
-
-#ifndef NO_FLOATS
-
-#ifndef REAL32_T
-# ifndef __MWERKS__
-#  if FLT_MANT_DIG >= 23
-#   define REAL32_T float
-#  endif
-# else
-#  define REAL32_T float
-# endif
-#endif
-#ifdef REAL32_T
- typedef REAL32_T real32_T;
-#endif
-
-
-#ifndef REAL64_T
-# ifndef __MWERKS__
-#  if DBL_MANT_DIG >= 52
-#   define REAL64_T double
-#  endif
-# else
-#  define REAL64_T double
-# endif
-#endif
-#ifdef REAL64_T
- typedef REAL64_T real64_T;
-#endif
-
-#endif /* NO_FLOATS*/
-
-/*=======================================================================*
- * Fixed width word size data types:                                     *
- *   int64_T                      - signed 64 bit integers               *
- *   uint64_T                     - unsigned 64 bit integers             *
- *=======================================================================*/
-
-# if   defined(MW_LIBTOOLING)
-#  ifdef INT64_T
-#    undef  INT64_T
-#  endif
-#  define  INT64_T int64_t
-#  ifdef UINT64_T
-#    undef  UINT64_T
-#  endif
-#  define  UINT64_T uint64_t
-# endif
-#if !defined(INT64_T) || !defined(UINT64_T) || !defined(FMT64)
-# if defined(__APPLE__) || defined(__clang__)
-#  ifndef INT64_T
-#   define INT64_T long long
-#  endif
-#  ifndef UINT64_T
-#   define UINT64_T unsigned long long
-#  endif
-#  ifndef FMT64
-#   define FMT64 "ll"
-#  endif
-#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
-#    define INT_TYPE_64_IS_LONG
-#  endif
-# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__) 
-#  ifndef INT64_T
-#   define INT64_T long
-#  endif
-#  ifndef UINT64_T
-#   define UINT64_T unsigned long
-#  endif
-#  ifndef FMT64
-#   define FMT64 "l"
-#  endif
-#  if !defined(INT_TYPE_64_IS_LONG)
-#    define INT_TYPE_64_IS_LONG
-#  endif
-# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
-                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
-#  ifndef INT64_T
-#   define INT64_T __int64
-#  endif
-#  ifndef UINT64_T
-#   define UINT64_T unsigned __int64
-#  endif
-#  ifndef FMT64
-#   define FMT64 "I64"
-#  endif
-# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
-                         || defined(__LCC64__)
-#  ifndef INT64_T
-#   define INT64_T long long
-#  endif
-#  ifndef UINT64_T
-#   define UINT64_T unsigned long long
-#  endif
-#  ifndef FMT64
-#   define FMT64 "ll"
-#  endif
-# endif
-
-#endif
-
-#if defined(INT64_T)
-# if defined(__GNUC__) && \
-    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
-  __extension__
-# endif
- typedef INT64_T int64_T;
-#endif
-
-#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \
-                    || defined(__x86_64__) \
-                    || defined(__LP64__)
-#  define INT_TYPE_64_IS_SUPPORTED
-#endif
-
-#if defined(UINT64_T)
-# if defined(__GNUC__) && \
-    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
-  __extension__
-# endif
- typedef UINT64_T uint64_T;
-#endif
-
-/*===========================================================================*
- * Format string modifiers for using size_t variables in printf statements.  *
- *===========================================================================*/
-
-#ifndef FMT_SIZE_T
-#  if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__)
-#    define FMT_SIZE_T "z"
-#  elif defined (__WATCOMC__)
-#    define FMT_SIZE_T "l"
-#  elif defined (_WIN32 )
-#    define FMT_SIZE_T "I"
-#  else
-#    define FMT_SIZE_T "l"
-#  endif
-#endif
-
-#ifndef FMT_PTRDIFF_T
-#  if defined(__APPLE__)
-#    define FMT_PTRDIFF_T "l"
-#  elif defined( __GNUC__ ) || defined(_STDC_C99)
-#    define FMT_PTRDIFF_T "t"
-#  elif defined (__WATCOMC__)
-#    define FMT_PTRDIFF_T "l"
-#  elif defined (_WIN32 )
-#    define FMT_PTRDIFF_T "I"
-#  else
-#    define FMT_PTRDIFF_T "l"
-#  endif
-#endif
-
-/*===========================================================================*
- * General or logical data types where the word size is not guaranteed.      *
- *  real_T     - possible settings include real32_T or real64_T              *
- *  time_T     - possible settings include real32_T or real64_T              *
- *  boolean_T                                                                *
- *  char_T                                                                   *
- *  int_T                                                                    *
- *  uint_T                                                                   *
- *  byte_T                                                                   *
- *===========================================================================*/
-
-#ifndef NO_FLOATS
-
-#ifndef REAL_T
-# ifdef REAL64_T
-#  define REAL_T real64_T
-# else
-#  ifdef REAL32_T
-#   define REAL_T real32_T
-#  endif
-# endif
-#endif
-#ifdef REAL_T
- typedef REAL_T real_T;
-#endif
-
-#ifndef TIME_T
-# ifdef REAL_T
-#  define TIME_T real_T
-# endif
-#endif
-#ifdef TIME_T
- typedef TIME_T time_T;
-#endif
-
-#endif /* NO_FLOATS */
-
-#ifndef BOOLEAN_T
-# if defined(UINT8_T)
-#  define BOOLEAN_T UINT8_T
-# else
-#  define BOOLEAN_T unsigned int
-# endif
-#endif
-typedef BOOLEAN_T boolean_T;
-
-
-#ifndef CHARACTER_T
-# define CHARACTER_T char
-#endif
-typedef CHARACTER_T char_T;
-
-
-#ifndef INTEGER_T
-# define INTEGER_T int
-#endif
-typedef INTEGER_T int_T;
-
-
-#ifndef UINTEGER_T
-# define UINTEGER_T unsigned
-#endif
-typedef UINTEGER_T uint_T;
-
-
-#ifndef BYTE_T
-# define BYTE_T unsigned char
-#endif
-typedef BYTE_T byte_T;
-
-
-/*===========================================================================*
- * Define Complex Structures                                                 *
- *===========================================================================*/
-#ifndef NO_FLOATS
-
-#ifndef CREAL32_T
-#  ifdef REAL32_T
-    typedef struct {
-      real32_T re, im;
-    } creal32_T;
-#    define CREAL32_T creal32_T
-#  endif
-#endif
-
-#ifndef CREAL64_T
-#  ifdef REAL64_T
-    typedef struct {
-      real64_T re, im;
-    } creal64_T;
-#    define CREAL64_T creal64_T
-#  endif
-#endif
-
-#ifndef CREAL_T
-#  ifdef REAL_T
-    typedef struct {
-      real_T re, im;
-    } creal_T;
-#    define CREAL_T creal_T
-#  endif
-#endif
-
-#endif /* NO_FLOATS */
-
-#ifndef CINT8_T
-#  ifdef INT8_T
-    typedef struct {
-      int8_T re, im;
-    } cint8_T;
-#    define CINT8_T cint8_T
-#  endif
-#endif
-
-#ifndef CUINT8_T
-#  ifdef UINT8_T
-    typedef struct {
-      uint8_T re, im;
-    } cuint8_T;
-#    define CUINT8_T cuint8_T
-#  endif
-#endif
-
-#ifndef CINT16_T
-#  ifdef INT16_T
-    typedef struct {
-      int16_T re, im;
-    } cint16_T;
-#    define CINT16_T cint16_T
-#  endif
-#endif
-
-#ifndef CUINT16_T
-#  ifdef UINT16_T
-    typedef struct {
-      uint16_T re, im;
-    } cuint16_T;
-#    define CUINT16_T cuint16_T
-#  endif
-#endif
-
-#ifndef CINT32_T
-#  ifdef INT32_T
-    typedef struct {
-      int32_T re, im;
-    } cint32_T;
-#    define CINT32_T cint32_T
-#  endif
-#endif
-
-#ifndef CUINT32_T
-#  ifdef UINT32_T
-    typedef struct {
-      uint32_T re, im;
-    } cuint32_T;
-#    define CUINT32_T cuint32_T
-#  endif
-#endif
-
-#ifndef CINT64_T
-#  ifdef INT64_T
-    typedef struct {
-      int64_T re, im;
-    } cint64_T;
-#    define CINT64_T cint64_T
-#  endif
-#endif
-
-#ifndef CUINT64_T
-#  ifdef UINT64_T
-    typedef struct {
-      uint64_T re, im;
-    } cuint64_T;
-#    define CUINT64_T cuint64_T
-#  endif
-#endif
-
-/*=======================================================================*
- * Min and Max:                                                          *
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
- *=======================================================================*/
-
-#define  MAX_int8_T      ((int8_T)(127))            /* 127  */
-#define  MIN_int8_T      ((int8_T)(-128))           /* -128 */
-#define  MAX_uint8_T     ((uint8_T)(255))           /* 255  */
-#define  MIN_uint8_T     ((uint8_T)(0))
-                           
-#define  MAX_int16_T     ((int16_T)(32767))         /* 32767 */
-#define  MIN_int16_T     ((int16_T)(-32768))        /* -32768 */
-#define  MAX_uint16_T    ((uint16_T)(65535))        /* 65535 */
-#define  MIN_uint16_T    ((uint16_T)(0))
-                           
-#define  MAX_int32_T     ((int32_T)(2147483647))    /* 2147483647  */
-#define  MIN_int32_T     ((int32_T)(-2147483647-1)) /* -2147483648 */
-#define  MAX_uint32_T    ((uint32_T)(0xFFFFFFFFU))  /* 4294967295  */
-#define  MIN_uint32_T    ((uint32_T)(0))
-
-#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
-                      || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100) \
-                      || defined(__LCC64__)
-#  ifdef INT64_T
-#    define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
-#    define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
-#  endif
-#  ifdef UINT64_T
-#    define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
-#    define  MIN_uint64_T    ((uint64_T)(0))
-#  endif
-#else
-#  ifdef INT64_T
-#    ifdef INT_TYPE_64_IS_LONG
-#      define  MAX_int64_T     ((int64_T)(9223372036854775807L))
-#      define  MIN_int64_T     ((int64_T)(-9223372036854775807L-1L))
-#    else
-#      define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
-#      define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
-#    endif
-#  endif
-#  ifdef UINT64_T
-#    ifdef INT_TYPE_64_IS_LONG
-#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
-#      define  MIN_uint64_T    ((uint64_T)(0))
-#    else
-#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
-#      define  MIN_uint64_T    ((uint64_T)(0))
-#    endif
-#  endif
-#endif
-
-#if (defined(_MSC_VER) && !defined(__clang__))
-
-/* Conversion from unsigned __int64 to double is not implemented in Visual Studio
- * and results in a compile error, thus the value must first be cast to
- * signed __int64, and then to double.
- *
- * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
- * the macro below provides a workaround for casting a uint64 value to a double
- * in windows.
- */
-#  define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
-            (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
-            (double)(__int64)(u) )
-
-/* The following inline function should only be used in the macro double_to_uint64,
- * as it only handles the specfic range of double between 2^63 and 2^64-1 */
-__forceinline
-uint64_T double_to_uint64_helper(double d) {
-  union double_to_uint64_union_type {
-    double dd;
-    uint64_T i64;
-  } di;
-  di.dd = d;
-  return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11);
-}
-
-/* The largest double value that can be cast to uint64 in windows is the
- * signed int64 max, which is 2^63-1. The macro below provides
- * a workaround for casting large double values to uint64 in windows.
- */
-/* The magic number 18446744073709551616.0 is 2^64 */
-/* The magic number 9223372036854775808.0 is 2^63 */
-#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
-            0xffffffffffffffffULL : \
-            ((d) >= 0.0) ? \
-            ((d) >= 9223372036854775808.0) ? \
-            double_to_uint64_helper(d) : \
-            (unsigned __int64)(d) : \
-            0ULL )
-#else
-#  define uint64_to_double(u) ((double)(u))
-#  if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
-/* double_to_uint64 defined only for MSVC and UNIX */
-#  else
-#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
-            (unsigned long long) 0xffffffffffffffffULL : \
-            ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 )
-#  endif
-#endif
-
-#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
-
-#ifndef _bool_T
-#define _bool_T
-
-typedef boolean_T bool;
-
-#ifndef false
-#define false (0)
-#endif
-#ifndef true 
-#define true (1)
-#endif
-
-#endif /* _bool_T */
-
-#endif /* !__cplusplus */
-
-/* 
- * This software assumes that the code is being compiled on a target using a 
- * 2's complement representation for signed integer values.
- */
-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
-#error "This code must be compiled using a 2's complement representation for signed integer values"
-#endif
-
-/*
- * Maximum length of a MATLAB identifier (function/variable/model)
- * including the null-termination character.
- */
-#define TMW_NAME_LENGTH_MAX 2049
-
-/*
- * Maximum values for indices and dimensions
- */
-#include <stddef.h>
-
-#ifdef MX_COMPAT_32
-typedef int mwSize;
-typedef int mwIndex;
-typedef int mwSignedIndex;
-#else
-typedef size_t    mwSize;         /* unsigned pointer-width integer */
-typedef size_t    mwIndex;        /* unsigned pointer-width integer */
-typedef ptrdiff_t mwSignedIndex;  /* a signed pointer-width integer */
-#endif
-
-                                  /* for the individual dim */
-/* If updating SLSize or SLIndex, update defintions in sl_types_def.h
-   as well. */
-#ifndef SLSIZE_SLINDEX
-  #define SLSIZE_SLINDEX
-  #ifdef INT_TYPE_64_IS_SUPPORTED
-    typedef int64_T SLIndex;
-    typedef int64_T SLSize;
-  #else
-    typedef int SLIndex;
-    typedef int SLSize;
-  #endif
-#endif
-
-/* for the total size */
-#define SLIndexType size_t
-#define INVALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max())
-#define MAX_VALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max() -1)
-
-
-#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
-/* Currently 2^48 based on hardware limitations */
-# define MWSIZE_MAX    281474976710655UL
-# define MWINDEX_MAX   281474976710655UL
-# define MWSINDEX_MAX  281474976710655L
-# define MWSINDEX_MIN -281474976710655L
-#else
-# define MWSIZE_MAX    2147483647UL
-# define MWINDEX_MAX   2147483647UL
-# define MWSINDEX_MAX  2147483647L
-# define MWSINDEX_MIN -2147483647L
-#endif
-#define MWSIZE_MIN    0UL
-#define MWINDEX_MIN   0UL
-
-/** UTF-16 character type */
-
-#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT)
-typedef char16_t CHAR16_T;
-#define U16_STRING_LITERAL_PREFIX u
-#elif defined(_MSC_VER)
-typedef wchar_t CHAR16_T;
-#define U16_STRING_LITERAL_PREFIX L
-#else
-typedef UINT16_T CHAR16_T;
-#endif
-
-#endif  /* __TMWTYPES__ */
-
-#endif /* tmwtypes_h */

aerpaw/loadTargetsFromYaml.m

@@ -1,42 +0,0 @@
-function targets = loadTargetsFromYaml(filename)
-%LOADTARGETSFROMYAML Load target coordinates from YAML config file
-%   targets = loadTargetsFromYaml(filename) reads the targets section
-%   from a YAML config file and returns an Nx3 matrix of [x, y, z] coordinates.
-
-targets = [];
-fid = fopen(filename, 'r');
-if fid == -1
-    error('Could not open file: %s', filename);
-end
-
-inTargets = false;
-while ~feof(fid)
-    line = fgetl(fid);
-    if ~ischar(line)
-        break;
-    end
-
-    % Check if we've entered the targets section
-    if contains(line, 'targets:')
-        inTargets = true;
-        continue;
-    end
-
-    % If we hit another top-level key, exit targets section
-    if inTargets && ~isempty(line) && line(1) ~= ' ' && line(1) ~= '#'
-        break;
-    end
-
-    % Parse target entries: "  - [x, y, z]"
-    if inTargets
-        % Extract numbers from array format
-        match = regexp(line, '\[\s*([-\d.]+)\s*,\s*([-\d.]+)\s*,\s*([-\d.]+)\s*\]', 'tokens');
-        if ~isempty(match)
-            coords = str2double(match{1});
-            targets = [targets; coords]; %#ok<AGROW>
-        end
-    end
-end
-
-fclose(fid);
-end

aerpaw/radio/CSwSNRTX.py

@@ -1,336 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-
-#
-# SPDX-License-Identifier: GPL-3.0
-#
-# GNU Radio Python Flow Graph
-# Title: CSwSNRTX
-# Author: Ozgur Ozdemir
-# Description: Channel Sounder Transmitter with offset freq
-# GNU Radio version: v3.8.5.0-6-g57bd109d
-
-from gnuradio import analog
-from gnuradio import blocks
-from gnuradio import digital
-from gnuradio import filter
-from gnuradio.filter import firdes
-from gnuradio import gr
-import sys
-import signal
-import threading
-from argparse import ArgumentParser
-from gnuradio.eng_arg import eng_float, intx
-from gnuradio import eng_notation
-from gnuradio import uhd
-import time
-
-
-def derive_num_uavs_from_csv(csv_path=None):
-    """Derive number of UAVs from scenario.csv by counting initial positions.
-
-    The initialPositions column contains a quoted comma-separated list of
-    x,y,z triples.  num_uavs = len(values) / 3.
-    """
-    import os, csv
-    if csv_path is None:
-        csv_path = '/root/miSim/aerpaw/config/scenario.csv'
-    with open(csv_path, 'r') as f:
-        reader = csv.reader(f, skipinitialspace=True)
-        header = [h.strip() for h in next(reader)]
-        row = next(reader)
-    col = header.index('initialPositions')
-    init_pos = row[col].strip()
-    n_vals = len([v.strip() for v in init_pos.split(',') if v.strip()])
-    if n_vals % 3 != 0:
-        raise ValueError(f"initialPositions has {n_vals} values; expected a multiple of 3")
-    return n_vals // 3
-
-
-class TdmScheduler(threading.Thread):
-    """Daemon thread that mutes/unmutes a GNU Radio mute_cc block on a
-    wall-clock TDM schedule.
-
-    Slot assignment:  current_slot = floor(utc_time / slot_duration) % num_uavs
-    Guard interval:   the first *guard_interval* seconds of every slot are
-                      always muted to avoid TX overlap due to clock skew.
-    """
-
-    def __init__(self, mute_block, uav_id, num_uavs,
-                 slot_duration=0.5, guard_interval=0.05):
-        super().__init__(daemon=True)
-        self.mute_block = mute_block
-        self.uav_id = uav_id
-        self.num_uavs = num_uavs
-        self.slot_duration = slot_duration
-        self.guard_interval = guard_interval
-        self._stop_event = threading.Event()
-
-    def run(self):
-        print(f"[TDM] Scheduler started: uav_id={self.uav_id}, "
-              f"num_uavs={self.num_uavs}, slot={self.slot_duration}s, "
-              f"guard={self.guard_interval}s")
-        while not self._stop_event.is_set():
-            now = time.time()
-            slot_time = now % (self.slot_duration * self.num_uavs)
-            current_slot = int(slot_time / self.slot_duration)
-            time_into_slot = slot_time - current_slot * self.slot_duration
-
-            my_slot = (current_slot == self.uav_id)
-            in_guard = (time_into_slot < self.guard_interval)
-
-            should_mute = (not my_slot) or in_guard
-            self.mute_block.set_mute(should_mute)
-
-            # Sleep ~1 ms for responsive timing without busy-waiting
-            self._stop_event.wait(0.001)
-
-    def stop(self):
-        self._stop_event.set()
-
-
-class CSwSNRTX(gr.top_block):
-
-    def __init__(self, args='', freq=3.32e9, gaintx=76, offset=250e3, samp_rate=2e6, sps=16,
-                 uav_id=0, num_uavs=1, slot_duration=0.5, guard_interval=0.05):
-        gr.top_block.__init__(self, "CSwSNRTX")
-
-        ##################################################
-        # Parameters
-        ##################################################
-        self.args = args
-        self.freq = freq
-        self.gaintx = gaintx
-        self.offset = offset
-        self.samp_rate = samp_rate
-        self.sps = sps
-        self.uav_id = uav_id
-        self.num_uavs = num_uavs
-        self.slot_duration = slot_duration
-        self.guard_interval = guard_interval
-
-        ##################################################
-        # Variables
-        ##################################################
-        self.alpha = alpha = 0.99
-
-        ##################################################
-        # Blocks
-        ##################################################
-        self.uhd_usrp_sink_0 = uhd.usrp_sink(
-            ",".join(("", args)),
-            uhd.stream_args(
-                cpu_format="fc32",
-                args='',
-                channels=list(range(0,1)),
-            ),
-            '',
-        )
-        self.uhd_usrp_sink_0.set_center_freq(freq, 0)
-        self.uhd_usrp_sink_0.set_gain(gaintx, 0)
-        self.uhd_usrp_sink_0.set_antenna('TX/RX', 0)
-        self.uhd_usrp_sink_0.set_samp_rate(samp_rate)
-        self.uhd_usrp_sink_0.set_time_unknown_pps(uhd.time_spec())
-        self.root_raised_cosine_filter_0 = filter.fir_filter_ccf(
-            1,
-            firdes.root_raised_cosine(
-                sps,
-                samp_rate,
-                samp_rate/sps,
-                alpha,
-                10*sps+1))
-        self.interp_fir_filter_xxx_0 = filter.interp_fir_filter_ccc(sps, [1]+[0]*(sps-1))
-        self.interp_fir_filter_xxx_0.declare_sample_delay(0)
-        self.digital_glfsr_source_x_0 = digital.glfsr_source_b(12, True, 0, 1)
-        self.digital_chunks_to_symbols_xx_0 = digital.chunks_to_symbols_bc((-1,1), 1)
-        self.blocks_multiply_xx_0 = blocks.multiply_vcc(1)
-        self.blocks_multiply_const_vxx_0 = blocks.multiply_const_cc(1/1.58)
-        self.blocks_mute_0 = blocks.mute_cc(True)  # TDM: start muted
-        self.analog_sig_source_x_0 = analog.sig_source_c(samp_rate, analog.GR_COS_WAVE, offset, 1, 0, 0)
-
-
-        ##################################################
-        # Connections
-        ##################################################
-        self.connect((self.analog_sig_source_x_0, 0), (self.blocks_multiply_xx_0, 1))
-        self.connect((self.blocks_multiply_const_vxx_0, 0), (self.blocks_mute_0, 0))
-        self.connect((self.blocks_mute_0, 0), (self.uhd_usrp_sink_0, 0))
-        self.connect((self.blocks_multiply_xx_0, 0), (self.blocks_multiply_const_vxx_0, 0))
-        self.connect((self.digital_chunks_to_symbols_xx_0, 0), (self.interp_fir_filter_xxx_0, 0))
-        self.connect((self.digital_glfsr_source_x_0, 0), (self.digital_chunks_to_symbols_xx_0, 0))
-        self.connect((self.interp_fir_filter_xxx_0, 0), (self.root_raised_cosine_filter_0, 0))
-        self.connect((self.root_raised_cosine_filter_0, 0), (self.blocks_multiply_xx_0, 0))
-
-
-    def get_args(self):
-        return self.args
-
-    def set_args(self, args):
-        self.args = args
-
-    def get_freq(self):
-        return self.freq
-
-    def set_freq(self, freq):
-        self.freq = freq
-        self.uhd_usrp_sink_0.set_center_freq(self.freq, 0)
-
-    def get_gaintx(self):
-        return self.gaintx
-
-    def set_gaintx(self, gaintx):
-        self.gaintx = gaintx
-        self.uhd_usrp_sink_0.set_gain(self.gaintx, 0)
-
-    def get_offset(self):
-        return self.offset
-
-    def set_offset(self, offset):
-        self.offset = offset
-        self.analog_sig_source_x_0.set_frequency(self.offset)
-
-    def get_samp_rate(self):
-        return self.samp_rate
-
-    def set_samp_rate(self, samp_rate):
-        self.samp_rate = samp_rate
-        self.analog_sig_source_x_0.set_sampling_freq(self.samp_rate)
-        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
-        self.uhd_usrp_sink_0.set_samp_rate(self.samp_rate)
-
-    def get_sps(self):
-        return self.sps
-
-    def set_sps(self, sps):
-        self.sps = sps
-        self.interp_fir_filter_xxx_0.set_taps([1]+[0]*(self.sps-1))
-        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
-
-    def get_alpha(self):
-        return self.alpha
-
-    def set_alpha(self, alpha):
-        self.alpha = alpha
-        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
-
-
-def argument_parser():
-    description = 'Channel Sounder Transmitter with offset freq'
-    parser = ArgumentParser(description=description)
-    parser.add_argument(
-        "--args", dest="args", type=str, default='',
-        help="Set args [default=%(default)r]")
-    parser.add_argument(
-        "--freq", dest="freq", type=eng_float, default="3.32G",
-        help="Set freq [default=%(default)r]")
-    parser.add_argument(
-        "--gaintx", dest="gaintx", type=eng_float, default="76.0",
-        help="Set gaintx [default=%(default)r]")
-    parser.add_argument(
-        "--offset", dest="offset", type=eng_float, default="250.0k",
-        help="Set offset [default=%(default)r]")
-    parser.add_argument(
-        "--samp-rate", dest="samp_rate", type=eng_float, default="2.0M",
-        help="Set samp_rate [default=%(default)r]")
-    parser.add_argument(
-        "--sps", dest="sps", type=intx, default=16,
-        help="Set sps [default=%(default)r]")
-    parser.add_argument(
-        "--uav-id", dest="uav_id", type=int, default=None,
-        help="TDM slot index for this UAV (0-indexed). "
-             "If omitted, read from config/client.yaml.")
-    parser.add_argument(
-        "--num-uavs", dest="num_uavs", type=int, default=None,
-        help="Total number of UAVs (TDM slots). "
-             "If omitted, derived from config/scenario.csv.")
-    parser.add_argument(
-        "--slot-duration", dest="slot_duration", type=float, default=None,
-        help="TDM slot duration in seconds [default: 0.5 or from client.yaml]")
-    parser.add_argument(
-        "--guard-interval", dest="guard_interval", type=float, default=None,
-        help="TDM guard interval in seconds [default: 0.05 or from client.yaml]")
-    return parser
-
-
-def _resolve_tdm_options(options):
-    """Fill in TDM parameters from client.yaml / scenario.csv when not
-    provided on the command line."""
-    import os, yaml
-    cfg_dir = '/root/miSim/aerpaw/config'
-    env_cfg = os.environ.get('AERPAW_CLIENT_CONFIG', '')
-    if env_cfg:
-        yaml_path = env_cfg if os.path.isabs(env_cfg) else os.path.join('/root/miSim/aerpaw', env_cfg)
-    else:
-        yaml_path = os.path.join(cfg_dir, 'client.yaml')
-
-    cfg = {}
-    if os.path.isfile(yaml_path):
-        with open(yaml_path, 'r') as f:
-            cfg = yaml.safe_load(f) or {}
-
-    tdm_cfg = cfg.get('tdm', {})
-
-    if options.uav_id is None:
-        options.uav_id = int(cfg.get('uav_id', 0))
-    if options.slot_duration is None:
-        options.slot_duration = float(tdm_cfg.get('slot_duration', 0.5))
-    if options.guard_interval is None:
-        options.guard_interval = float(tdm_cfg.get('guard_interval', 0.05))
-    if options.num_uavs is None:
-        try:
-            options.num_uavs = derive_num_uavs_from_csv(
-                '/root/miSim/aerpaw/config/scenario.csv')
-        except Exception as e:
-            print(f"[TDM] Warning: could not derive num_uavs from scenario.csv: {e}")
-            print("[TDM] Defaulting to num_uavs=1 (TDM effectively disabled)")
-            options.num_uavs = 1
-
-    return options
-
-
-def main(top_block_cls=CSwSNRTX, options=None):
-    if options is None:
-        options = argument_parser().parse_args()
-
-    options = _resolve_tdm_options(options)
-
-    tb = top_block_cls(
-        args=options.args, freq=options.freq, gaintx=options.gaintx,
-        offset=options.offset, samp_rate=options.samp_rate, sps=options.sps,
-        uav_id=options.uav_id, num_uavs=options.num_uavs,
-        slot_duration=options.slot_duration,
-        guard_interval=options.guard_interval)
-
-    tdm_sched = TdmScheduler(
-        tb.blocks_mute_0,
-        uav_id=options.uav_id,
-        num_uavs=options.num_uavs,
-        slot_duration=options.slot_duration,
-        guard_interval=options.guard_interval)
-
-    def sig_handler(sig=None, frame=None):
-        tdm_sched.stop()
-        tb.stop()
-        tb.wait()
-        sys.exit(0)
-
-    signal.signal(signal.SIGINT, sig_handler)
-    signal.signal(signal.SIGTERM, sig_handler)
-
-    tb.start()
-    tdm_sched.start()
-
-    try:
-        input('Press Enter to quit: ')
-    except EOFError:
-        pass
-    tdm_sched.stop()
-    tb.stop()
-    tb.wait()
-
-
-if __name__ == '__main__':
-    main()
-(END)
-
-

aerpaw/radio/startchannelsounderRXGRC.sh

@@ -1,23 +0,0 @@
-#!/bin/bash
-GAIN_RX=30
-OFFSET=250e3
-SAMP_RATE=2e6
-SPS=16
-
-# Custom
-RX_FREQ=3.32e9
-
-if [ "$LAUNCH_MODE" == "TESTBED" ]; then
-  #To select a specific device
-  #ARGS="serial=31E74A9"
-  ARGS=NULL
-elif [ "$LAUNCH_MODE" == "EMULATION" ]; then
-  #ARGS='type=zmq'
-  ARGS=NULL
-else
-  echo "Warning: LAUNCH_MODE not set (got '${LAUNCH_MODE}'). Defaulting to TESTBED."
-  ARGS=NULL
-fi
-
-cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
-python3 CSwSNRRX.py --freq $RX_FREQ --gainrx $GAIN_RX --noise 0 --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS "$@"

aerpaw/radio/startchannelsounderTXGRC.sh

@@ -1,23 +0,0 @@
-#!/bin/bash
-GAIN_TX=76
-OFFSET=250e3
-SAMP_RATE=2e6
-SPS=16
-
-# Custom
-TX_FREQ=3.32e9
-
-if [ "$LAUNCH_MODE" == "TESTBED" ]; then
-  #To select a specific device
-  #ARGS="serial=31E74A9"
-  ARGS=NULL
-elif [ "$LAUNCH_MODE" == "EMULATION" ]; then
-  #ARGS='type=zmq'
-  ARGS=NULL
-else
-  echo "Warning: LAUNCH_MODE not set (got '${LAUNCH_MODE}'). Defaulting to TESTBED."
-  ARGS=NULL
-fi
-
-cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
-python3 CSwSNRTX.py --freq $TX_FREQ --gaintx $GAIN_TX --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS "$@"

aerpaw/radio/updateScripts.sh

@@ -1,16 +0,0 @@
-#!/bin/bash
-
-# Drop in replacements for channel sounder scripts
-cp startchannelsounderRXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
-cp startchannelsounderTXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
-cp CSwSNRRX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
-cp CSwSNRTX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
-
-# Replace start scripts
-cp ../scripts/startexperiment.sh /root/.
-cp ../scripts/startRadio.sh /root/Profiles/ProfileScripts/Radio/.
-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/controllerAnalysis.m

@@ -1,28 +0,0 @@
-function controller = controllerAnalysis(resultsPath)
-    arguments (Input)
-        resultsPath (1, 1) string;
-    end
-    arguments (Output)
-        controller table;
-    end
-    
-    % Measure intervals between issuing commands from the controller 
-    % (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
-    r = dir(resultsPath);
-    controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
-    controllerPath = dir(controllerPath);
-    controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
-    controller = readControllerLogs(controllerPath);
-    rpIdx = startsWith(controller.message, "Iteration duration: ");
-    s = split(controller.message(rpIdx), "Iteration duration: ");
-    s = split(s(:, 2), ' s');
-    s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
-    s.Format = "mm:ss.SSS";
-    fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
-    fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
-    fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
-    fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
-    if seconds(min(s)) < 4 
-        warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
-    end
-end

aerpaw/results/plotGpsLogs.m

@@ -1,122 +0,0 @@
-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;
-        G cell;
-    end
-    % Plot setup
-    f = uifigure;
-    gf = geoglobe(f);
-    hold(gf, "on");
-    c = ["g", "b", "m", "c"]; % plotting colors
-    
-    % paths
-    scenarioCsv = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv");
-    
-    % configured data
-    params = readScenarioCsv(scenarioCsv);
-    
-    fID = fopen(fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "client1.yaml"), 'r');
-    yaml = fscanf(fID, '%s');
-    fclose(fID);
-    % origin (LLA)
-    lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel];
-    
-    logDirs = dir(logDirs);
-    logDirs = logDirs(3:end);
-    logDirs = logDirs([logDirs.isdir] == 1);
-    logDirs = logDirs(~startsWith({logDirs.name}, "controller_"));
-
-    G = cell(size(logDirs));
-    for ii = 1:size(logDirs, 1)
-        % Find GPS log CSV
-        gpsCsv = dir(fullfile(logDirs(ii).folder, logDirs(ii).name));
-        gpsCsv = gpsCsv(endsWith({gpsCsv(:).name}, "_gps_log.csv"));
-        gpsCsv = fullfile(gpsCsv.folder, gpsCsv.name);
-    
-        % Read GPS log CSV
-        G{ii} = readGpsLogs(gpsCsv);
-    
-        % Find when algorithm begins/ends (using ENU altitude rate change)
-        verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]);
-    
-        % Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight)
-        pctThreshold = 60; % pctThreshold may need adjusting depending on your flight
-        startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "first");
-        stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last");
-    
-        % % Plot whole flight, including setup/cleanup
-        if plotWholeFlight
-            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
-        enu = NaN(height(G{ii}), 3);
-        enu(startIdx:stopIdx, :) = lla2enu([G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx)], lla0, "flat");
-        enu = array2table(enu, 'VariableNames', ["East", "North", "Up"]);
-        G{ii} = [G{ii}, enu];
-
-        % Do crude comparison of pairwise distances between this UAV and
-        % all previous UAVs
-        for jj = 1:(ii - 1)
-            Ai = G{ii}(:, [1, end-2:end]);
-            Aj = G{jj}(:, [1, end-2:end]);
-
-            % Trim data to match sizes
-            idx = min([size(Ai, 1), size(Aj, 1)]);
-            Ai = Ai(1:idx, :); Aj = Aj(1:idx, :);
-
-            pos_i = [Ai.East, Ai.North, Ai.Up];
-            pos_j = [Aj.East, Aj.North, Aj.Up];
-            d = vecnorm(pos_i - pos_j, 2, 2);
-            d = d(~isnan(d));
-
-            fprintf("Minimum distance between agents %d and %d is %2.3f\n", ii, jj, min(d));
-            if min(d) < 6
-                warning("Minimum distance between agents %d and %d of %2.3f is questionable for AERPAW", ii, jj, min(d));
-            end
-        end
-    
-        % Plot recorded trajectory over specified range of indices
-        geoplot3(gf, G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx) + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);
-    end
-    
-    % Plot domain
-    altOffset = 1; % to avoid clipping into the ground when displayed
-    domain = [lla0; enu2lla(params.domainMax, lla0, "flat")];
-    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
-    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
-    geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
-    geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
-    geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
-    geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
-    
-    % Plot floor (minimum altitude constraint)
-    floorAlt = params.minAlt;
-    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset + floorAlt, 1, 5), 'LineWidth', 3, 'Color', 'r');
-    
-    % Plot objective
-    objectivePos = [params.objectivePos, 0];
-    llaObj = enu2lla(objectivePos, lla0, "flat");
-    geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
-    
-    % Plot obstacles
-    for ii = 1:params.numObstacles
-        obstacle = enu2lla([params.obstacleMin((1 + (ii - 1) * 3):(ii * 3)); params.obstacleMax((1 + (ii - 1) * 3):(ii * 3))], lla0, "flat");
-        geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r');
-        geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r');
-        geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
-        geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
-        geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
-        geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
-    end
-    
-    % finish
-    hold(gf, "off");
-end

aerpaw/results/plotRadioLogs.m

@@ -1,259 +0,0 @@
-function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
-    arguments (Input)
-        resultsPath (1, 1) string;
-        G cell = {};
-        tLim (1, 2) datetime = [datetime(-Inf, 'ConvertFrom', 'datenum'), datetime(Inf, 'ConvertFrom', 'datenum')];
-    end
-    arguments (Output)
-        f (1, 1) matlab.ui.Figure;
-        fDist (1, 1) matlab.ui.Figure;
-        R cell;
-    end
-
-    logDirs = dir(resultsPath);
-    logDirs = logDirs(3:end);
-    logDirs = logDirs([logDirs.isdir] == 1);
-
-    R = cell(size(logDirs));
-    for ii = 1:size(logDirs, 1)
-        R{ii} = readRadioLogs(fullfile(logDirs(ii).folder, logDirs(ii).name));
-    end
-
-    % Discard rows where any non-NaN dB metric is below -200 (sentinel values)
-    for ii = 1:numel(R)
-        snr = R{ii}.SNR;
-        pwr = R{ii}.Power;
-        bad = (snr < -200 & ~isnan(snr)) | (pwr < -200 & ~isnan(pwr));
-        R{ii}(bad, :) = [];
-    end
-
-    % Compute path loss from Power (post-processing)
-    % Power = 20*log10(peak_mag) - rxGain; path loss = txGain - rxGain - Power
-    txGain_dB = 76;   % from startchannelsounderTXGRC.sh GAIN_TX
-    rxGain_dB = 30;   % from startchannelsounderRXGRC.sh GAIN_RX
-    for ii = 1:numel(R)
-        R{ii}.PathLoss = txGain_dB - rxGain_dB - R{ii}.Power;
-        R{ii}.FreqOffset = R{ii}.FreqOffset / 1e6; % Hz to MHz
-    end
-
-    % Build legend labels and color map for up to 4 UAVs
-    nUAV = numel(R);
-    colors = lines(nUAV * nUAV);
-    styles = ["-o", "-s", "-^", "-d", "-v", "-p", "-h", "-<", "->", "-+", "-x", "-*"];
-
-    metricNames = ["SNR", "Power", "Quality", "PathLoss", "NoiseFloor", "FreqOffset"];
-    yLabels     = ["SNR (dB)", "Power (dB)", "Quality", "Path Loss (dB)", "Noise Floor (dB)", "Freq Offset (MHz)"];
-    nMetrics    = numel(metricNames);
-
-    % --- Time-based figure ---
-    f = figure;
-    tl = tiledlayout(nMetrics + 1, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
-
-    % Distance vs time tile (first)
-    ax = nexttile(tl);
-    hold(ax, 'on'); grid(ax, 'on');
-    legendEntries = string.empty;
-    ci = 1;
-    if ~isempty(G)
-        for rxIdx = 1:nUAV
-            tbl = R{rxIdx};
-            txIDs = unique(tbl.TxUAVID);
-            for ti = 1:numel(txIDs)
-                txID = txIDs(ti);
-                rows = tbl(tbl.TxUAVID == txID, :);
-                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
-                if isempty(rows), continue; end
-                [~, ia] = unique(rows.Timestamp);
-                [radioPt, dist] = pairDist(rows(ia, :), G);
-                if isempty(dist) || all(isnan(dist)), continue; end
-                valid = ~isnan(dist);
-                si = mod(ci - 1, numel(styles)) + 1;
-                plot(ax, datetime(radioPt(valid), 'ConvertFrom', 'posixtime'), dist(valid), ...
-                    styles(si), 'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
-                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
-                ci = ci + 1;
-            end
-        end
-    end
-    ylabel(ax, 'Distance (m)');
-    xlabel(ax, 'Time');
-    legend(ax, legendEntries, 'Location', 'best');
-    hold(ax, 'off');
-
-    for mi = 1:nMetrics
-        ax = nexttile(tl);
-        hold(ax, 'on');
-        grid(ax, 'on');
-
-        legendEntries = string.empty;
-        ci = 1;
-        for rxIdx = 1:nUAV
-            tbl = R{rxIdx};
-            txIDs = unique(tbl.TxUAVID);
-            for ti = 1:numel(txIDs)
-                txID = txIDs(ti);
-                rows = tbl(tbl.TxUAVID == txID, :);
-                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
-                vals = rows.(metricNames(mi));
-                valid = ~isnan(vals);
-                rows = rows(valid, :);
-                vals = vals(valid);
-
-                if isempty(rows)
-                    continue;
-                end
-
-                si = mod(ci - 1, numel(styles)) + 1;
-                plot(ax, rows.Timestamp, vals, styles(si), ...
-                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
-                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, tbl.RxUAVID(1)); %#ok<AGROW>
-
-                % Median per 1/3-second time bin
-                [t_med, v_med] = timeBinMedian(posixtime(rows.Timestamp), vals, 1/3);
-                plot(ax, datetime(t_med, 'ConvertFrom', 'posixtime'), v_med, '-', ...
-                    'Color', 'r', 'LineWidth', 2);
-                legendEntries(end+1) = sprintf("TX %d → RX %d (median)", txID, tbl.RxUAVID(1)); %#ok<AGROW>
-                ci = ci + 1;
-            end
-        end
-
-        ylabel(ax, yLabels(mi));
-        if mi == nMetrics
-            xlabel(ax, 'Time');
-        end
-        legend(ax, legendEntries, 'Location', 'best');
-        hold(ax, 'off');
-    end
-
-    title(tl, 'Radio Channel Metrics');
-
-    % --- Distance-based figure ---
-    fDist = figure;
-
-    if isempty(G)
-        return;
-    end
-
-    tl2 = tiledlayout(nMetrics + 1, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
-
-    % Distance vs time tile (first)
-    ax = nexttile(tl2);
-    hold(ax, 'on'); grid(ax, 'on');
-    legendEntries = string.empty;
-    ci = 1;
-    for rxIdx = 1:nUAV
-        tbl = R{rxIdx};
-        txIDs = unique(tbl.TxUAVID);
-        for ti = 1:numel(txIDs)
-            txID = txIDs(ti);
-            rows = tbl(tbl.TxUAVID == txID, :);
-            rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
-            if isempty(rows), continue; end
-            [~, ia] = unique(rows.Timestamp);
-            [radioPt, dist] = pairDist(rows(ia, :), G);
-            if isempty(dist) || all(isnan(dist)), continue; end
-            valid = ~isnan(dist);
-            si = mod(ci - 1, numel(styles)) + 1;
-            plot(ax, datetime(radioPt(valid), 'ConvertFrom', 'posixtime'), dist(valid), ...
-                styles(si), 'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
-            legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
-            ci = ci + 1;
-        end
-    end
-    ylabel(ax, 'Distance (m)');
-    xlabel(ax, 'Time');
-    legend(ax, legendEntries, 'Location', 'best');
-    hold(ax, 'off');
-
-    for mi = 1:nMetrics
-        ax = nexttile(tl2);
-        hold(ax, 'on');
-        grid(ax, 'on');
-
-        legendEntries = string.empty;
-        ci = 1;
-        for rxIdx = 1:nUAV
-            tbl = R{rxIdx};
-            txIDs = unique(tbl.TxUAVID);
-            for ti = 1:numel(txIDs)
-                txID = txIDs(ti);
-                rows = tbl(tbl.TxUAVID == txID, :);
-
-                if isempty(rows)
-                    continue;
-                end
-
-                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
-                if isempty(rows)
-                    continue;
-                end
-
-                vals = rows.(metricNames(mi));
-                valid = ~isnan(vals);
-                rows = rows(valid, :);
-                vals = vals(valid);
-
-                if isempty(rows)
-                    continue;
-                end
-
-                [radioPt, dist] = pairDist(rows, G);
-                if isempty(dist) || all(isnan(dist)), continue; end
-
-                % Drop points where GPS interpolation returned NaN
-                validDist = ~isnan(dist);
-                rowTs = radioPt(validDist);
-                dist  = dist(validDist);
-                vals  = vals(validDist);
-
-                si = mod(ci - 1, numel(styles)) + 1;
-                scatter(ax, dist, vals, 9, colors(ci, :), strrep(styles(si), "-", ""), 'filled');
-                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
-
-                % Median per 1/3-second time bin, plotted against median distance
-                [~, dv_med] = timeBinMedian(rowTs, [dist, vals], 1/3);
-                [d_med, si_sort] = sort(dv_med(:, 1));
-                v_med = dv_med(si_sort, 2);
-                plot(ax, d_med, v_med, '-', 'Color', 'r', 'LineWidth', 2);
-                legendEntries(end+1) = sprintf("TX %d → RX %d (median)", txID, rows.RxUAVID(1)); %#ok<AGROW>
-                ci = ci + 1;
-            end
-        end
-
-        ylabel(ax, yLabels(mi));
-        if mi == nMetrics
-            xlabel(ax, 'Distance (m)');
-        end
-        legend(ax, legendEntries, 'Location', 'best');
-        hold(ax, 'off');
-    end
-
-    title(tl2, 'Radio Channel Metrics vs Distance');
-end
-
-
-function [radioPt, dist] = pairDist(rows, G)
-    % Interpolate GPS-based inter-UAV distance at each row's timestamp.
-    radioPt = []; dist = [];
-    txGpsIdx = double(rows.TxUAVID(1)) + 1;
-    rxGpsIdx = double(rows.RxUAVID(1)) + 1;
-    if txGpsIdx > numel(G) || rxGpsIdx > numel(G), return; end
-    Gtx = G{txGpsIdx};
-    Grx = G{rxGpsIdx};
-    if ~ismember('East', Gtx.Properties.VariableNames) || ...
-       ~ismember('East', Grx.Properties.VariableNames), return; end
-    txTs = Gtx.Timestamp; txTs.TimeZone = '';
-    rxTs = Grx.Timestamp; rxTs.TimeZone = '';
-    txPt = posixtime(txTs);
-    rxPt = posixtime(rxTs);
-    radioPt = posixtime(rows.Timestamp);
-    validTx = ~isnan(Gtx.East);
-    validRx = ~isnan(Grx.East);
-    txE = interp1(txPt(validTx), Gtx.East(validTx),  radioPt, 'linear', NaN);
-    txN = interp1(txPt(validTx), Gtx.North(validTx), radioPt, 'linear', NaN);
-    txU = interp1(txPt(validTx), Gtx.Up(validTx),    radioPt, 'linear', NaN);
-    rxE = interp1(rxPt(validRx), Grx.East(validRx),  radioPt, 'linear', NaN);
-    rxN = interp1(rxPt(validRx), Grx.North(validRx), radioPt, 'linear', NaN);
-    rxU = interp1(rxPt(validRx), Grx.Up(validRx),    radioPt, 'linear', NaN);
-    dist = vecnorm([txE - rxE, txN - rxN, txU - rxU], 2, 2);
-end

aerpaw/results/readControllerLogs.m

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

aerpaw/results/readGpsLogs.m

@@ -1,32 +0,0 @@
-function [G] = readGpsLogs(logPath)
-    arguments (Input)
-        logPath (1, 1) string {isfile(logPath)};
-    end
-    
-    arguments (Output)
-        G (:, 10) table;
-    end
-    
-    G = readtable(logPath, "ReadVariableNames", false);
-    
-    % first column is just index, meaningless, toss it
-    G = G(:, 2:end);
-    
-    % switch to the correct LLA convention (lat, lon, alt)
-    tmp = G(:, 2);
-    G(:, 2) = G(:, 1);
-    G(:, 1) = tmp;
-    
-    % Split pitch, yaw, roll data read in as one string per timestep into separate columns
-    PYR = cell2mat(cellfun(@(x) str2num(strip(strip(x, "left", "("), "right", ")")), table2cell(G(:, 5)), "UniformOutput", false)); %#ok<ST2NM>
-    % Reinsert to original table
-    G = [G(:, 1:3), table(PYR(:, 1), VariableNames="Pitch"), table(PYR(:, 2), VariableNames="Yaw"), table(PYR(:, 3), VariableNames="Roll"), G(:, 6:end)];
-    
-    % Clean up datetime entry
-    G = [table(datetime(G{:,8}, "InputFormat","yyyy-MM-dd HH:mm:ss.SSS", "TimeZone","America/New_York")), G(:, [1:7, 9:10])];
-    
-    % Fix variable names
-    G.Properties.VariableNames = ["Timestamp", "Latitude", "Longitude", "Altitude", "Pitch", "Yaw", "Roll", "Voltage", "GPS Status", "Satellites"];
-    G.Properties.VariableTypes = ["datetime", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
-    G.Properties.VariableUnits = ["yyyy-MM-dd HH:mm:ss.SSS (UTC+5)", "deg", "deg", "m", "deg", "deg", "deg", "Volts", "", ""];
-end

aerpaw/results/readRadioLogs.m

@@ -1,112 +0,0 @@
-function R = readRadioLogs(logPath)
-    arguments (Input)
-        logPath (1, 1) string {isfolder(logPath)};
-    end
-    arguments (Output)
-        R (:, 8) table;
-    end
-
-    % Extract receiving UAV ID from directory name (e.g. "uav0_..." → 0)
-    [~, dirName] = fileparts(logPath);
-    rxID = int32(sscanf(dirName, 'uav%d'));
-
-    metrics = ["quality", "snr", "power", "noisefloor", "freqoffset"];
-    logs = dir(logPath);
-    logs = logs(endsWith({logs(:).name}, metrics + "_log.txt"));
-
-    R = table(datetime.empty(0,1), zeros(0,1,'int32'), zeros(0,1,'int32'), zeros(0,1), zeros(0,1), zeros(0,1), zeros(0,1), zeros(0,1), ...
-        'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality", "NoiseFloor", "FreqOffset"]);
-
-    for ii = 1:numel(logs)
-        filepath = fullfile(logs(ii).folder, logs(ii).name);
-
-        % Determine which metric this file contains
-        metric = "";
-        for m = 1:numel(metrics)
-            if endsWith(logs(ii).name, metrics(m) + "_log.txt")
-                metric = metrics(m);
-                break;
-            end
-        end
-
-        fid = fopen(filepath, 'r');
-        % Skip header lines: some files have 2 tail-error lines + 1 column
-        % header ("tx_uav_id,value"), others start with data immediately.
-        % 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(cellstr(data{1}), 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
-        txId = int32(data{2});
-        val = data{3};
-
-        n = numel(ts);
-        t = table(ts, txId, repmat(rxID, n, 1), NaN(n,1), NaN(n,1), NaN(n,1), NaN(n,1), NaN(n,1), ...
-            'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality", "NoiseFloor", "FreqOffset"]);
-
-        switch metric
-            case "snr",        t.SNR = val;
-            case "power",      t.Power = val;
-            case "quality",    t.Quality = val;
-            case "noisefloor", t.NoiseFloor = val;
-            case "freqoffset", t.FreqOffset = val;
-        end
-
-        R = [R; t]; %#ok<AGROW>
-    end
-
-    R = sortrows(R, "Timestamp");
-
-    % Reconstruct per-measurement timestamps within GNURadio processing batches.
-    % The flowgraph accumulates one full PN sequence (4095 chips at samp_rate/sps)
-    % per measurement, but outputs the whole batch simultaneously with a single
-    % wall-clock timestamp. We reassign timestamps by counting backward from the
-    % batch processing time at the known PN period interval.
-    pn_period = 4095 / (2e6 / 16);  % 32.76 ms per PN correlation period
-
-    for txId = unique(R.TxUAVID)'
-        rows = find(R.TxUAVID == txId);
-        if numel(rows) < 2, continue; end
-
-        dt = seconds(diff(R.Timestamp(rows)));
-        break_pos = [1; find(dt > 0.5) + 1];
-        end_pos   = [break_pos(2:end) - 1; numel(rows)];
-
-        for b = 1:numel(break_pos)
-            idx      = rows(break_pos(b) : end_pos(b));
-            batch_ts = posixtime(R.Timestamp(idx));
-            t_ref    = max(batch_ts);
-
-            % Multiple metric files share the same processing timestamp for
-            % each PN period, so group by unique original timestamp rather
-            % than treating every row as a separate PN period.
-            [~, ~, group_id] = unique(batch_ts);
-            n_groups = max(group_id);
-            new_ts   = t_ref - (n_groups - 1 : -1 : 0)' * pn_period;
-
-            for g = 1:n_groups
-                R.Timestamp(idx(group_id == g)) = ...
-                    datetime(new_ts(g), 'ConvertFrom', 'posixtime');
-            end
-        end
-    end
-
-    % Remove rows during defined guard period between TDM shifts
-    R(R.TxUAVID == -1, :) = [];
-
-    % Remove self-reception rows (TX == RX)
-    R(R.TxUAVID == R.RxUAVID, :) = [];
-end

aerpaw/results/resultsAnalysis.m

@@ -1,75 +0,0 @@
-%% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
-
-% Check timeline in controller logs
-controller = controllerAnalysis(resultsPath);
-
-% Plot GPS logged data and scenario information (domain, objective, obstacles)
-seaToGroundLevel = 110; % measured approximately from USGS national map viewer
-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 (time-based and distance-based)
-[fRadio, fRadioDist, R] = plotRadioLogs(resultsPath, G, controller.timestamp([1, end]));
-
-%% Run simulation
-% Run miSim using same AERPAW scenario definition CSV
-csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv");
-params = readScenarioCsv(csvPath);
-
-% Visualization settings
-plotCommsGeometry = false;
-makePlots = true;
-makeVideo = true;
-
-% Define scenario according to CSV specification
-domain = rectangularPrism;
-domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
-domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [1, 2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
-
-agents = cell(size(params.initialPositions, 2) / 3, 1);
-for ii = 1:size(agents, 1)
-    agents{ii} = agent;
-
-    sensorModel = sigmoidSensor;
-    sensorModel = sensorModel.initialize(params.alphaDist(ii), params.betaDist(ii), params.alphaTilt(ii), params.betaTilt(ii));
-
-    collisionGeometry = spherical;
-    collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
-
-    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, 5.0, sprintf("Agent %d", ii), plotCommsGeometry);
-end
-
-% Create obstacles
-obstacles = cell(params.numObstacles, 1);
-for ii = 1:size(obstacles, 1)
-    obstacles{ii} = rectangularPrism;
-    obstacles{ii} = obstacles{ii}.initialize([params.obstacleMin((((ii - 1) * 3) + 1):(ii * 3)); params.obstacleMax((((ii - 1) * 3) + 1):(ii * 3))], "OBSTACLE", sprintf("Obstacle %d", ii));
-end
-
-% Set up simulation
-sim = miSim;
-sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo);
-
-% Save simulation parameters to output file
-sim.writeInits();
-
-% Run
-sim = sim.run();
-
-%% Plot AERPAW trajectory logs onto simulated result for comparison
-% Duplicate plot to overlay with logged trajectories
-comparison = figure;
-copyobj(sim.f.Children, comparison);
-
-% Plot trajectories on top
-for ii = 1:size(G, 1)
-    gpsTimes = G{ii}.Timestamp;
-    gpsTimes.TimeZone = '';
-    inRange = gpsTimes >= controller.timestamp(1) & gpsTimes <= controller.timestamp(end);
-    for jj = 1:size(sim.spatialPlotIndices, 2)
-        hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "on");
-        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East(inRange), G{ii}.North(inRange), G{ii}.Up(inRange) + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
-        hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "off");
-    end
-end

aerpaw/results/timeBinMedian.m

@@ -1,29 +0,0 @@
-function [t_med, v_med] = timeBinMedian(t, v, binWidth)
-    % Compute median of each column of v within fixed-width time bins.
-    %
-    %   t        - (N,1) posixtime values
-    %   v        - (N,K) data matrix; one column per quantity
-    %   binWidth - scalar bin width in seconds
-    %
-    %   t_med    - (B,1) median time of each non-empty bin
-    %   v_med    - (B,K) median of each column per non-empty bin
-
-    edges = (floor(min(t) / binWidth) * binWidth) : binWidth : ...
-            (floor(max(t) / binWidth) * binWidth + binWidth);
-    bins  = discretize(t, edges);
-    nBins = numel(edges) - 1;
-    K     = size(v, 2);
-
-    t_all = NaN(nBins, 1);
-    v_all = NaN(nBins, K);
-    for bi = 1:nBins
-        mask = bins == bi;
-        if ~any(mask), continue; end
-        t_all(bi)   = median(t(mask));
-        v_all(bi,:) = median(v(mask,:), 1);
-    end
-
-    ok    = ~isnan(t_all);
-    t_med = t_all(ok);
-    v_med = v_all(ok, :);
-end

aerpaw/run_uav.sh

@@ -1,65 +0,0 @@
-#!/bin/bash
-# run_uav.sh - Wrapper for UAV runner
-# Launches UAV client with environment-specific configuration
-#
-# Usage:
-#   ./run_uav.sh local                          # defaults to config/client.yaml
-#   ./run_uav.sh testbed config/client2.yaml     # use a specific config file
-
-set -e
-
-# Change to aerpaw directory
-cd /root/miSim/aerpaw
-
-# Activate venv if it exists
-if [ -d "venv" ]; then
-    source venv/bin/activate
-fi
-
-# Determine environment from argument (required)
-if [ "$1" = "testbed" ]; then
-    ENV="testbed"
-elif [ "$1" = "local" ]; then
-    ENV="local"
-else
-    echo "Error: Environment not specified."
-    echo "Usage: $0 [local|testbed] [config_file]"
-    echo ""
-    echo "  local   - Use local/simulation configuration"
-    echo "  testbed - Use AERPAW testbed configuration"
-    echo ""
-    echo "  config_file - Path to client YAML (default: config/client.yaml)"
-    exit 1
-fi
-
-# Client config file: 2nd argument > AERPAW_CLIENT_CONFIG env var > default
-CONFIG_FILE="${2:-${AERPAW_CLIENT_CONFIG:-config/client.yaml}}"
-if [ ! -f "$CONFIG_FILE" ]; then
-    echo "Error: Config file not found: $CONFIG_FILE"
-    exit 1
-fi
-
-echo "[run_uav] Environment: $ENV"
-echo "[run_uav] Config file: $CONFIG_FILE"
-
-# Export for Python scripts to use
-export AERPAW_ENV="$ENV"
-export AERPAW_CLIENT_CONFIG="$(realpath "$CONFIG_FILE")"
-
-# Read MAVLink connection from config file using Python
-CONN=$(python3 -c "
-import yaml
-with open('$CONFIG_FILE') as f:
-    cfg = yaml.safe_load(f)
-env = cfg['environments']['$ENV']['mavlink']
-print(f\"udp:{env['ip']}:{env['port']}\")
-")
-
-echo "[run_uav] MAVLink connection: $CONN"
-
-# Run via aerpawlib
-echo "[run_uav] Starting UAV runner..."
-python3 -u -m aerpawlib \
-    --script client.uav_runner \
-    --conn "$CONN" \
-    --vehicle drone

aerpaw/scripts/startRadio.sh

@@ -1,100 +0,0 @@
-#!/bin/bash
-
-# Derive number of UAVs from scenario.csv
-NUM_UAVS=$(python3 -c "
-import csv, os
-csv_path = '/root/miSim/aerpaw/config/scenario.csv'
-with open(csv_path, 'r') as f:
-    reader = csv.reader(f, skipinitialspace=True)
-    header = [h.strip() for h in next(reader)]
-    row = next(reader)
-col = header.index('initialPositions')
-vals = [v.strip() for v in row[col].strip().split(',') if v.strip()]
-print(len(vals) // 3)
-" 2>/dev/null || echo 0)
-
-cd $PROFILE_DIR"/ProfileScripts/Radio/Helpers"
-
-if [ "$NUM_UAVS" -eq 2 ]; then
-    # Direct 1-to-1 mode: UAV 0 = TX only, UAV 1 = RX only
-    echo "[Radio] 2-UAV direct mode: UAV_ID=$UAV_ID"
-
-    if [ "$UAV_ID" -eq 0 ]; then
-        # TX only (--num-uavs 1 disables TDM muting)
-        screen -S txGRC -dm \
-               bash -c "stdbuf -oL -eL ./startchannelsounderTXGRC.sh --num-uavs 1 \
-               2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsoundertxgrc_log.txt"
-    else
-        # RX only (--num-uavs 1 disables TDM tagging)
-        screen -S rxGRC -dm \
-               bash -c "stdbuf -oL -eL ./startchannelsounderRXGRC.sh --num-uavs 1 \
-               2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsounderrxgrc_log.txt"
-
-        screen -S power -dm \
-               bash -c "stdbuf -oL -eL tail -F /root/Power\
-                2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
-
-        screen -S quality -dm \
-               bash -c "stdbuf -oL -eL tail -F /root/Quality\
-                2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
-
-        screen -S snr -dm \
-               bash -c "stdbuf -oL -eL tail -F /root/SNR\
-                2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
-
-        screen -S noisefloor -dm \
-              bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
-                2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_noisefloor_log.txt"
-
-        screen -S freqoffset -dm \
-               bash -c "stdbuf -oL -eL tail -F /root/FreqOffset\
-                2>&1 | ts $TS_FORMAT \
-               | tee $RESULTS_DIR/$LOG_PREFIX\_freqoffset_log.txt"
-    fi
-else
-    # 3+ UAVs: full TDM mode — every node runs both TX and RX
-    echo "[Radio] TDM mode: $NUM_UAVS UAVs, UAV_ID=$UAV_ID"
-
-    screen -S rxGRC -dm \
-           bash -c "stdbuf -oL -eL ./startchannelsounderRXGRC.sh \
-           2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsounderrxgrc_log.txt"
-
-    screen -S power -dm \
-           bash -c "stdbuf -oL -eL tail -F /root/Power\
-            2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
-
-    screen -S quality -dm \
-           bash -c "stdbuf -oL -eL tail -F /root/Quality\
-            2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
-
-    screen -S snr -dm \
-           bash -c "stdbuf -oL -eL tail -F /root/SNR\
-            2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
-
-    screen -S noisefloor -dm \
-          bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
-            2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_noisefloor_log.txt"
-
-    screen -S freqoffset -dm \
-           bash -c "stdbuf -oL -eL tail -F /root/FreqOffset\
-            2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_freqoffset_log.txt"
-
-    screen -S txGRC -dm \
-           bash -c "stdbuf -oL -eL ./startchannelsounderTXGRC.sh \
-           2>&1 | ts $TS_FORMAT \
-           | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsoundertxgrc_log.txt"
-fi
-
-cd -

aerpaw/scripts/startVehicle.sh

@@ -1,30 +0,0 @@
-#!/bin/bash
-
-### Sample GPS logger portion
-# use vehicle type generic to skip the arming requirement
-export VEHICLE_TYPE="${VEHICLE_TYPE:-generic}"
-
-# GPS Logger sample application (this does not move the vehicle)
-
-#cd $PROFILE_DIR"/ProfileScripts/Vehicle/Helpers"
-#
-#screen -S vehicle -dm \
-#       bash -c "stdbuf -oL -eL ./gpsLoggerHelper.sh \
-#       2> >(ts $TS_FORMAT >> $RESULTS_DIR/${LOG_PREFIX}_vehicle_log_err.txt) \
-#       | ts $TS_FORMAT \
-#       | tee $RESULTS_DIR/$LOG_PREFIX\_vehicle_log.txt"
-#
-#cd -
-
-### Actual control portion (custom)
-export VEHICLE_TYPE="${VEHICLE_TYPE:-drone}" # out of rover, drone, generic
-
-cd /root/miSim/aerpaw
-
-# Use screen/ts/tee aerpawism from sample script
-screen -S vehicle -dm \
-       bash -c "stdbuf -oL -eL ./run_uav.sh testbed \
-       | ts $TS_FORMAT \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_vehicle_log.txt"     
-
-cd -

aerpaw/scripts/startVehicle_controller.sh

@@ -1,11 +0,0 @@
-#!/bin/bash
-
-cd /root/miSim/aerpaw
-
-# Compile controller
-/bin/bash compile.sh
-
-# Run controller
-./build/controller_app
-
-cd -

aerpaw/scripts/startexperiment.sh

@@ -1,50 +0,0 @@
-#!/bin/bash
-/root/stopexperiment.sh
-
-source /root/.ap-set-experiment-env.sh
-source /root/.bashrc
-
-# set path to client config YAML
-export AERPAW_CLIENT_CONFIG=/root/miSim/aerpaw/config/client1.yaml
-
-export AERPAW_REPO=${AERPAW_REPO:-/root/AERPAW-Dev}
-export AERPAW_PYTHON=${AERPAW_PYTHON:-python3}
-export PYTHONPATH=/usr/local/lib/python3/dist-packages/
-export EXP_NUMBER=${EXP_NUMBER:-1}
-
-if [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_uav" ]; then
-    export VEHICLE_TYPE=drone
-elif [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_ugv" ]; then
-    export VEHICLE_TYPE=rover
-else
-    export VEHICLE_TYPE=none
-fi
-
-if [ "$AP_EXPENV_SESSION_ENV" == "Virtual" ]; then
-    export LAUNCH_MODE=EMULATION
-elif [ "$AP_EXPENV_SESSION_ENV" == "Testbed" ]; then
-    export LAUNCH_MODE=TESTBED
-else
-    export LAUNCH_MODE=none
-fi
-
-# prepare results directory
-export UAV_ID=$(python3 -c "import yaml; print(yaml.safe_load(open('$AERPAW_CLIENT_CONFIG'))['uav_id'])")
-export RESULTS_DIR_TIMESTAMP=$(date +%Y-%m-%d_%H_%M_%S)
-export RESULTS_DIR="/root/Results/uav${UAV_ID}_${RESULTS_DIR_TIMESTAMP}"
-mkdir -p "$RESULTS_DIR"
-
-export TS_FORMAT="${TS_FORMAT:-'[%Y-%m-%d %H:%M:%.S]'}"
-export LOG_PREFIX="$(date +%Y-%m-%d_%H_%M_%S)"
-
-export TX_FREQ=3.32e9
-export RX_FREQ=3.32e9
-
-export PROFILE_DIR=$AERPAW_REPO"/AHN/E-VM/Profile_software"
-cd $PROFILE_DIR"/ProfileScripts"
-
-./Radio/startRadio.sh
-#./Traffic/startTraffic.sh
-./Vehicle/startVehicle.sh
-
-schedule_stop.sh 30

aerpaw/scripts/startexperiment_controller.sh

@@ -1,47 +0,0 @@
-#!/bin/bash
-/root/stopexperiment.sh
-
-source /root/.ap-set-experiment-env.sh
-source /root/.bashrc
-
-export AERPAW_REPO=${AERPAW_REPO:-/root/AERPAW-Dev}
-export AERPAW_PYTHON=${AERPAW_PYTHON:-python3}
-export PYTHONPATH=/usr/local/lib/python3/dist-packages/
-export EXP_NUMBER=${EXP_NUMBER:-1}
-
-if [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_uav" ]; then
-    export VEHICLE_TYPE=drone
-elif [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_ugv" ]; then
-    export VEHICLE_TYPE=rover
-else
-    export VEHICLE_TYPE=none
-fi
-
-if [ "$AP_EXPENV_SESSION_ENV" == "Virtual" ]; then
-    export LAUNCH_MODE=EMULATION
-elif [ "$AP_EXPENV_SESSION_ENV" == "Testbed" ]; then
-    export LAUNCH_MODE=TESTBED
-else
-    export LAUNCH_MODE=none
-fi
-
-# prepare results directory
-export RESULTS_DIR_TIMESTAMP=$(date +%Y-%m-%d_%H_%M_%S)
-export RESULTS_DIR="/root/Results/controller_${RESULTS_DIR_TIMESTAMP}"
-mkdir -p "$RESULTS_DIR"
-
-export TS_FORMAT="${TS_FORMAT:-'[%Y-%m-%d %H:%M:%.S]'}"
-export LOG_PREFIX="$(date +%Y-%m-%d_%H_%M_%S)"
-
-export TX_FREQ=3.32e9
-export RX_FREQ=3.32e9
-
-export PROFILE_DIR=$AERPAW_REPO"/AHN/E-VM/Profile_software"
-cd $PROFILE_DIR"/ProfileScripts"
-
-screen -S controller -dm \
-	bash -c "stdbuf -oL -eL ./Vehicle/startVehicle.sh \
-	| ts $TS_FORMAT \
-	| tee $RESULTS_DIR/$LOG_PREFIX\_controller_log.txt"
-
-schedule_stop.sh 30

geometries/@cone/cone.m

@@ -6,11 +6,9 @@ classdef cone
         label = "";
 
         % Spatial
-        center  = NaN;
-        radius  = NaN;
-        height  = NaN;
-        tilt    = 0;   % degrees, 0=nadir 90=horizon
-        azimuth = 0;   % degrees, 0=+Y 90=+X clockwise
+        center = NaN;
+        radius = NaN;
+        height = NaN;
 
         % Plotting
         surface;

geometries/@cone/initialize.m

@@ -1,23 +1,19 @@
-function obj = initialize(obj, center, radius, height, tag, label, tilt, azimuth)
+function obj = initialize(obj, center, radius, height, tag, label)
     arguments (Input)
-        obj     (1, 1) {mustBeA(obj, "cone")};
-        center  (1, 3) double;
-        radius  (1, 1) double;
-        height  (1, 1) double;
-        tag     (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
-        label   (1, 1) string      = "";
-        tilt    (1, 1) double      = 0;
-        azimuth (1, 1) double      = 0;
+        obj (1, 1) {mustBeA(obj, 'cone')};
+        center (1, 3) double;
+        radius (1, 1) double;
+        height (1, 1) double;
+        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        label (1, 1) string = "";
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "cone")};
+        obj (1, 1) {mustBeA(obj, 'cone')};
     end
 
-    obj.center  = center;
-    obj.radius  = radius;
-    obj.height  = height;
-    obj.tag     = tag;
-    obj.label   = label;
-    obj.tilt    = tilt;
-    obj.azimuth = azimuth;
+    obj.center = center;
+    obj.radius = radius;
+    obj.height = height;
+    obj.tag = tag;
+    obj.label = label;
 end

geometries/@cone/plot.m

@@ -1,13 +1,13 @@
 function [obj, f] = plot(obj, ind, f, maxAlt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "cone")};
+        obj (1, 1) {mustBeA(obj, 'cone')};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
         maxAlt (1, 1) = 10;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "cone")};
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+        obj (1, 1) {mustBeA(obj, 'cone')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
     
     % Create axes if they don't already exist
@@ -21,17 +21,6 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
     % Scale to match height
     Z = Z * maxAlt;
     
-    % Rotate mesh around apex to match boresight tilt and azimuth.
-    % Apex sits at [0, 0, maxAlt] before center translation.
-    % Convention: tilt 0=nadir, 90=horizon; azimuth 0=+Y, 90=+X, clockwise.
-    Ry = [cosd(obj.tilt), 0, -sind(obj.tilt); 0, 1, 0; sind(obj.tilt), 0, cosd(obj.tilt)];
-    Rz = [sind(obj.azimuth), -cosd(obj.azimuth), 0; cosd(obj.azimuth), sind(obj.azimuth), 0; 0, 0, 1];
-    R  = Rz * Ry;
-    pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
-    X = reshape(pts(1, :), size(X));
-    Y = reshape(pts(2, :), size(Y));
-    Z = reshape(pts(3, :) + maxAlt, size(Z));
-
     % Move to center location
     X = X + obj.center(1);
     Y = Y + obj.center(2);
@@ -42,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
         o = surf(f.CurrentAxes, X, Y, Z);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(regionTypeColor(obj.tag), 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
+        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
         hold(f.Children(1).Children(ind(1)), "off");
     end
     

geometries/@rectangularPrism/closestToPoint.m

@@ -1,6 +1,6 @@
 function cPos = closestToPoint(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/contains.m

@@ -1,6 +1,6 @@
 function c = contains(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/containsLine.m

@@ -1,6 +1,6 @@
 function c = containsLine(obj, pos1, pos2)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         pos1 (1, 3) double;
         pos2 (1, 3) double;
     end
@@ -21,7 +21,7 @@ function c = containsLine(obj, pos1, pos2)
     tMax = 1;
     for ii = 1:3
         % line is parallel to geometry
-        if abs(d(ii)) < 1e-9
+        if abs(d(ii)) < 1e-12
             if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii)
                 c = false;
                 return;

geometries/@rectangularPrism/distance.m

@@ -1,6 +1,6 @@
 function d = distance(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/distanceGradient.m

@@ -1,6 +1,6 @@
 function g = distanceGradient(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         bounds (2, 3) double;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
@@ -8,7 +8,7 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
         discretizationStep (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
     end
 
     obj.tag = tag;