organizing

.gitignore

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

@agent/agent.m

@@ -19,8 +19,7 @@ classdef agent
         fovGeometry;
 
         % Communication
-        commsGeometry;          % fixed-radius comms model (spherical, radius = comRange)
-        txPower = 0.1;          % transmit power (W) for the SINR comms model (isotropic)
+        commsGeometry;
         lesserNeighbors = zeros(1, 0);
 
         % Performance
@@ -33,9 +32,7 @@ classdef agent
 
     properties (SetAccess = private, GetAccess = public)
         initialStepSize = NaN;
-        initialMaxAngleStepSize = NaN;
         stepDecayRate = NaN;
-        angleStepDecayRate = NaN;
     end
 
     methods (Access = public)
@@ -50,9 +47,9 @@ classdef agent
             obj.fovGeometry = cone;
             obj.commsGeometry = spherical;
         end
-        [obj] = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry, txPower);
-        [obj] = run(obj, domain, partitioning, t, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents);
-        [partitioning, agents] = partition(obj, agents, objective)
+        [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
+        [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,4 +1,4 @@
-function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry, txPower)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
@@ -7,10 +7,8 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
         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;
-        txPower (1, 1) double = 0.1;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "agent")};
@@ -25,10 +23,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     obj.label = label;
     obj.plotCommsGeometry = plotCommsGeometry;
     obj.initialStepSize = initialStepSize;
-    obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
-    obj.txPower = txPower;
     obj.stepDecayRate = obj.initialStepSize / maxIter;
-    obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
 
     % Initialize performance vector
     if coder.target('MATLAB')
@@ -40,5 +35,5 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
 
     % 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,4 +1,4 @@
-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")};
@@ -6,7 +6,6 @@ function [partitioning, agents] = partition(obj, agents, objective)
     end
     arguments (Output)
         partitioning (:, :) double;
-        agents (:, 1) cell;
     end
 
     nAgents = size(agents, 1);
@@ -19,22 +18,8 @@ function [partitioning, agents] = partition(obj, agents, objective)
     % 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
+        p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
         agentPerf(:, aa) = p(:);
     end
     agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;

@agent/run.m

@@ -1,15 +1,14 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
         timestepIndex (1, 1) double;
         index (1, 1) double;
+        agents (:, 1) {mustBeA(agents, "cell")};
         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();
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "agent")};
@@ -34,62 +33,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 < 6
+            % 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,54 +73,37 @@ 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
 
     % 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
+    gradNorm = 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);
+        if gradNorm < 1e-100
+            a_gradient = zeros(1, 3);
         else
             % Scale so steady-state step ≈ targetRate (matching SI behavior)
-            a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
+            a_gradient = (targetRate * dampingCoeff / (gradNorm * 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.vel = (obj.vel + a_gradient * 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);
+        if gradNorm >= 1e-100
+            obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
         end
     end
 
-    % 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
-
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;
     if isa(obj.collisionGeometry, "rectangularPrism")

@agent/updatePlots.m

@@ -7,68 +7,45 @@ function updatePlots(obj)
 
     % 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

@@ -61,9 +61,7 @@ function [obj] = constrainMotion(obj)
     end
 
     idx = length(h(triu(true(size(h)), 1)));
-    if coder.target('MATLAB')
-	obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
-    end
+    obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
     idx = idx + 1;
 
     hObs = NaN(nAgents, size(obj.obstacles, 1));
@@ -86,9 +84,7 @@ function [obj] = constrainMotion(obj)
         end
     end
 
-    if coder.target('MATLAB')
-	obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
-    end
+    obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
     idx = idx + numel(hObs);
 
     % Set up domain constraints (walls and ceiling only)
@@ -132,106 +128,43 @@ function [obj] = constrainMotion(obj)
         b(kk) = obj.barrierGain * max(0, 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;
+        obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
+        idx = idx + 6;
+    end
+
+    if coder.target('MATLAB')
+        % Save off h function values (logging only — not needed in compiled mode)
+        obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
     end
 
     % Add communication network constraints
     hComms = NaN(nAgents, nAgents);
     hComms(logical(eye(nAgents))) = 0;
-    if ~obj.useSinrComms
-        % Fixed-radius comms: keep each maintained pair within the smaller of
-        % the two comms radii via the barrier h = r_comms^2 - dist^2.
-        for ii = 1:(nAgents - 1)
-            for jj = (ii + 1):nAgents
-                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;
+    for ii = 1:(nAgents - 1)
+        for jj = (ii + 1):nAgents
+            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;
 
-                    A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
-                    A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
+                A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
 
-                    % One-step forward invariance: b = h/dt ensures h cannot
-                    % 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
-
-                    kk = kk + 1;
+                % 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
-            end
-        end
-    else
-        % SINR comms: a link is feasible only if each agent receives the other
-        % above threshold, so every maintained pair is held with TWO barrier
-        % rows, h = SINR - gammaCbf, one per receiver direction. Each gradient
-        % couples the receiver, transmitter, and every interferer, so the QP
-        % rows are dense across all agent blocks.
-        % Maintain SINR a margin above the connectivity threshold so that
-        % discrete-time overshoot of the (convex) SINR does not transiently
-        % drop a maintained link below threshold. Mirrors the fixed-radius
-        % barrier's 0.9 padding: the CBF holds SINR >= gammaCbf > gammaLin,
-        % while connectivity (updateAdjacency) still uses gammaLin.
-        paddingDb = 3.0;
-        gammaCbf = 10^((obj.sinrThreshold + paddingDb) / 10);
-        lastPositions = zeros(nAgents, 3);
-        for ll = 1:nAgents
-            lastPositions(ll, :) = obj.agents{ll}.lastPos;
-        end
-        for ii = 1:(nAgents - 1)
-            for jj = (ii + 1):nAgents
-                if obj.constraintAdjacencyMatrix(ii, jj)
-                    % Direction 1: lower-index agent (ii) receives higher (jj).
-                    % Upper triangle of hComms stores this direction's barrier.
-                    [sinr1, grad1] = obj.sinrLink(lastPositions, ii, jj);
-                    hComms(ii, jj) = sinr1 - gammaCbf;
-                    % CBF row: A = -grad(h), b = h/dt (one-step forward invariance)
-                    for aa = 1:nAgents
-                        A(kk, (3 * aa - 2):(3 * aa)) = -grad1(aa, :);
-                    end
-                    if norm(A(kk, :)) < 1e-9
-                        b(kk) = 0;
-                    else
-                        b(kk) = hComms(ii, jj) / obj.timestep;
-                    end
-                    kk = kk + 1;
 
-                    % Direction 2: higher-index agent (jj) receives lower (ii).
-                    % Lower triangle of hComms stores this direction's barrier.
-                    [sinr2, grad2] = obj.sinrLink(lastPositions, jj, ii);
-                    hComms(jj, ii) = sinr2 - gammaCbf;
-                    for aa = 1:nAgents
-                        A(kk, (3 * aa - 2):(3 * aa)) = -grad2(aa, :);
-                    end
-                    if norm(A(kk, :)) < 1e-9
-                        b(kk) = 0;
-                    else
-                        b(kk) = hComms(jj, ii) / obj.timestep;
-                    end
-                    kk = kk + 1;
-                end
+                kk = kk + 1;
             end
         end
     end
-
-    if coder.target('MATLAB')
-        if obj.useSinrComms
-            % Both directions are maintained: log the upper triangle
-            % (lower-index receiver) then the lower triangle (higher-index
-            % receiver), matching the doubled comms-barrier allocation.
-            commsLog = [hComms(triu(true(size(hComms)), 1)); hComms(tril(true(size(hComms)), -1))];
-        else
-            commsLog = hComms(triu(true(size(hComms)), 1));
-        end
-        obj.barriers(idx:(idx + numel(commsLog) - 1), obj.timestepIndex) = commsLog;
-    end
+    obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
 
     % Double-integrator: transform QP from velocity to acceleration space.
     % Single-integrator constraint: A * v <= b

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
@@ -14,14 +14,6 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         useDoubleIntegrator (1, 1) logical = false;
         dampingCoeff (1, 1) double = 2.0;
         useFixedTopology (1, 1) logical = false;
-        optimizeSensorPointing (1, 1) logical = false;
-        useSinrComms (1, 1) logical = false;
-        sinrThreshold (1, 1) double = 0;
-        pathLossExponent (1, 1) double = 2.0;
-        ambientTemp (1, 1) double = 290.0;
-        centerFreq (1, 1) double = 2.4e9;
-        bandwidth (1, 1) double = 20e6;
-        useRoutingTopology (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "miSim")};
@@ -79,18 +71,16 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
     % Set labels for agents and collision geometries in cases where they
-    % were not provieded at the time of their initialization.
-    % Guarded by coder.target: label is a fixed-size "" in codegen, and
-    % sprintf returns variable-length — incompatible even in a dead branch.
-    % On the compiled path agents always receive explicit labels from the caller.
-    if coder.target('MATLAB')
-        for ii = 1:size(obj.agents, 1)
-            if isempty(char(obj.agents{ii}.label))
-                obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
-            end
-            if isempty(char(obj.agents{ii}.collisionGeometry.label))
-                obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
-            end
+    % were not provieded at the time of their initialization
+    for ii = 1:size(obj.agents, 1)
+        % Agent
+        if isempty(char(obj.agents{ii}.label))
+            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
+        end
+
+        % Collision geometry
+        if isempty(char(obj.agents{ii}.collisionGeometry.label))
+            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
         end
     end
 
@@ -103,62 +93,15 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.useDoubleIntegrator = useDoubleIntegrator;
     obj.dampingCoeff = dampingCoeff;
     obj.useFixedTopology = useFixedTopology;
-    obj.optimizeSensorPointing = optimizeSensorPointing;
 
-    % Set communications model and SINR parameters
-    obj.useSinrComms = useSinrComms;
-    obj.sinrThreshold = sinrThreshold;
-    obj.pathLossExponent = pathLossExponent;
-    obj.ambientTemp = ambientTemp;
-    obj.centerFreq = centerFreq;
-    obj.bandwidth = bandwidth;
-
-    % Set topology-selection algorithm; fail fast on invalid mode combinations
-    assert(~(useRoutingTopology && useFixedTopology), ...
-        "useRoutingTopology and useFixedTopology are mutually exclusive topology selectors");
-    assert(~useRoutingTopology || useSinrComms, ...
-        "useRoutingTopology requires the SINR comms model (useSinrComms)");
-    obj.useRoutingTopology = useRoutingTopology;
-
-    % Compute adjacency matrix and network topology. The Capacity-Aware
-    % Lesser Sink Neighbor algorithm replaces lesser-neighbor when selected
-    % (MATLAB sim only; the compiled path never sets the flag).
+    % Compute adjacency matrix and network topology
     obj = obj.updateAdjacency();
     if obj.useFixedTopology
         obj.constraintAdjacencyMatrix = obj.adjacency;
-    elseif coder.target('MATLAB') && obj.useRoutingTopology
-        obj = obj.lesserSinkNeighbor();
     else
         obj = obj.lesserNeighbor();
     end
 
-    % Guard against the SINR CBF deadlock band. Connectivity admits links
-    % with SINR anywhere above sinrThreshold, but constrainMotion maintains
-    % every selected link at sinrThreshold + 3 dB (paddingDb — keep in
-    % sync) in BOTH directions. A link selected inside that 3 dB band is
-    % born with a violated barrier; the QP can then go infeasible and hold
-    % positions, and since a frozen geometry re-selects the same link, the
-    % fleet deadlocks permanently. This catches the initial configuration;
-    % geometry evolution can still enter the band mid-run (fixing that
-    % requires pad-aware selection or a bounded-recovery barrier floor).
-    if coder.target('MATLAB') && obj.useSinrComms
-        gammaCbfLin = 10^((obj.sinrThreshold + 3.0) / 10);
-        posChk = zeros(size(obj.agents, 1), 3);
-        for kk = 1:size(obj.agents, 1)
-            posChk(kk, :) = obj.agents{kk}.pos;
-        end
-        for ii = 1:(size(obj.agents, 1) - 1)
-            for jj = (ii + 1):size(obj.agents, 1)
-                if obj.constraintAdjacencyMatrix(ii, jj)
-                    sinrBinding = min(obj.sinrLink(posChk, ii, jj), obj.sinrLink(posChk, jj, ii));
-                    assert(sinrBinding >= gammaCbfLin, ...
-                        "Initial topology link %d-%d has SINR %.1f dB, inside the CBF maintenance band (sinrThreshold %.1f dB + 3 dB padding): the QP starts with a violated comms barrier and can deadlock. Lower sinrThreshold or adjust the initial geometry.", ...
-                        ii, jj, 10 * log10(sinrBinding), obj.sinrThreshold);
-                end
-            end
-        end
-    end
-
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
 
@@ -166,6 +109,8 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Prepare performance data store (at t = 0, all have 0 performance)
         obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
+        % Prepare h function data store
+        obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
     end
 
     % Create initial partitioning
@@ -179,16 +124,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     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)
-    % Comms barriers: size to the upper bound (full mesh) rather than the initial
-    % constraint-adjacency count. Connectivity is dynamic (lesser-neighbor and,
-    % especially, SINR) and can later maintain more pairs than at init; sizing to
-    % nAAPairs prevents the QP A/b matrices from overflowing mid-run. SINR mode
-    % enforces both directions per link, so it needs two barrier rows per pair.
-    if obj.useSinrComms
-        nLNAPairs = 2 * nAAPairs;
-    else
-        nLNAPairs = nAAPairs;
-    end
+    nLNAPairs = sum(triu(obj.constraintAdjacencyMatrix, 1), "all");
     obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
 
     if coder.target('MATLAB')
@@ -207,7 +143,6 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
         obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
 
-
         % Set up plots showing initialized state
         obj = obj.plot();
 

@miSim/initializeFromCsv.m

@@ -52,19 +52,8 @@ 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
-
+OBJECTIVE_GROUND_POS       = scenario.objectivePos;             % 1×2
+OBJECTIVE_VAR              = reshape(scenario.objectiveVar, 2, 2); % 2×2 covariance matrix
 SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
 
 % Initial UAV positions: flat vector reshaped to N×3
@@ -107,53 +96,6 @@ else
     USE_FIXED_TOPOLOGY = false;
 end
 
-% SINR communications model (optional columns — backward compatible with
-% older CSVs that predate the SINR model; defaults keep the fixed-radius path)
-if isfield(scenario, 'txPower')
-    TX_POWER_VEC = scenario.txPower;                       % 1×N (Watts)
-    assert(numel(TX_POWER_VEC) == numAgents, ...
-        "txPower has %d values but expected %d (one per UAV)", numel(TX_POWER_VEC), numAgents);
-else
-    TX_POWER_VEC = 0.1 * ones(1, numAgents);
-end
-if isfield(scenario, 'useSinrComms')
-    USE_SINR_COMMS = logical(scenario.useSinrComms);
-else
-    USE_SINR_COMMS = false;
-end
-if isfield(scenario, 'sinrThreshold')
-    SINR_THRESHOLD = scenario.sinrThreshold;               % dB
-else
-    SINR_THRESHOLD = 0;
-end
-if isfield(scenario, 'pathLossExponent')
-    PATH_LOSS_EXPONENT = scenario.pathLossExponent;
-else
-    PATH_LOSS_EXPONENT = 2.0;
-end
-if isfield(scenario, 'ambientTemp')
-    AMBIENT_TEMP = scenario.ambientTemp;                   % Kelvin
-else
-    AMBIENT_TEMP = 290.0;
-end
-if isfield(scenario, 'centerFreq')
-    CENTER_FREQ = scenario.centerFreq;                     % Hz
-else
-    CENTER_FREQ = 2.4e9;
-end
-if isfield(scenario, 'bandwidth')
-    BANDWIDTH = scenario.bandwidth;                        % Hz
-else
-    BANDWIDTH = 20e6;
-end
-
-% Routing topology selection (optional column; MATLAB sim only)
-if isfield(scenario, 'useRoutingTopology')
-    USE_ROUTING_TOPOLOGY = logical(scenario.useRoutingTopology);
-else
-    USE_ROUTING_TOPOLOGY = false;
-end
-
 % ---- Build domain --------------------------------------------------------
 dom = rectangularPrism;
 dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
@@ -181,7 +123,6 @@ for ii = 1:numAgents
     ag = agent;
     ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
                        INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
-    ag.txPower = TX_POWER_VEC(ii); % SINR comms model transmit power (W)
     agentList{ii} = ag;
 end
 
@@ -201,8 +142,6 @@ 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, false, ...
-                     USE_SINR_COMMS, SINR_THRESHOLD, PATH_LOSS_EXPONENT, AMBIENT_TEMP, CENTER_FREQ, BANDWIDTH, ...
-                     USE_ROUTING_TOPOLOGY);
+                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
 
 end

@miSim/initializeFromInits.m

@@ -1,127 +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));
-    if isfield(inits, 'txPower')
-        ag.txPower = inits.txPower(ii); % SINR comms model transmit power (W)
-    end
-    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
-if isfield(inits, 'useSinrComms')
-    useSinrComms = logical(inits.useSinrComms);
-else
-    useSinrComms = false;
-end
-if isfield(inits, 'sinrThreshold')
-    sinrThreshold = inits.sinrThreshold;
-else
-    sinrThreshold = 0;
-end
-if isfield(inits, 'pathLossExponent')
-    pathLossExponent = inits.pathLossExponent;
-else
-    pathLossExponent = 2.0;
-end
-if isfield(inits, 'ambientTemp')
-    ambientTemp = inits.ambientTemp;
-else
-    ambientTemp = 290.0;
-end
-if isfield(inits, 'centerFreq')
-    centerFreq = inits.centerFreq;
-else
-    centerFreq = 2.4e9;
-end
-if isfield(inits, 'bandwidth')
-    bandwidth = inits.bandwidth;
-else
-    bandwidth = 20e6;
-end
-if isfield(inits, 'useRoutingTopology')
-    useRoutingTopology = logical(inits.useRoutingTopology);
-else
-    useRoutingTopology = 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, false, ...
-                     useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, ...
-                     useRoutingTopology);
-
-end

@miSim/lesserSinkNeighbor.m

@@ -1,372 +0,0 @@
-function obj = lesserSinkNeighbor(obj)
-% LESSERSINKNEIGHBOR  Capacity-Aware Lesser Sink Neighbor algorithm (CALSN).
-%
-% Implements the algorithm specified in new/new_routing_algorithm.pdf as the
-% selectable alternative to lesserNeighbor. UAVs 1 and N are base stations;
-% every other UAV generates r = 1 unit of data per step, and all data must
-% reach a base station. The algorithm outputs both a sparse maintained
-% topology and a routing table.
-%
-% 1. Base-station potentials phi via distributed Bellman-Ford over the
-%    current communication neighborhoods (obj.adjacency), with link cost
-%    c_ij = 1/(C_ij + eps), where C_ij = bandwidth * log2(1 + SINR_ij) is
-%    the Shannon capacity of direction i -> j.
-% 2. Lesser sink neighbors L_i replace LNA's "j < i" rule:
-%        L_i = { j in N_i : phi_j < phi_i, or phi_j == phi_i and j < i }.
-%    The equal-potential lexicographic tie-break is a documented deviation
-%    from the spec's strict inequality: at an exact potential watershed
-%    (e.g. a symmetric chain) the strict rule would split A_control into
-%    one tree per base station; the tie-break keeps the airborne graph
-%    connected, which the mission requires.
-% 3. LNA-style sparsity: one parent per connected component of the
-%    L_i-induced subgraph (only links with positive capacity in at least
-%    one direction count as edges), chosen by
-%        p = argmin_{j in C} ( phi_j + 1/(C_ij + eps) ).
-%    A_control(i,p) = A_control(p,i) = 1 (diagonal = I).
-%    BASIN EXTENSION (documented deviation): with two potential minima
-%    (the two bases), one-parent-per-component alone yields a spanning
-%    forest with up to two disjoint base-rooted trees whenever a UAV's
-%    lesser sink neighbors collapse into a single component (dense usable
-%    graphs). To restore LNA's global-connectivity property, each UAV
-%    selects one representative per component AND per base-station BASIN
-%    (the base a neighbor's potential descent terminates at). Interior
-%    UAVs are unaffected (their whole neighborhood shares one basin);
-%    watershed UAVs whose neighborhoods span both basins gain a second
-%    parent, which is exactly the bridge between the two trees. A final
-%    stitch guard covers degenerate geometries: if A_control still has
-%    multiple components, the lowest-cost feasible inter-component link
-%    is added (maintained for connectivity only; carries no flow).
-% 4. Routing, children first (decreasing phi; equal phi processed in
-%    decreasing index order so tie-break children precede their parents):
-%    aggregate demand D_i = r + inflows, split capacity-proportionally
-%    over parents; saturated links are capped at capacity and the residual
-%    redistributed over unsaturated parents; any demand beyond the total
-%    parent capacity is reported as unrouted and the UAV marked infeasible.
-%
-% Outputs (set on obj):
-%   constraintAdjacencyMatrix  A_control: undirected maintained topology
-%   routingFlows               F: flow on each directed link (units of r)
-%   routingFractions           R = F/D: share of the sender's demand, 0..1
-%   routingAdjacencyMatrix     F > 0 (directed, child -> parent)
-%   routingPotentials          phi (base-station potential per UAV)
-%   routingUnrouted            unrouted demand per UAV (zeros when feasible)
-
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, "miSim")};
-    end
-
-    nAgents = size(obj.agents, 1);
-    r = 1.0;         % data generated per non-base UAV per step (abstract unit)
-    EPS_CAP = 1e-12; % epsilon in c = 1/(C + eps); avoids division by zero
-    TOL = 1e-12;     % residual-demand tolerance for the saturation loop
-
-    isBase = false(nAgents, 1);
-    isBase(1) = true;
-    isBase(nAgents) = true; % base stations are UAVs 1 and N
-
-    positions = zeros(nAgents, 3);
-    for kk = 1:nAgents
-        positions(kk, :) = obj.agents{kk}.pos;
-    end
-
-    % Directed Shannon link capacities over the current communication
-    % neighborhoods: C(i, j) = capacity of sending from i to j (receiver j)
-    C = zeros(nAgents);
-    for ii = 1:nAgents
-        for jj = 1:nAgents
-            if jj ~= ii && obj.adjacency(ii, jj)
-                C(ii, jj) = obj.bandwidth * log2(1 + obj.sinrLink(positions, jj, ii));
-            end
-        end
-    end
-
-    % ---- Base-station potentials (Bellman-Ford, at most N-1 sweeps) -----
-    phi = inf(nAgents, 1);
-    phi(isBase) = 0;
-    for sweep = 1:max(nAgents - 1, 1)
-        changed = false;
-        for ii = 1:nAgents
-            if isBase(ii)
-                continue;
-            end
-            newPhi = inf;
-            for jj = 1:nAgents
-                if jj == ii || ~obj.adjacency(ii, jj)
-                    continue;
-                end
-                newPhi = min(newPhi, phi(jj) + 1 / (C(ii, jj) + EPS_CAP));
-            end
-            if newPhi ~= phi(ii)
-                phi(ii) = newPhi;
-                changed = true;
-            end
-        end
-        if ~changed
-            break;
-        end
-    end
-
-    % ---- Base-station basins (for the basin extension) -------------------
-    % Each finite-potential UAV descends its argmin neighbor chain; phi is
-    % strictly decreasing along it, so the chain terminates at a base. The
-    % basin label records which base that is.
-    descent = zeros(nAgents, 1);
-    for ii = 1:nAgents
-        if isBase(ii) || isinf(phi(ii))
-            continue;
-        end
-        bestMetric = inf;
-        for jj = 1:nAgents
-            if jj == ii || ~obj.adjacency(ii, jj)
-                continue;
-            end
-            metric = phi(jj) + 1 / (C(ii, jj) + EPS_CAP);
-            if metric < bestMetric
-                bestMetric = metric;
-                descent(ii) = jj;
-            end
-        end
-    end
-    basin = zeros(nAgents, 1);
-    basin(1) = 1;
-    basin(nAgents) = nAgents;
-    for ii = 1:nAgents
-        if isBase(ii) || isinf(phi(ii))
-            continue;
-        end
-        kk = ii;
-        while ~isBase(kk)
-            kk = descent(kk);
-        end
-        basin(ii) = kk;
-    end
-
-    % ---- Parent selection (one per lesser-sink component and basin) ------
-    Acontrol = logical(eye(nAgents));
-    isParent = false(nAgents); % isParent(i, p): p is a selected parent of i
-    for ii = 1:nAgents
-        if isBase(ii) || isinf(phi(ii))
-            % Base stations select no parents; an infinite potential means
-            % no current local route toward a base station
-            continue;
-        end
-
-        % Lesser sink neighbors (with the equal-potential tie-break)
-        lsnBuf = zeros(1, nAgents);
-        lsnCount = 0;
-        for jj = 1:nAgents
-            if jj == ii || ~obj.adjacency(ii, jj)
-                continue;
-            end
-            if phi(jj) < phi(ii) || (phi(jj) == phi(ii) && jj < ii)
-                lsnCount = lsnCount + 1;
-                lsnBuf(lsnCount) = jj;
-            end
-        end
-        if lsnCount == 0
-            continue;
-        end
-        lsn = lsnBuf(1:lsnCount);
-
-        % Subgraph induced by the lesser sink neighbors in the usable
-        % communication graph: only links with positive capacity in at
-        % least one direction count as edges (a zero-capacity link cannot
-        % carry data, so it must not merge two components)
-        subgraphAdjacency = false(lsnCount);
-        for aa = 1:lsnCount
-            for bb = (aa + 1):lsnCount
-                ja = lsn(aa);
-                jb = lsn(bb);
-                if obj.adjacency(ja, jb) && (C(ja, jb) > 0 || C(jb, ja) > 0)
-                    subgraphAdjacency(aa, bb) = true;
-                    subgraphAdjacency(bb, aa) = true;
-                end
-            end
-        end
-
-        % One parent per connected component and per base-station basin
-        % present in it (basin extension), selected by capacity and
-        % potential: p = argmin( phi_j + 1/(C_ij + eps) )
-        visited = false(1, lsnCount);
-        for aa = 1:lsnCount
-            if visited(aa)
-                continue;
-            end
-            component = bfs(subgraphAdjacency, aa);
-            visited(component) = true;
-
-            for bb = [1, nAgents] % the two possible basin labels
-                bestMetric = inf;
-                bestParent = 0;
-                for cc = 1:numel(component)
-                    jj = lsn(component(cc));
-                    if basin(jj) ~= bb
-                        continue;
-                    end
-                    metric = phi(jj) + 1 / (C(ii, jj) + EPS_CAP);
-                    if metric < bestMetric
-                        bestMetric = metric;
-                        bestParent = jj;
-                    end
-                end
-                if bestParent > 0
-                    isParent(ii, bestParent) = true;
-                    Acontrol(ii, bestParent) = true;
-                    Acontrol(bestParent, ii) = true;
-                end
-            end
-        end
-    end
-
-    % ---- Connectivity stitch guard ---------------------------------------
-    % The basin extension bridges the two base-rooted trees through
-    % watershed UAVs whenever each base borders a routing basin. Degenerate
-    % geometries (e.g. a base whose only feasible neighbor is the other
-    % base) can still leave A_control split; add the lowest-cost feasible
-    % link between components until one remains. Stitched links are
-    % maintained for connectivity only and carry no routed flow. If the
-    % feasibility graph itself is disconnected no stitch exists; validate()
-    % reports that case.
-    while true
-        comp = zeros(1, nAgents);
-        nComp = 0;
-        for ii = 1:nAgents
-            if comp(ii) == 0
-                nComp = nComp + 1;
-                comp(bfs(Acontrol, ii)) = nComp;
-            end
-        end
-        if nComp <= 1
-            break;
-        end
-        bestMetric = inf;
-        bestU = 0;
-        bestV = 0;
-        for uu = 1:nAgents
-            for vv = (uu + 1):nAgents
-                if comp(uu) ~= comp(vv) && obj.adjacency(uu, vv)
-                    % Bidirectional link cost (finite even for infinite
-                    % potentials, so isolated islands remain stitchable)
-                    metric = 1 / (C(uu, vv) + EPS_CAP) + 1 / (C(vv, uu) + EPS_CAP);
-                    if metric < bestMetric
-                        bestMetric = metric;
-                        bestU = uu;
-                        bestV = vv;
-                    end
-                end
-            end
-        end
-        if bestU == 0
-            break; % feasibility graph disconnected: nothing to stitch with
-        end
-        Acontrol(bestU, bestV) = true;
-        Acontrol(bestV, bestU) = true;
-    end
-
-    % ---- Routing (children first: decreasing potential order) -----------
-    % Equal potentials are processed in decreasing index order: with the
-    % lexicographic tie-break, flow within an equal-potential group goes
-    % from higher to lower index, so higher indices must aggregate first.
-    F = zeros(nAgents);
-    R = zeros(nAgents);
-    unrouted = zeros(nAgents, 1);
-
-    nonBase = find(~isBase);
-    [~, ord] = sortrows([phi(nonBase), nonBase], [-1, -2]);
-    order = nonBase(ord);
-
-    for oo = 1:numel(order)
-        ii = order(oo);
-
-        % Aggregated outgoing demand: own data plus everything received
-        D = r + sum(F(:, ii));
-        parents = find(isParent(ii, :));
-        if D == 0 || isempty(parents)
-            continue;
-        end
-
-        % Capacity-proportional split, saturating full links first and
-        % redistributing the remainder across parents with remaining
-        % capacity. Each pass either routes all residual demand or
-        % saturates at least one parent, so the loop terminates.
-        capP = C(ii, parents);
-        flows = zeros(1, numel(parents));
-        active = true(1, numel(parents));
-        residual = D;
-        while residual > TOL && any(active)
-            totalCap = sum(capP(active));
-            if totalCap <= 0
-                break;
-            end
-            alloc = zeros(1, numel(parents));
-            alloc(active) = residual .* capP(active) ./ totalCap;
-            flows = flows + alloc;
-            saturated = active & (flows > capP);
-            if any(saturated)
-                residual = sum(flows(saturated) - capP(saturated));
-                flows(saturated) = capP(saturated);
-                active(saturated) = false;
-            else
-                residual = 0;
-            end
-        end
-        if residual > TOL
-            % Demand exceeds the total parent capacity: route as much as
-            % possible and report the remainder as unrouted (infeasible)
-            unrouted(ii) = residual;
-            warning("miSim:routingInfeasible", ...
-                "UAV %d demand %.3g exceeds total parent capacity; %.3g units unrouted", ...
-                ii, D, residual);
-        end
-
-        F(ii, parents) = flows;
-        R(ii, parents) = flows ./ D;
-    end
-
-    % ---- Outputs ---------------------------------------------------------
-    obj.constraintAdjacencyMatrix = Acontrol;
-    obj.routingFlows = F;
-    obj.routingFractions = R;
-    obj.routingAdjacencyMatrix = F > 0;
-    obj.routingPotentials = phi;
-    obj.routingUnrouted = unrouted;
-end
-
-function cComp = bfs(subgraphAdjacency, startIdx)
-    % Breadth-first search returning the connected component of startIdx.
-    % Pre-allocated queue and component buffer with head/tail pointers to
-    % avoid element deletion and dynamic array growth (same pattern as the
-    % local bfs in lesserNeighbor.m).
-    n = size(subgraphAdjacency, 1);
-    visited = false(1, n);
-
-    queue    = zeros(1, n);
-    cCompBuf = zeros(1, n);
-    qHead = 1;
-    qTail = 2;
-    queue(1)    = startIdx;
-    cCompBuf(1) = startIdx;
-    cSize = 1;
-    visited(startIdx) = true;
-
-    while qHead < qTail
-        current = queue(qHead);
-        qHead = qHead + 1;
-
-        neighbors = find(subgraphAdjacency(current, :));
-        for kk = 1:numel(neighbors)
-            neighbor = neighbors(kk);
-            if ~visited(neighbor)
-                visited(neighbor) = true;
-                cCompBuf(cSize + 1) = neighbor;
-                cSize = cSize + 1;
-                queue(qTail) = neighbor;
-                qTail = qTail + 1;
-            end
-        end
-    end
-    cComp = sort(cCompBuf(1:cSize));
-end

@miSim/miSim.m

@@ -20,28 +20,6 @@ classdef miSim
         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
-        % Communications model selection and SINR-model parameters.
-        % useSinrComms = false -> fixed-radius comms (comRange / commsGeometry).
-        % useSinrComms = true  -> SINR/path-loss comms (txPower + params below).
-        useSinrComms = false;
-        sinrThreshold = 0;        % SINR threshold for connectivity & CBF maintenance (dB)
-        pathLossExponent = 2.0;   % path-loss exponent n in S = P / (K * d^n)
-        ambientTemp = 290.0;      % ambient temperature (K) for thermal noise N = k_B*T*B
-        centerFreq = 2.4e9;       % carrier frequency (Hz); sets path-loss offset K = (4*pi*f_c/c)^2
-        bandwidth = 20e6;         % channel bandwidth (Hz) for thermal noise N = k_B*T*B
-        % Topology-selection algorithm (requires useSinrComms; MATLAB sim only).
-        % false -> lesser-neighbor; true -> Capacity-Aware Lesser Sink Neighbor
-        % (CALSN, see new/new_routing_algorithm.pdf): base-station potentials
-        % replace the index-based "lesser" rule, and the maintained topology
-        % comes with a routing table (flows F, fractions R) that sends every
-        % UAV's data toward base stations 1 and N.
-        useRoutingTopology = false;
-        routingAdjacencyMatrix = false(0, 0); % directed flow links (i, j) = child i sends to parent j (F > 0)
-        routingFlows = zeros(0, 0);      % F: flow on each directed link (units of r = 1 per UAV per step)
-        routingFractions = zeros(0, 0);  % R = F/D: share of the sender's aggregated demand per link (0..1)
-        routingPotentials = zeros(0, 1); % phi: Bellman-Ford base-station potential per UAV
-        routingUnrouted = zeros(0, 1);   % unrouted demand per UAV (zeros when routing is feasible)
         artifactName = "";
         f; % main plotting tiled layout figure
         fPerf; % performance plot figure
@@ -76,6 +54,7 @@ classdef miSim
         partitionGraphIndex = 1;
 
         % CBF plotting
+        h; % h function values
         hf; % h function plotting figure
         caPlot; % objects for collision avoidance h function plot
         obsPlot; % objects for obstacle h function plot
@@ -91,15 +70,11 @@ classdef miSim
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
         [obj] = initializeFromCsv(obj, csvPath);
-        [obj] = initializeFromInits(obj, initsPath);
-        [obj] = plotFromSimHist(obj, initsPath, histPath);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
-        [obj] = lesserSinkNeighbor(obj);
         [obj] = constrainMotion(obj);
-        [sinr, grad] = sinrLink(obj, positions, rxIdx, txIdx);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
         [obj] = plot(obj);

@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

@@ -6,82 +6,23 @@ function obj = plotGraph(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % The maintained links are always drawn as a dotted undirected graph.
-    % In routing (CALSN) mode the flow-carrying links are overlaid as SOLID
-    % DIRECTED edges (arrow = child -> parent, toward a base station) on
-    % the same node layout.
+    % Form graph from adjacency matrix
     G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
 
-    % Resolve the target axes for the network graph tile
+    % Plot graph object
     if isnan(obj.networkGraphIndex)
-        ax = obj.f.CurrentAxes;
+        hold(obj.f.CurrentAxes, "on");
+        o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
+        hold(obj.f.CurrentAxes, "off");
     else
-        ax = obj.f.Children(1).Children(obj.networkGraphIndex(1));
-    end
-
-    % Plot graph object(s)
-    hold(ax, "on");
-    o = plot(ax, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
-    if obj.useRoutingTopology
-        % Flow overlay: reuse the base layout's node coordinates and hide
-        % the overlay's own nodes/labels so only its edges show. Sizeable
-        % arrowheads so the flow direction reads at tile size.
-        D = digraph(obj.routingAdjacencyMatrix);
-        o = [o; plot(ax, D, "XData", o(1).XData, "YData", o(1).YData, ...
-                     "LineStyle", "-", "EdgeColor", "g", "LineWidth", 2, ...
-                     "ArrowSize", 12, "Marker", "none", "NodeLabel", {})];
-    end
-    hold(ax, "off");
-    nBase = size(o, 1);
-    if ~isnan(obj.networkGraphIndex) && size(obj.networkGraphIndex, 2) > 1
-        for ii = 2:size(obj.networkGraphIndex, 2)
-            o = [o; copyobj(o(1:nBase), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
-        end
-    end
-
-    % In SINR comms mode, label each edge with the connection's SINR and report
-    % the connectivity threshold in the tile title. The fixed-radius model is
-    % left unchanged (no edge labels, title stays as set in firstPlotSetup).
-    if obj.useSinrComms
-        endNodes = G.Edges.EndNodes;
-        nAgents = size(obj.agents, 1);
-        positions = zeros(nAgents, 3);
-        for kk = 1:nAgents
-            positions(kk, :) = obj.agents{kk}.pos;
-        end
-        % Labels are bare numbers (no units) so they stay compact; GraphPlot
-        % renders them horizontally, left-to-right. Dotted (maintained) edges
-        % are labelled with the binding (worse) directional SINR — a link is
-        % feasible only if BOTH directions clear the threshold. Links that
-        % also carry flow leave the label to the solid directed edge, which
-        % shows the flow value (units of r) sent child -> parent.
-        edgeLabels = strings(size(endNodes, 1), 1);
-        for ee = 1:size(endNodes, 1)
-            lo = endNodes(ee, 1);
-            hi = endNodes(ee, 2);
-            if obj.useRoutingTopology && (obj.routingAdjacencyMatrix(lo, hi) || obj.routingAdjacencyMatrix(hi, lo))
-                edgeLabels(ee) = ""; % solid overlay carries this link's label
-            else
-                sinrLoRx = obj.sinrLink(positions, lo, hi); % receiver lo (lower index)
-                sinrHiRx = obj.sinrLink(positions, hi, lo); % receiver hi (higher index)
-                edgeLabels(ee) = sprintf("%.1f", 10 * log10(min(sinrLoRx, sinrHiRx)));
+        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)))];
             end
         end
-        o(1).EdgeLabel = edgeLabels;
-        if obj.useRoutingTopology
-            endNodesD = D.Edges.EndNodes;   % each row [child parent]
-            flowLabels = strings(size(endNodesD, 1), 1);
-            for ee = 1:size(endNodesD, 1)
-                flowLabels(ee) = sprintf("%.2f", obj.routingFlows(endNodesD(ee, 1), endNodesD(ee, 2)));
-            end
-            o(2).EdgeLabel = flowLabels;
-        end
-        % Threshold goes in the existing single-line title (not a subtitle) so
-        % the small tile is not compressed further, and the hover toolbar is
-        % disabled so the R2026 "..." menu stops covering the readout.
-        title(ax, sprintf("Network Graph (\\geq %.1f dB)", obj.sinrThreshold));
-        ax.Toolbar.Visible = "off";
     end
-
     obj.graphPlot = o;
 end

@miSim/plotH.m

@@ -6,10 +6,6 @@ function obj = plotH(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    nCA  = size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2;
-    nObs = size(obj.agents, 1) * size(obj.obstacles, 1);
-    nDom = size(obj.agents, 1) * 6;
-
     obj.hf = figure;
     tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
 
@@ -19,7 +15,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.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)
@@ -35,7 +31,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.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)
@@ -51,13 +47,8 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.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));

@miSim/run.m

@@ -10,13 +10,7 @@ function [obj] = run(obj)
         % 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
     end
 
@@ -31,25 +25,13 @@ function [obj] = run(obj)
             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
-
         % 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: the Capacity-Aware Lesser
-        % Sink Neighbor algorithm replaces lesser-neighbor when selected
+        % Determine desired communications links
         if ~obj.useFixedTopology
-            if coder.target('MATLAB') && obj.useRoutingTopology
-                obj = obj.lesserSinkNeighbor();
-            else
-                obj = obj.lesserNeighbor();
-            end
+            obj = obj.lesserNeighbor();
         end
 
         % Log constraint adjacency for this timestep
@@ -60,7 +42,7 @@ function [obj] = run(obj)
         % 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, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep);
         end
 
         % Adjust motion determined by unconstrained gradient ascent using
@@ -84,9 +66,6 @@ function [obj] = run(obj)
             % 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

@miSim/sinrLink.m

@@ -1,101 +0,0 @@
-function [sinr, grad] = sinrLink(obj, positions, rxIdx, txIdx)
-% SINRLINK  Linear SINR of the link txIdx -> rxIdx and its position gradient.
-%
-% Computes the signal-to-interference-plus-noise ratio seen at receiver rxIdx
-% for the signal transmitted by txIdx, treating every other agent as an
-% interferer at that receiver. Isotropic transmit/receive (no antenna gains).
-%
-% Returns the linear SINR and (optionally) its gradient with respect to every
-% agent position, grad(a, :) = d(SINR)/d(p_a). The value is used by
-% updateAdjacency (connectivity) and both the value and gradient by
-% constrainMotion (the SINR CBF barrier).
-%
-% Model (linear power domain), receiver r = rxIdx, transmitter t = txIdx:
-%   S    = P_t / (K * d_tr^n)                 % received signal at r from t (W)
-%   I    = sum_{k != t,r} P_k / (K * d_kr^n)  % interference at r from others (W)
-%   N    = k_B * T * B                        % thermal noise (W)
-%   SINR = S / (N + I)
-% with K = (4*pi*f_c/c)^2 and physical constants baked in below. The only
-% tunables are per-agent txPower (P), pathLossExponent (n), ambientTemp (T),
-% all read from obj; the SINR threshold is applied by the callers.
-
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "miSim")};
-        positions (:, 3) double;
-        rxIdx (1, 1) double;
-        txIdx (1, 1) double;
-    end
-    arguments (Output)
-        sinr (1, 1) double;
-        grad (:, 3) double;
-    end
-
-    % Universal physical constants
-    k_B = 1.380649e-23;          % Boltzmann constant (J/K)
-    c   = 3e8;                   % speed of light (m/s)
-
-    % System parameters
-    f_c = obj.centerFreq;        % carrier frequency (Hz)
-    B   = obj.bandwidth;         % channel bandwidth (Hz)
-    K   = (4 * pi * f_c / c)^2;  % free-space path-loss reference constant
-
-    n  = obj.pathLossExponent;
-    T  = obj.ambientTemp;
-    d0 = 1.0;                    % near-field distance floor (m)
-
-    nAgents = size(positions, 1);
-    grad = zeros(nAgents, 3);
-
-    r  = rxIdx;
-    t  = txIdx;
-    pr = positions(r, :);
-
-    % Thermal noise (always > 0, so the denominator never vanishes)
-    N = k_B * T * B;
-
-    % Signal term S (source = transmitter t)
-    P_t    = obj.agents{t}.txPower;
-    dvec_t = positions(t, :) - pr;
-    d_t    = max(norm(dvec_t), d0);
-    S      = P_t / (K * d_t^n);
-
-    % Interference I = sum of received powers from every other agent at r
-    I = 0.0;
-    for k = 1:nAgents
-        if k == r || k == t
-            continue;
-        end
-        d_k = max(norm(positions(k, :) - pr), d0);
-        I   = I + obj.agents{k}.txPower / (K * d_k^n);
-    end
-
-    D    = N + I;       % denominator: noise + interference
-    sinr = S / D;
-
-    % ---- Gradient (skip unless requested) -------------------------------
-    % For a received term P_x/(K*d_xr^n):
-    %   d(term)/dp_x = -n*term/d_xr^2 * (p_x - p_r),  d(term)/dp_r = -d(term)/dp_x
-    if nargout > 1
-        % Transmitter affects S only: dSINR/dp_t = (dS/dp_t)/D
-        dS_dpt = (-n * S / (d_t^2)) * dvec_t;
-        grad(t, :) = dS_dpt / D;
-
-        % Interferers affect I only; accumulate the receiver's I-contribution
-        dI_dpr = zeros(1, 3);
-        for k = 1:nAgents
-            if k == r || k == t
-                continue;
-            end
-            dvec_k    = positions(k, :) - pr;
-            d_k       = max(norm(dvec_k), d0);
-            term_k    = obj.agents{k}.txPower / (K * d_k^n);
-            dterm_dpk = (-n * term_k / (d_k^2)) * dvec_k;
-            grad(k, :) = (-S / (D^2)) * dterm_dpk;      % dSINR/dp_k
-            dI_dpr     = dI_dpr - dterm_dpk;            % d(term_k)/dp_r = -d(term_k)/dp_k
-        end
-
-        % Receiver affects both S and I
-        dS_dpr = -dS_dpt;
-        grad(r, :) = dS_dpr / D - (S / (D^2)) * dI_dpr;
-    end
-end

@miSim/updateAdjacency.m

@@ -6,41 +6,16 @@ function obj = updateAdjacency(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    nAgents = size(obj.agents, 1);
+    % Initialize assuming only self-connections
+    A = true(size(obj.agents, 1));
 
-    % Initialize assuming all agents are connected
-    A = true(nAgents);
-
-    if ~obj.useSinrComms
-        % Fixed-radius model: pair connected iff separation is within the
-        % smaller of the two comms radii.
-        for ii = 2:nAgents
-            for jj = 1:(ii - 1)
-                if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
-                    A(ii, jj) = false; % comm range violation
-                    continue;
-                end
-            end
-        end
-    else
-        % SINR model: a link between a pair is feasible only if BOTH directional
-        % SINRs clear the threshold, i.e. each agent receives the other's signal
-        % above gammaLin (with interference from all other agents at the
-        % receiver). Every ordered pair (permutation) is evaluated; a single
-        % failing direction eliminates the undirected link.
-        gammaLin = 10^(obj.sinrThreshold / 10); % dB threshold -> linear
-        positions = zeros(nAgents, 3);
-        for kk = 1:nAgents
-            positions(kk, :) = obj.agents{kk}.pos;
-        end
-        for ii = 2:nAgents
-            for jj = 1:(ii - 1)
-                sinrLoRx = obj.sinrLink(positions, jj, ii); % receiver jj (lower), transmitter ii
-                sinrHiRx = obj.sinrLink(positions, ii, jj); % receiver ii (higher), transmitter jj
-                if sinrLoRx < gammaLin || sinrHiRx < gammaLin
-                    A(ii, jj) = false; % at least one direction below threshold
-                    continue;
-                end
+    % Check lower triangle off-diagonal connections
+    for ii = 2:size(A, 1)
+        for jj = 1:(ii - 1)
+            % Check that agents are not out of range
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
+                A(ii, jj) = false; % comm range violation
+                continue;
             end
         end
     end

@miSim/updatePlots.m

@@ -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

@@ -5,26 +5,11 @@ function validate(obj)
     arguments (Output)
     end
 
-    %% Mode Validators
-    if obj.useRoutingTopology && ~obj.useSinrComms
-        error("Routing topology requires SINR comms mode (useSinrComms)");
-    end
-
-    if obj.useRoutingTopology && obj.useFixedTopology
-        error("useRoutingTopology and useFixedTopology are mutually exclusive topology selectors");
-    end
-
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
         error("Network is not connected");
     end
 
-    % In routing (CALSN) mode the MAINTAINED topology itself must form a
-    % single connected component (basin extension + stitch guard invariant)
-    if obj.useRoutingTopology && max(conncomp(graph(obj.constraintAdjacencyMatrix))) ~= 1
-        error("Maintained routing topology (A_control) is not connected");
-    end
-
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
         error("Eliminated network connections that were necessary");
     end

@miSim/writeInits.m

@@ -5,70 +5,29 @@ function writeInits(obj)
     arguments (Output)
     end
 
-    % User-supplied obstacles only: initialize() appends a floor obstacle at
-    % the end when minAlt > 0, so exclude it here to avoid double-counting on
-    % reconstruction (initializeFromInits re-adds the floor via minAlt).
-    numInputObs = size(obj.obstacles, 1) - (obj.minAlt > 0);
-    userObstacles = obj.obstacles(1:numInputObs);
-
     % Collect agent parameters
     collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
-    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);
+    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);
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
-    txPower = cellfun(@(x) x.txPower, 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));
+    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
+    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
+
 
     % Combine with simulation parameters
-    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter + 1, "minAlt", obj.minAlt, ...
+    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
                     "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, ...
+                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
                     "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
-                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
-                    "useSinrComms", obj.useSinrComms, "sinrThreshold", obj.sinrThreshold, "pathLossExponent", obj.pathLossExponent, "ambientTemp", obj.ambientTemp, ...
-                    "centerFreq", obj.centerFreq, "bandwidth", obj.bandwidth, "txPower", txPower, ... % SINR comms model parameters
-                    "useRoutingTopology", obj.useRoutingTopology, ... % topology-selection algorithm (CALSN when true)
-                    "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
+                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
+                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
                     "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
-                    "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...
                     "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
                     "objectiveIntegral", sum(obj.domain.objective.values(:)));
 

@rfSensor/RSS.m

@@ -1,23 +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);
-    value = obj.P_TX_dBm + obj.transmitterGain(theta) + 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,19 +0,0 @@
-function value = halfAngle(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
-    end
-    arguments (Output)
-        value (1, 1) double;
-    end
-    dtheta = (0:0.1:179.9)';
-    gain = obj.transmitterGain(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,35 +0,0 @@
-function obj = initialize(obj, txPower, temperature, bandwidth, centerFreq, rxGain_dBi, beamwidthExponent, tilt, azimuth, lossExponent)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, "rfSensor")}
-        txPower (1, 1) double;
-        temperature (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.T_0 = temperature; % Temperature (K)
-    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
-    obj.constantGainTerm_dB = 10 .* log10(2 * (obj.beamwidthExponent + 1)); % used in RSS calculation, only need to compute once
-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((4 * pi * obj.f_c) / 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,55 +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);
-
-        % Convert LOS (D, T, A) to displacement vectors for RSS
-        dx = D .* sind(T) .* sind(A);
-        dy = D .* sind(T) .* cosd(A);
-        dz = -D .* cosd(T);   % negative: ground targets are below agent
-        rss = obj.RSS(D(:), dx(:), dy(:), dz(:));
-        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,89 +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));
-
-    % 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');
-    cb = colorbar; cb.Label.String = "SNR (dB)";
-    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');
-    cb = colorbar; cb.Label.String = "SNR (dB)";
-    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,41 +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 = NaN; % 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
-        constantGainTerm_dB = NaN; % used in RSS calculation, only need to compute once
-        % 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, temperature, 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, theta); % Antenna gain for a given angle from boresight (degrees)
-        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,12 +0,0 @@
-function value = transmitterGain(obj, theta)
-    arguments (Input)
-        obj   (1, 1) {mustBeA(obj, "rfSensor")};
-        theta (:, 1) double; % angle from boresight (degrees)
-    end
-    arguments (Output)
-        value (:, 1) double
-    end
-    % Cosine pattern: peak at boresight, -inf dB at 90°.
-    % Clamped so targets behind the antenna (theta > 90°) get -inf rather than NaN.
-    value = obj.constantGainTerm_dB + 10 .* obj.beamwidthExponent .* log10(max(0, cosd(theta)));
-end

@sensingObjective/initialize.m

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

@sensingObjective/initializeRandomMvnpdf.m

@@ -16,11 +16,11 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     end
 
     % Set random distribution parameters
-    sig = 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/plot.m

@@ -11,39 +11,25 @@ function f = plot(obj, ind, f)
     % 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");
+        hold(f.CurrentAxes, "on");
+        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        clim(ax, cRange);
-        hold(ax, "off");
+        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");
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        clim(ax, cRange);
-        hold(ax, "off");
+        hold(f.Children(1).Children(ind(1)), "off");
     end
 
     % Add to other perspectives
     if size(ind, 2) > 1
         for ii = 2:size(ind, 2)
-            axCopy = f.Children(1).Children(ind(ii));
-            copyobj(o, axCopy);
-            % copyobj does not carry color scaling. The objective values are
-            % sum-normalized (tiny), so without matching CLim the copied surface
-            % is squished to one color once the destination axes' auto CLim is
-            % widened by later surfaces (e.g. agent geometries). Pin it to the
-            % source range so the heatmap renders as a visible gradient.
-            clim(axCopy, cRange);
+            copyobj(o, f.Children(1).Children(ind(ii)));
         end
     end
 end

@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")}
         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")}
     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,32 +1,22 @@
-function f = plotParameters(obj, f)
+function f = plotParameters(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
-        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
         f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Distance and tilt sample points
-    d = 0:(obj.alphaDist / 1000):(2*obj.alphaDist);
-    t = -90:0.1:90;
+    d = 0:(obj.alphaDist / 100):(2*obj.alphaDist);
+    t = -90:1:90;
 
     % Sample membership functions
     d_x = obj.distanceMembership(d);
     t_x = obj.tiltMembership(t);
 
-    % pad ends of sampled results with zeros at +/- some big number
-    big = 1e6 * max(d);
-    d = [-big, d, big];
-    t = [-big, t, big];
-    d_x = [0, d_x', 0];
-    t_x = [0, t_x', 0];
-
     % Plot resultant sigmoid curves
-    hold(gca(f), "on");
-    if isempty(gca(f).Children)
-        tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
-    end
+    f = figure;
+    tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
 
     % Distance
     nexttile(1, [1,  1]);
@@ -34,12 +24,9 @@ function f = plotParameters(obj, f)
     title("Distance Membership Sigmoid");
     xlabel("Distance (m)");
     ylabel("Membership");
-    ld = legend();
     hold("on");
     plot(d, d_x, "LineWidth", 2);
     hold("off");
-    xData = cell2mat(arrayfun(@(x) x.XData(2:(end - 1)), f.Children.Children(end).Children, 'UniformOutput', false));
-    xlim([min(xData, [], "all"), max(xData, [], "all")]);
     ylim([0, 1]);
 
     % Tilt
@@ -48,21 +35,8 @@ function f = plotParameters(obj, f)
     title("Tilt Membership Sigmoid");
     xlabel("Tilt (deg)");
     ylabel("Membership");
-    lt = legend();
     hold("on");
     plot(t, t_x, "LineWidth", 2);
     hold("off");
-    xlim([-90, 90]);
     ylim([0, 1]);
-
-    % add legends
-    nexttile(1, [1,  1]);
-    f.Children.Children(3).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
-    f.Children.Children(1).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
-
-    nexttile(2, [1,  1]);
-    f.Children.Children(3).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
-    f.Children.Children(1).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
-
-    hold(gca(f), "off");
 end

@sigmoidSensor/sensorPerformance.m

@@ -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, f); % 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

aerpaw/MESSAGE_TYPE.m

@@ -8,6 +8,5 @@ classdef MESSAGE_TYPE < uint8
         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)
-        TAKEOFF          (9)  % Server->Client: take off to 25 m, ACK then READY on arrival
     end
 end

aerpaw/client/uav_runner.py

@@ -44,7 +44,6 @@ class MessageType(IntEnum):
     GUIDANCE_TOGGLE  = 6
     REQUEST_POSITION = 7
     POSITION         = 8
-    TAKEOFF          = 9
 
 
 AERPAW_DIR = Path('/root/miSim/aerpaw')
@@ -165,7 +164,7 @@ class UAVRunner(BasicRunner):
 
         # Retry connection up to 10 times (~30 seconds total)
         reader, writer = None, None
-        for attempt in range(100):
+        for attempt in range(10):
             try:
                 reader, writer = await asyncio.wait_for(
                     asyncio.open_connection(self.server_ip, self.server_port),
@@ -185,11 +184,13 @@ class UAVRunner(BasicRunner):
         log_task = None
         nav_task = None
         try:
-            # Takeoff is now driven by a TAKEOFF command from the controller
-            # (staggered in time between UAVs), not performed at startup. Start
-            # GPS logging immediately so the ground/idle period is captured too.
+            # 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))
-            print("[UAV] Connected; waiting for commands...")
 
             # Command loop - handle all messages from controller
             waypoint_num = 0
@@ -212,27 +213,6 @@ class UAVRunner(BasicRunner):
                         await send_message_type(writer, MessageType.ACK)
                         print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
 
-                elif msg_type == MessageType.TAKEOFF:
-                    # aerpawlib only switches the vehicle to GUIDED inside
-                    # _initialize_postarm(), which await_ready_to_move() runs only
-                    # when the vehicle is DISARMED at the first movement command.
-                    # The safety pilot arms all UAVs at experiment start, so a UAV
-                    # whose takeoff is deliberately delayed (for time separation)
-                    # is already armed by now and would skip that path -> no GUIDED
-                    # switch -> simple_takeoff is ignored in ALT_HOLD and it never
-                    # leaves the ground. Run the post-arm init explicitly to
-                    # guarantee GUIDED mode (and home capture) before taking off;
-                    # takeoff()'s own gate then sees the vehicle armed and does not
-                    # re-initialize.
-                    await send_message_type(writer, MessageType.ACK)
-                    print("[UAV] Sent ACK (takeoff)")
-                    print("[UAV] Initializing (GUIDED) and taking off to 25 m...")
-                    await drone._initialize_postarm()
-                    await drone.takeoff(25)
-                    print("[UAV] Takeoff complete")
-                    await send_message_type(writer, MessageType.READY)
-                    print("[UAV] Sent READY")
-
                 elif msg_type == MessageType.REQUEST_POSITION:
                     # Respond immediately with current ENU position relative to origin
                     pos = drone.position
@@ -308,11 +288,6 @@ class UAVRunner(BasicRunner):
         except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
             print(f"[UAV] Error: {e}")
 
-        except Exception as e:
-            import traceback
-            print(f"[UAV] Unhandled exception: {type(e).__name__}: {e}")
-            traceback.print_exc()
-
         finally:
             if nav_task is not None and not nav_task.done():
                 nav_task.cancel()

aerpaw/config/client1.yaml

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

aerpaw/config/client2.yaml

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

aerpaw/config/scenario.csv

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

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, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_lock.csv

@@ -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, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_two_around_wall.csv

@@ -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, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1

aerpaw/controller.m

@@ -7,50 +7,34 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-% Three waypoints per UAV: stage (climb) → traverse (XY) → settle (descend)
-MAX_TARGETS = MAX_CLIENTS * 3;
-
-% Altitude-staggered flyout/flyback currently supports exactly 2 UAVs
-if numClients ~= int32(2)
-    error('Altitude-staggered flyout/flyback requires exactly 2 UAVs');
-end
-
-% Minimum vertical separation maintained between UAVs during XY transit (metres)
-TRANSIT_SEP = 30.0;
+MAX_TARGETS = MAX_CLIENTS;
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
-numWaypoints = int32(0); %#ok<NASGU>
-totalLoaded  = int32(0);
+numWaypoints = int32(0);
+totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
 
-% Experiment start positions from scenario CSV (N x 3)
-scenarioPositions = zeros(MAX_CLIENTS, 3);
-
-% Load experiment start positions from scenario CSV
+% 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
+    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
     totalLoaded = int32(size(posMatrix, 1));
-    scenarioPositions(1:totalLoaded, :) = posMatrix;
-    % MATLAB path: send directly to scenario positions in one waypoint
     targets(1:totalLoaded, :) = posMatrix;
     numWaypoints = int32(1);
     disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
 else
     coder.cinclude('controller_impl.h');
     filename = ['config/scenario.csv', char(0)];
-    % Load into targets temporarily; copy to scenarioPositions below
     totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
                 coder.ref(targets), int32(MAX_TARGETS));
-    scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :);
-    numWaypoints = int32(3);
+    numWaypoints = totalLoaded / int32(numClients);
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 65;
+NUM_SCENARIO_PARAMS = 45;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
@@ -84,126 +68,43 @@ for i = 1:numClients
     end
 end
 
-% Query takeoff-pad GPS positions before sending any waypoints.
-% These are also the flyback targets so each UAV lands where it took off.
-initialPositions = zeros(MAX_CLIENTS, 3);
-if ~coder.target('MATLAB')
-    coder.ceval('sendRequestPositions', int32(numClients));
-    coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS));
-else
-    % Simulation: treat scenario positions as the takeoff locations
-    initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
-end
+% 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, :);
 
-% ---- Build altitude-staggered flyout waypoints --------------------------------
-% Each UAV follows three phases:
-%   WP1: Climb to transit altitude at pad XY
-%   WP2: Traverse to final XY at transit altitude  (>= TRANSIT_SEP vertical gap)
-%   WP3: Descend to final altitude at final XY
-%
-% Transit altitude assignment: the UAV with the highest final altitude keeps
-% that altitude as its transit altitude; the other UAV climbs TRANSIT_SEP above
-% it, guaranteeing >= 30 m vertical separation during the simultaneous XY
-% traverse (WP2).  Both transit altitudes are well above minAlt.
-if ~coder.target('MATLAB')
-    if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
-        higherIdx = int32(1);
-        lowerIdx  = int32(2);
-    else
-        higherIdx = int32(2);
-        lowerIdx  = int32(1);
-    end
-
-    transitAlt = zeros(1, MAX_CLIENTS);
-    transitAlt(lowerIdx)  = scenarioPositions(lowerIdx, 3);
-    transitAlt(higherIdx) = scenarioPositions(lowerIdx, 3) + TRANSIT_SEP;
-
-    for iUAV = 1:numClients
-        base = double(iUAV - 1) * double(numWaypoints);
-        targets(base + 1, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), transitAlt(iUAV)];
-        targets(base + 2, :) = [scenarioPositions(iUAV,1), scenarioPositions(iUAV,2), transitAlt(iUAV)];
-        targets(base + 3, :) =  scenarioPositions(iUAV, :);
-    end
-end
-% -------------------------------------------------------------------------------
-
-% Waypoint loop
-if coder.target('MATLAB')
-    % Simulation: simple lockstep (collisions are not modelled here).
-    for w = 1:numWaypoints
-        for i = 1:numClients
-            targetIdx = (i - 1) * numWaypoints + w;
-            target = targets(targetIdx, :);
-            disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
+        if coder.target('MATLAB')
+            disp([datestr(now, 'HH:MM:SS'), ' 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
-        disp('Waiting for ACK from all clients...');
-        disp(['All UAVs arrived at waypoint ', num2str(w)]);
     end
-else
-    % ---- Staggered flyout -----------------------------------------------------
-    % Separate the two UAVs in TIME so they are never doing the same vertical
-    % climb at the same moment over their ~10 m-separated pads (which can trip
-    % the proximity guard on a spurious GPS sample). The UAV that transits
-    % highest leads by exactly one event.
-    %
-    % Events per UAV: 1 = TAKEOFF (25 m), 2 = WP1 (climb at pad), 3 = WP2
-    % (traverse), 4 = WP3 (descend). At each step the higher UAV does event
-    % `step` while the lower UAV does event `step-1`; both are dispatched, then
-    % we collect ACK and then READY (arrival) from each active UAV.
-    numEvents = double(numWaypoints) + 1;   % 3 waypoints + 1 takeoff
-    for step = 1:(numEvents + 1)
-        hEvent = step;
-        lEvent = step - 1;
-        hActive = (hEvent >= 1) && (hEvent <= numEvents);
-        lActive = (lEvent >= 1) && (lEvent <= numEvents);
 
-        % --- Dispatch this step's event to each active UAV ---
-        if hActive
-            if hEvent == 1
-                coder.ceval('sendMessageType', higherIdx, int32(MESSAGE_TYPE.TAKEOFF));
-            else
-                hRow = (double(higherIdx) - 1) * double(numWaypoints) + (hEvent - 1);
-                hTarget = targets(hRow, :);
-                coder.ceval('sendTarget', higherIdx, coder.ref(hTarget));
-            end
-        end
-        if lActive
-            if lEvent == 1
-                coder.ceval('sendMessageType', lowerIdx, int32(MESSAGE_TYPE.TAKEOFF));
-            else
-                lRow = (double(lowerIdx) - 1) * double(numWaypoints) + (lEvent - 1);
-                lTarget = targets(lRow, :);
-                coder.ceval('sendTarget', lowerIdx, coder.ref(lTarget));
-            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
 
-        % --- Collect ACK (command received) from each active UAV ---
-        if hActive
-            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.ACK));
-        end
-        if lActive
-            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.ACK));
-        end
-
-        % --- Collect READY (arrived at event) from each active UAV ---
-        if hActive
-            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.READY));
-        end
-        if lActive
-            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.READY));
-        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 ----------------------------------------
-% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2))
-% so the controller and the agent step-decay schedule stay in sync.
-if coder.target('MATLAB')
-    MAX_GUIDANCE_STEPS = int32(sc.maxIter);
-else
-    MAX_GUIDANCE_STEPS = int32(scenarioParams(2));
-end
+% 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')
@@ -216,18 +117,14 @@ 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 scenario positions so agents don't start at origin
-    positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
+    % 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);
@@ -238,7 +135,7 @@ for step = 1:MAX_GUIDANCE_STEPS
         if ~coder.target('MATLAB')
             coder.ceval('sendTarget', int32(i), coder.ref(target));
         else
-            disp(['[step ', num2str(step), '] target UAV ', num2str(i), ': ', num2str(target)]);
+            disp([datestr(now, 'HH:MM:SS'), ' [guidance] target UAV ', num2str(i), ': ', num2str(target)]);
         end
     end
 
@@ -267,81 +164,6 @@ if ~coder.target('MATLAB')
     % last guidance navigation and is back in sequential (ACK/READY) mode.
     coder.ceval('waitForAllMessageType', int32(numClients), ...
                 int32(MESSAGE_TYPE.ACK));
-    % Reset step counter so post-guidance logging carries no step prefix.
-    coder.ceval('setGuidanceStep', int32(0), int32(MAX_GUIDANCE_STEPS));
-end
-% --------------------------------------------------------------------------
-
-% ---- Altitude-staggered flyback -------------------------------------------
-% Mirror of flyout: stage (climb) → traverse (XY) → settle (descend to pad).
-% Transit altitudes are based on post-guidance positions so the UAV that ended
-% higher stays highest, guaranteeing >= TRANSIT_SEP vertical clearance during
-% the simultaneous XY traverse.
-NUM_RETURN_WP = int32(3);
-returnTargets = zeros(MAX_TARGETS, 3);
-
-if ~coder.target('MATLAB')
-    if positions(1, 3) >= positions(2, 3)
-        higherRetIdx = int32(1);
-        lowerRetIdx  = int32(2);
-    else
-        higherRetIdx = int32(2);
-        lowerRetIdx  = int32(1);
-    end
-
-    returnTransitAlt = zeros(1, MAX_CLIENTS);
-    returnTransitAlt(lowerRetIdx)  = positions(lowerRetIdx, 3);
-    returnTransitAlt(higherRetIdx) = positions(lowerRetIdx, 3) + TRANSIT_SEP;
-
-    for iUAV = 1:numClients
-        retBase = double(iUAV - 1) * double(NUM_RETURN_WP);
-        returnTargets(retBase + 1, :) = [positions(iUAV,1),         positions(iUAV,2),         returnTransitAlt(iUAV)];
-        returnTargets(retBase + 2, :) = [initialPositions(iUAV,1),  initialPositions(iUAV,2),  returnTransitAlt(iUAV)];
-        % Final return waypoint: pad XY from the pre-takeoff GPS query, but a fixed
-        % 25 m up. That query ran while the UAV was on the ground, so its ENU
-        % up-component is ~0 (often slightly negative) — using it directly commands
-        % a negative altitude. RTL/LAND perform the actual touchdown from here.
-        returnTargets(retBase + 3, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), 25.0];
-    end
-
-    % Staggered flyback: same higher-leads-by-one pattern as the flyout, but
-    % over the 3 return waypoints only (the UAVs are already airborne, so there
-    % is no TAKEOFF event). The higher UAV climbs/traverses/descends one event
-    % ahead of the lower, so they never share the same descent over their pads.
-    numReturnEvents = double(NUM_RETURN_WP);   % 3
-    for step = 1:(numReturnEvents + 1)
-        hEvent = step;
-        lEvent = step - 1;
-        hActive = (hEvent >= 1) && (hEvent <= numReturnEvents);
-        lActive = (lEvent >= 1) && (lEvent <= numReturnEvents);
-
-        if hActive
-            hRow = (double(higherRetIdx) - 1) * double(NUM_RETURN_WP) + hEvent;
-            hTarget = returnTargets(hRow, :);
-            coder.ceval('sendTarget', higherRetIdx, coder.ref(hTarget));
-        end
-        if lActive
-            lRow = (double(lowerRetIdx) - 1) * double(NUM_RETURN_WP) + lEvent;
-            lTarget = returnTargets(lRow, :);
-            coder.ceval('sendTarget', lowerRetIdx, coder.ref(lTarget));
-        end
-
-        if hActive
-            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.ACK));
-        end
-        if lActive
-            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.ACK));
-        end
-
-        if hActive
-            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.READY));
-        end
-        if lActive
-            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.READY));
-        end
-    end
-else
-    disp('Altitude-staggered return (simulation): UAVs commanded back to takeoff positions.');
 end
 % --------------------------------------------------------------------------
 

aerpaw/guidance_step.m

@@ -17,8 +17,7 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                       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; authoritative map in
-%                       aerpaw/impl/controller_impl.h):
+%                       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]
@@ -27,20 +26,9 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                         6  initialStepSize    29-32  betaTilt[1:4]
 %                         7  barrierGain        33-35  domainMin
 %                         8  barrierExponent    36-38  domainMax
-%                                               39     numObjectiveComponents
-%                                               40-43  objectivePos [x1,y1,x2,y2]
-%                                               44-51  objectiveVar [per component]
-%                                               52     sensorPerformanceMinimum
-%                                               53     useDoubleIntegrator
-%                                               54     dampingCoeff
-%                                               55     useFixedTopology
-%                                               56-59  txPower[1:4]
-%                                               60     useSinrComms
-%                                               61     sinrThreshold (dB)
-%                                               62     pathLossExponent
-%                                               63     ambientTemp (K)
-%                                               64     centerFreq (Hz)
-%                                               65     bandwidth (Hz)
+%                                               39-40  objectivePos
+%                                               41-44  objectiveVar (2x2, col-major)
+%                                               45     sensorPerformanceMinimum
 %   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
 %   obstacleMax       (MAX_OBSTACLES × 3) double
 %   numObstacles      (1,1) int32                 actual obstacle count
@@ -103,41 +91,26 @@ if isInit
         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));
-        TX_POWER_VEC                = scenarioParams(56:59); % per-UAV [1:MAX_CLIENTS] (W)
-        USE_SINR_COMMS              = logical(scenarioParams(60));
-        SINR_THRESHOLD              = scenarioParams(61);    % dB
-        PATH_LOSS_EXPONENT          = scenarioParams(62);
-        AMBIENT_TEMP                = scenarioParams(63);    % Kelvin
-        CENTER_FREQ                 = scenarioParams(64);    % Hz
-        BANDWIDTH                   = scenarioParams(65);    % Hz
+        OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
+        OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
+        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
 
         % --- 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) ---
+        % --- Build sensing objective (inline Gaussian; 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
+        dx = gridX - OBJECTIVE_GROUND_POS(1);
+        dy = gridY - OBJECTIVE_GROUND_POS(2);
+        % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv())
+        ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2);
+        ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2);
+        ov_det = ov_a * ov_d - ov_b * ov_c;
+        objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
         dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
             DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
 
@@ -157,7 +130,6 @@ if isInit
             ag = agent;
             ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
                                INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
-            ag.txPower = TX_POWER_VEC(ii); % SINR comms model transmit power (W)
             agentList{ii} = ag;
         end
 
@@ -174,9 +146,7 @@ if isInit
         % --- Initialise simulation (plots and video disabled) ---
         sim = miSim;
         sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
-                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                             USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY, false, ...
-                             USE_SINR_COMMS, SINR_THRESHOLD, PATH_LOSS_EXPONENT, AMBIENT_TEMP, CENTER_FREQ, BANDWIDTH);
+                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
     end
 
     % On the init call return current positions unchanged
@@ -205,15 +175,8 @@ else
     % 2. Advance timestep counter
     sim.timestepIndex = sim.timestepIndex + 1;
 
-    % 3. Update communications topology (Lesser Neighbour Assignment).
-    % Refresh the adjacency graph from current positions first so connectivity
-    % (fixed-radius or SINR) tracks the agents as they move; otherwise the graph
-    % would be frozen at init on the compiled path (the MATLAB sim refreshes it
-    % in @miSim/run.m). Skip when topology is fixed.
-    if ~sim.useFixedTopology
-        sim = sim.updateAdjacency();
-        sim = sim.lesserNeighbor();
-    end
+    % 3. Update communications topology (Lesser Neighbour Assignment)
+    sim = sim.lesserNeighbor();
 
     % 4. Compute Voronoi partitioning
     sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
@@ -221,8 +184,7 @@ else
     % 5. Unconstrained gradient-ascent step for each agent
     for ii = 1:size(sim.agents, 1)
         sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, ...
-                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
+                                             sim.timestepIndex, ii, sim.agents);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

aerpaw/impl/controller_impl.cpp

@@ -16,44 +16,6 @@
 
 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() {}
@@ -222,13 +184,9 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
 //   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)
+//   38-39: objectivePos (east, north)
+//   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
+//   44   : sensorPerformanceMinimum
 // Returns 1 on success, 0 on failure.
 int loadScenario(const char* filename, double* params) {
     char line[4096];
@@ -238,10 +196,10 @@ int loadScenario(const char* filename, double* params) {
     strncpy(copy, line, sizeof(copy) - 1);
     copy[sizeof(copy) - 1] = '\0';
 
-    char* fields[40];
-    int nf = splitCSVRow(copy, fields, 40);
-    if (nf < 26) {
-        fprintf(stderr, "loadScenario: expected >=26 columns, got %d\n", nf);
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 19) {
+        fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf);
         return 0;
     }
 
@@ -306,141 +264,34 @@ int loadScenario(const char* filename, double* params) {
         }
     }
 
-    // objectivePos: column 16 — 2 values per component (up to 2 components).
-    // Infer numObjectiveComponents from the number of values parsed.
+    // objectivePos: column 16
     {
         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");
+        if (sscanf(t, "%lf , %lf", &params[38], &params[39]) != 2) {
+            fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
             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).
+    // objectiveVar: column 17, format "v11, v12, v21, v22"
     {
-        char tmp[512]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char tmp[256]; 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);
+        if (sscanf(t, "%lf , %lf , %lf , %lf", &params[40], &params[41], &params[42], &params[43]) != 4) {
+            fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
             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));
+        params[44] = 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));
-    }
-
-    // ---- SINR communications model (columns 26–30; optional) ----
-    // Defaults mirror @miSim/initializeFromCsv.m so a CSV predating the SINR
-    // model still loads (useSinrComms stays 0 -> fixed-radius path is used).
-    //   txPower[1:4] -> params[55:58];  useSinrComms -> 59;
-    //   sinrThreshold (dB) -> 60;  pathLossExponent -> 61;  ambientTemp (K) -> 62;
-    //   centerFreq (Hz) -> 63;  bandwidth (Hz) -> 64.
-    params[55] = 0.1; params[56] = 0.1; params[57] = 0.1; params[58] = 0.1;
-    params[59] = 0.0;     // useSinrComms
-    params[60] = 0.0;     // sinrThreshold (dB)
-    params[61] = 2.0;     // pathLossExponent
-    params[62] = 290.0;   // ambientTemp (K)
-    params[63] = 2.4e9;   // centerFreq (Hz)
-    params[64] = 20e6;    // bandwidth (Hz)
-
-    // txPower: column 26 (per-UAV, up to MAX_CLIENTS_PER_PARAM values, zero-padded)
-    if (nf > 26) {
-        char tmp[256]; strncpy(tmp, fields[26], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        char* t = trimField(tmp);
-        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) {
-            for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) params[55 + k] = vals[k];
-        }
-    }
-
-    // useSinrComms: column 27
-    if (nf > 27) {
-        char tmp[64]; strncpy(tmp, fields[27], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[59] = atof(trimField(tmp));
-    }
-    // sinrThreshold: column 28 (dB)
-    if (nf > 28) {
-        char tmp[64]; strncpy(tmp, fields[28], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[60] = atof(trimField(tmp));
-    }
-    // pathLossExponent: column 29
-    if (nf > 29) {
-        char tmp[64]; strncpy(tmp, fields[29], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[61] = atof(trimField(tmp));
-    }
-    // ambientTemp: column 30 (K)
-    if (nf > 30) {
-        char tmp[64]; strncpy(tmp, fields[30], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[62] = atof(trimField(tmp));
-    }
-    // centerFreq: column 31 (Hz)
-    if (nf > 31) {
-        char tmp[64]; strncpy(tmp, fields[31], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[63] = atof(trimField(tmp));
-    }
-    // bandwidth: column 32 (Hz)
-    if (nf > 32) {
-        char tmp[64]; strncpy(tmp, fields[32], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[64] = 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]);
-    printf("Comms model: %s (SINR threshold %g dB, path-loss exp %g, T=%g K, f_c=%g Hz, BW=%g Hz)\n",
-           params[59] != 0.0 ? "SINR" : "fixed-radius", params[60], params[61], params[62], params[63], params[64]);
+    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
+           params[32], params[33], params[34], params[35], params[36], params[37]);
     return 1;
 }
 
@@ -575,27 +426,22 @@ static const char* messageTypeName(uint8_t msgType) {
         case 6: return "GUIDANCE_TOGGLE";
         case 7: return "REQUEST_POSITION";
         case 8: return "POSITION";
-        case 9: return "TAKEOFF";
         default: return "UNKNOWN";
     }
 }
 
-// Send a single-byte message type to a client (no logging)
-static int sendMessageTypeRaw(int clientId, int msgType) {
+// Send a single-byte message type to a client
+int sendMessageType(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";
+    std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
     return 1;
 }
 
@@ -614,7 +460,13 @@ int sendTarget(int clientId, const double* coords) {
         return 0;
     }
 
-    std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
+    // Timestamp
+    time_t now = time(nullptr);
+    struct tm* lt = localtime(&now);
+    char ts[16];
+    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
+
+    std::cout << ts << " Sent TARGET to client " << clientId << ": "
               << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     return 1;
 }
@@ -662,63 +514,31 @@ int waitForAllMessageType(int numClients, int expectedType) {
                     return 0;
                 }
 
+                std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
+
                 if (msgType == expected) {
                     completed[i] = true;
                     completedCount++;
-                } else {
-                    std::cerr << logPrefix() << "Unexpected " << messageTypeName(msgType)
-                              << " from client " << (i + 1)
-                              << " (expected " << messageTypeName(expected) << ")\n";
                 }
             }
         }
     }
 
-    std::cout << logPrefix() << "Received " << messageTypeName(expected) << " from all clients\n";
     return 1;
 }
 
-// Wait for a specific message type from a SINGLE client (blocking).
-// Used by the staggered flyout/flyback, where the controller waits on one UAV
-// while the other is still flying its own (offset) event. Skips past any
-// unexpected byte and returns 0 on disconnect/error, 1 on success.
-int waitForClientMessageType(int clientId, int expectedType) {
-    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-    int sock = clientSockets[clientId - 1];
-    uint8_t expected = (uint8_t)expectedType;
-
-    while (true) {
-        uint8_t msg;
-        int len = recv(sock, &msg, 1, MSG_WAITALL);
-        if (len <= 0) {
-            std::cerr << logPrefix() << "waitForClientMessageType: client " << clientId
-                      << " disconnected while waiting for " << messageTypeName(expected) << "\n";
-            return 0;
-        }
-        if (msg == expected) {
-            std::cout << logPrefix() << "Received " << messageTypeName(expected)
-                      << " from client " << clientId << "\n";
-            return 1;
-        }
-        std::cerr << logPrefix() << "Unexpected " << messageTypeName(msg) << " from client "
-                  << clientId << " (expected " << messageTypeName(expected) << ")\n";
-    }
-}
-
 // Broadcast GUIDANCE_TOGGLE to all clients
 void sendGuidanceToggle(int numClients) {
     for (int i = 1; i <= numClients; i++) {
-        sendMessageTypeRaw(i, 6);  // GUIDANCE_TOGGLE = 6
+        sendMessageType(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
+        if (!sendMessageType(i, 7)) return 0;  // REQUEST_POSITION = 7
     }
-    std::cout << logPrefix() << "Sent REQUEST_POSITION to clients\n";
     return 1;
 }
 
@@ -753,7 +573,7 @@ int recvPositions(int numClients, double* positions, int maxClients) {
         positions[i + 1 * maxClients] = coords[1];  // north (y)
         positions[i + 2 * maxClients] = coords[2];  // up    (z)
 
-        std::cout << logPrefix() << "Position from client " << (i + 1) << ": "
+        std::cout << "Position from client " << (i + 1) << ": "
                   << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     }
     return 1;

aerpaw/impl/controller_impl.h

@@ -27,21 +27,10 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   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)
-//   55-58 txPower[1:4]         (per-UAV transmit power, Watts; SINR comms model)
-//   59    useSinrComms         (0=fixed-radius comms, 1=SINR/path-loss comms)
-//   60    sinrThreshold        (SINR connectivity/CBF threshold, dB)
-//   61    pathLossExponent     (path-loss exponent n)
-//   62    ambientTemp          (ambient temperature, K; thermal noise)
-//   63    centerFreq           (carrier frequency, Hz; path-loss offset)
-//   64    bandwidth            (channel bandwidth, Hz; thermal noise)
-#define NUM_SCENARIO_PARAMS 65
+//   38-39 objectivePos
+//   40-43 objectiveVar (2x2 col-major)
+//   44    sensorPerformanceMinimum
+#define NUM_SCENARIO_PARAMS 45
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8
@@ -68,12 +57,8 @@ int loadObstacles(const char* filename,
 int sendMessageType(int clientId, int msgType);
 int sendTarget(int clientId, const double* coords);
 int waitForAllMessageType(int numClients, int expectedType);
-// Block until a SINGLE client sends expectedType (used by the staggered
-// flyout/flyback to wait on one UAV while the other is mid-flight).
-int waitForClientMessageType(int clientId, int expectedType);
 
 // Guidance loop operations
-void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration
 void sendGuidanceToggle(int numClients);
 int  sendRequestPositions(int numClients);
 int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3

aerpaw/radio/updateScripts.sh

@@ -13,4 +13,3 @@ 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,8 +1,7 @@
-function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
+function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
     arguments (Input)
         logDirs (1, 1) string;
         seaToGroundLevel (1, 1) double = 110; % measured approximately from USGS national map viewer for the AERPAW test field
-        plotWholeFlight (1, 1) logical = false;
     end
     arguments (Output)
         f (1, 1) matlab.ui.Figure;
@@ -29,7 +28,6 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
     logDirs = dir(logDirs);
     logDirs = logDirs(3:end);
     logDirs = logDirs([logDirs.isdir] == 1);
-    logDirs = logDirs(~startsWith({logDirs.name}, "controller_"));
 
     G = cell(size(logDirs));
     for ii = 1:size(logDirs, 1)
@@ -50,10 +48,8 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
         stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last");
     
         % % Plot whole flight, including setup/cleanup
-        if plotWholeFlight
-            startIdx = 1;
-            stopIdx = length(verticalSpeed);
-        end
+        % startIdx = 1;
+        % stopIdx = length(verticalSpeed);
 
         % Convert LLA trajectory data to ENU for external analysis
         % NaN out entries outside the algorithm flight range so they don't plot
@@ -62,36 +58,9 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
         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));
-            figure;
-            plot(G{1}.Timestamp(1:length(d)), d);
-            xlabel("Time"); ylabel("Distance"); title("UAV0 and UAV1 distance"); grid("on");
-            fprintf("Average distance between agents %d and %d is %2.3f\n", ii, jj, mean(d));
-            fprintf("Median distance between agents %d and %d is %2.3f\n", ii, jj, median(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
-    fprintf("Flight duration is %s\n", string(G{1}.Timestamp(end) - G{1}.Timestamp(1), 'mm:ss'));
     
     % Plot domain
     altOffset = 1; % to avoid clipping into the ground when displayed
@@ -108,14 +77,9 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
     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
-    if length(params.objectivePos) > 2
-        objectivePos = reshape(params.objectivePos, [2, length(params.objectivePos) / 2])';
-    end
-    objectivePos = [objectivePos, zeros(size(objectivePos, 1), 1)];
+    objectivePos = [params.objectivePos, 0];
     llaObj = enu2lla(objectivePos, lla0, "flat");
-    for ii = 1:size(llaObj, 1)
-        geoplot3(gf, [llaObj(ii, 1), llaObj(ii, 1)], [llaObj(ii, 2), llaObj(ii, 2)], [llaObj(ii, 3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
-    end
+    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

aerpaw/results/plotRadioLogs.m

@@ -1,12 +1,9 @@
-function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
+function [f, R] = plotRadioLogs(resultsPath)
     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
 
@@ -43,44 +40,11 @@ function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
 
     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');
+    tl = tiledlayout(numel(metricNames), 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
+    for mi = 1:numel(metricNames)
         ax = nexttile(tl);
         hold(ax, 'on');
         grid(ax, 'on');
@@ -93,32 +57,23 @@ function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
             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)
+                % Skip if all NaN for this metric
+                if all(isnan(vals))
                     continue;
                 end
 
                 si = mod(ci - 1, numel(styles)) + 1;
                 plot(ax, rows.Timestamp, vals, styles(si), ...
-                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
+                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 1);
                 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
+        if mi == numel(metricNames)
             xlabel(ax, 'Time');
         end
         legend(ax, legendEntries, 'Location', 'best');
@@ -126,134 +81,4 @@ function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
     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/readRadioLogs.m

@@ -2,6 +2,7 @@ function R = readRadioLogs(logPath)
     arguments (Input)
         logPath (1, 1) string {isfolder(logPath)};
     end
+
     arguments (Output)
         R (:, 8) table;
     end
@@ -30,26 +31,14 @@ function R = readRadioLogs(logPath)
         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);
+        % Skip 3 lines: 2 junk (tail errors) + 1 header (tx_uav_id,value)
+        for k = 1:3
+            fgetl(fid);
         end
         data = textscan(fid, '[%26c] %d,%f');
         fclose(fid);
 
-        ts = datetime(cellstr(data{1}), 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
+        ts = datetime(data{1}, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
         txId = int32(data{2});
         val = data{3};
 
@@ -70,40 +59,6 @@ function R = readRadioLogs(logPath)
 
     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, :) = [];
 

aerpaw/results/resultsAnalysis.m

@@ -1,16 +1,12 @@
 %% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "columns_simulated"); % Define path to results copied from AERPAW platform
-
-% Check timeline in controller logs
-controller = controllerAnalysis(resultsPath);
+resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
 
 % Plot GPS logged data and scenario information (domain, objective, obstacles)
 seaToGroundLevel = 110; % measured approximately from USGS national map viewer
-plotWholeFlight = true; % do not attempt to automatically trim initial and final positioning and landing from flight plot (buggy)
-[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel, true);
+[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel);
 
-% Plot radio statistics (time-based and distance-based)
-[fRadio, fRadioDist, R] = plotRadioLogs(resultsPath, G, controller.timestamp([1, end]));
+% Plot radio statistics
+[fRadio, R] = plotRadioLogs(resultsPath);
 
 %% Run simulation
 % Run miSim using same AERPAW scenario definition CSV
@@ -25,14 +21,7 @@ makeVideo = true;
 % Define scenario according to CSV specification
 domain = rectangularPrism;
 domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
-if length(params.objectiveVar) > 4 && length(params.objectivePos) > 2
-    objectiveSigma = permute(reshape(params.objectiveVar, [length(params.objectiveVar)/4 2 2]), [3 1 2]);
-    objectivePos = reshape(params.objectivePos, [length(params.objectivePos)/2, 2])';
-else
-    objectiveSigma = reshape(params.objectiveVar, [1, 2, 2]);
-    objectivePos = params.objectivePos;
-end
-domain.objective = domain.objective.initialize(objectiveFunctionWrapper(objectivePos, objectiveSigma), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
+domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
 
 agents = cell(size(params.initialPositions, 2) / 3, 1);
 for ii = 1:size(agents, 1)
@@ -44,10 +33,7 @@ for ii = 1:size(agents, 1)
     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);
-    if isfield(params, 'txPower')
-        agents{ii}.txPower = params.txPower(ii); % SINR comms model transmit power (W)
-    end
+    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), plotCommsGeometry);
 end
 
 % Create obstacles
@@ -57,22 +43,9 @@ for ii = 1:size(obstacles, 1)
     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
 
-% SINR communications model parameters (optional CSV columns; default to the
-% fixed-radius path so existing scenarios are unaffected)
-if isfield(params, 'useSinrComms');     useSinrComms = logical(params.useSinrComms); else; useSinrComms = false; end
-if isfield(params, 'sinrThreshold');    sinrThreshold = params.sinrThreshold;        else; sinrThreshold = 0;        end
-if isfield(params, 'pathLossExponent'); pathLossExponent = params.pathLossExponent;  else; pathLossExponent = 2.0;    end
-if isfield(params, 'ambientTemp');      ambientTemp = params.ambientTemp;            else; ambientTemp = 290.0;      end
-if isfield(params, 'centerFreq');       centerFreq = params.centerFreq;              else; centerFreq = 2.4e9;       end
-if isfield(params, 'bandwidth');        bandwidth = params.bandwidth;                else; bandwidth = 20e6;         end
-
-% Set up simulation. The four args after makeVideo (useDoubleIntegrator,
-% dampingCoeff, useFixedTopology, optimizeSensorPointing) are kept at their
-% prior effective defaults to preserve existing analysis behavior; only the
-% SINR comms parameters are threaded through from the scenario.
+% Set up simulation
 sim = miSim;
-sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo, ...
-                     false, 2.0, false, false, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);
+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();
@@ -80,9 +53,6 @@ sim.writeInits();
 % Run
 sim = sim.run();
 
-% Save results
-sim = sim.teardown();
-
 %% Plot AERPAW trajectory logs onto simulated result for comparison
 % Duplicate plot to overlay with logged trajectories
 comparison = figure;
@@ -90,12 +60,9 @@ 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);
+        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East, G{ii}.North, G{ii}.Up + 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

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")};
     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

@@ -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);

plots_1_2.m

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

plots_3_4.m

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

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

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

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8d.xml

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

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8p.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/BsVG5tuGBaruV5jvy4e3yG1jm4Qp.xml

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

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

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

resources/project/Hp7WImb16Wc_c7gK28oUydAFVts/EhSD3dO3mU2KP9IqbTVz7g60j5Ad.xml

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

resources/project/Hp7WImb16Wc_c7gK28oUydAFVts/EhSD3dO3mU2KP9IqbTVz7g60j5Ap.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/7wWFVcqPEpBA2edOtCoXYRUzhTAp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnop.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/bflJ_m_JEPly6uLay0BQMV_1uAUd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/bflJ_m_JEPly6uLay0BQMV_1uAUp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/ugpKFhW2eBP_pSSkV0NyOiJaMEod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/vKw7EZP5Lkw7wHPSRNjXYJUWEOId.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/vKw7EZP5Lkw7wHPSRNjXYJUWEOIp.xml

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

resources/project/QVXvsR5cXQzGBi9QgRUFaflegRY/SltMFP7zr3UZsLPdRi9OyWAN7pMd.xml

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