Merge branch 'main' into AERPAW-experiments

@agent/agent.m

@@ -32,7 +32,9 @@ classdef agent
 
     properties (SetAccess = private, GetAccess = public)
         initialStepSize = NaN;
+        initialMaxAngleStepSize = NaN;
         stepDecayRate = NaN;
+        angleStepDecayRate = NaN;
     end
 
     methods (Access = public)
@@ -48,8 +50,8 @@ classdef agent
             obj.commsGeometry = spherical;
         end
         [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [obj] = run(obj, domain, partitioning, t, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents);
-        [partitioning] = partition(obj, agents, objective)
+        [partitioning, agents] = 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, label, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
@@ -7,6 +7,7 @@ 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;
     end
@@ -23,7 +24,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     obj.label = label;
     obj.plotCommsGeometry = plotCommsGeometry;
     obj.initialStepSize = initialStepSize;
+    obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
     obj.stepDecayRate = obj.initialStepSize / maxIter;
+    obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
 
     % Initialize performance vector
     if coder.target('MATLAB')
@@ -35,5 +38,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.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    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);
 end

@agent/partition.m

@@ -1,4 +1,4 @@
-function [partitioning] = partition(obj, agents, objective)
+function [partitioning, agents] = partition(obj, agents, objective)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         agents (:, 1) {mustBeA(agents, "cell")};
@@ -6,6 +6,7 @@ function [partitioning] = partition(obj, agents, objective)
     end
     arguments (Output)
         partitioning (:, :) double;
+        agents (:, 1) cell;
     end
 
     nAgents = size(agents, 1);
@@ -18,8 +19,22 @@ function [partitioning] = partition(obj, agents, objective)
     % minimum threshold that must be exceeded for any assignment.
     agentPerf = zeros(nPoints, nAgents + 1);
     for aa = 1:nAgents
-        p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+        if isa(agents{aa}.sensorModel, "sigmoidSensor")
-            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
+            p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+                [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
+        elseif isa(agents{aa}.sensorModel, "rfSensor")
+            otherSensorsIdx = [1:(aa - 1), (aa + 1):size(agents, 1)];
+            otherSensors = agents(otherSensorsIdx);
+            otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherSensors, "UniformOutput", false));
+            otherSensors = cellfun(@(x) x.sensorModel, otherSensors, "UniformOutput", false);
+            [p, ~, agents{aa}.sensorModel, otherSensors] = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+                [objective.X(:), objective.Y(:), zeros(nPoints, 1)], otherSensorsPos, otherSensors);
+            for k = 1:numel(otherSensorsIdx)
+                agents{otherSensorsIdx(k)}.sensorModel = otherSensors{k};
+            end
+        else
+            error("?");
+        end
         agentPerf(:, aa) = p(:);
     end
     agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;

@agent/run.m

@@ -1,14 +1,15 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
+function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents)
     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")};
@@ -33,33 +34,62 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
     maskedX = domain.objective.X(partitionMask);
     maskedY = domain.objective.Y(partitionMask);
 
-    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    if isa(obj.sensorModel, "rfSensor")
-    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+        % Extract other agents' sensor models and positions once, outside the delta loop.
-    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
+        % Mask the full-grid RSS caches (filled by partition()) down to this agent's
-    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+        % partition subset so sensorPerformance can reuse them for all perturbations.
-    for ii = 1:7
+        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
         % Apply delta to position
-        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+        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
         
         % Compute performance values on partition
-        if ii < 6
+        if isa(obj.sensorModel, "sigmoidSensor")
-            % Compute sensing performance
             sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-            % Objective performance does not change for 0, +/- X, +/- Y steps.
+        elseif isa(obj.sensorModel, "rfSensor")
-            % Those values are computed once before the loop and are only
+            if ii == 1
-            % recomputed when +/- Z steps are applied
+                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);
         else
-            % Redo partitioning for Z stepping only
+            error("?");
-            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
@@ -73,37 +103,54 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
         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*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
+    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
 
     % Compute scaling factor
-    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+    targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
-    gradNorm = norm(gradC);
+    gradPosNorm = norm(gradC(1:3));
 
     % Compute unconstrained next state
     if useDoubleIntegrator
         % Double-integrator: gradient produces desired acceleration with damping
-        if gradNorm < 1e-100
+        if gradPosNorm < 1e-100
-            a_gradient = zeros(1, 3);
+            a_gradient = zeros(1, 5);
         else
             % Scale so steady-state step ≈ targetRate (matching SI behavior)
-            a_gradient = (targetRate * dampingCoeff / (gradNorm * dt)) * gradC;
+            a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
         end
         % Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
-        obj.vel = (obj.vel + a_gradient * dt) / (1 + dampingCoeff * dt);
+        obj.vel = (obj.vel + a_gradient(1:3) * dt) / (1 + dampingCoeff * dt);
         obj.pos = obj.lastPos + obj.vel * dt;
     else
         % Single-integrator: gradient directly sets position step
-        if gradNorm >= 1e-100
+        if gradPosNorm >= 1e-100
-            obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
+            obj.pos = obj.pos + (targetPosRate / gradPosNorm) * gradC(1:3);
         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,45 +7,68 @@ function updatePlots(obj)
 
     % Find change in agent position since last timestep
     deltaPos = obj.pos - obj.lastPos;
-    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+    posChanged    = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
-        % Agent did not move, so nothing has to move on the plots
+    orientChanged = obj.sensorModel.tilt    ~= obj.fovGeometry.tilt || ...
+                    obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
+
+    if ~posChanged && ~orientChanged
         return;
     end
 
-    % Scatterplot point positions
+    if posChanged
-    for ii = 1:size(obj.scatterPoints, 1)
+        % Scatterplot point positions
-        obj.scatterPoints(ii).XData = obj.pos(1);
+        for ii = 1:size(obj.scatterPoints, 1)
-        obj.scatterPoints(ii).YData = obj.pos(2);
+            obj.scatterPoints(ii).XData = obj.pos(1);
-        obj.scatterPoints(ii).ZData = obj.pos(3);
+            obj.scatterPoints(ii).YData = obj.pos(2);
-    end
+            obj.scatterPoints(ii).ZData = obj.pos(3);
-
-    % 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
+        % Collision geometry edges
-    if obj.plotCommsGeometry
+        for jj = 1:size(obj.collisionGeometry.lines, 2)
-        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
 
-    % Update FOV geometry surfaces
+    % FOV cone: recompute full mesh whenever position or orientation changes
-    for jj = 1:size(obj.fovGeometry.surface, 2)
+    if ~isempty(obj.fovGeometry.surface)
-        % Update each plot
+        % Sync fovGeometry state to current agent position and sensor orientation
-        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
+        obj.fovGeometry = obj.fovGeometry.initialize( ...
-        obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
+            obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
-        obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
+            obj.fovGeometry.tag, obj.fovGeometry.label, ...
-        obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
+            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
     end
-end
+end

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
@@ -14,6 +14,7 @@ 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;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "miSim")};
@@ -95,6 +96,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.useDoubleIntegrator = useDoubleIntegrator;
     obj.dampingCoeff = dampingCoeff;
     obj.useFixedTopology = useFixedTopology;
+    obj.optimizeSensorPointing = optimizeSensorPointing;
 
     % Compute adjacency matrix and network topology
     obj = obj.updateAdjacency();

@miSim/miSim.m

@@ -20,6 +20,7 @@ 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
         artifactName = "";
         f; % main plotting tiled layout figure
         fPerf; % performance plot figure

@miSim/run.m

@@ -10,11 +10,13 @@ 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
-            % Write initialization state frame in to video
+            % due to figure reflow during plot updates (e.g. in headless mode).
-            I = getframe(obj.f);
+            I_ref = getframe(obj.f);
-            v.writeVideo(I);
+            v.writeVideo(I_ref);
+            videoFrameSize = [size(I_ref.cdata, 2), size(I_ref.cdata, 1)];
         end
     end
 
@@ -29,9 +31,16 @@ 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{1}.partition(obj.agents, obj.domain.objective);
+        [obj.partitioning, obj.agents] = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
         % Determine desired communications links
         if ~obj.useFixedTopology
@@ -46,7 +55,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.agents, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep);
+            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)]));
         end
 
         % Adjust motion determined by unconstrained gradient ascent using
@@ -70,6 +79,9 @@ 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/writeInits.m

@@ -13,10 +13,39 @@ function writeInits(obj)
 
     % Collect agent parameters
     collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
-    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    if isprop(obj.agents{1}.sensorModel, "alphaDist")
-    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+        % sigmoidSensor parameters
-    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+        alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
-    betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents);
+        betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+        alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+        betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents);
+
+        % others to zero
+        lossExponent = zeros(size(obj.agents));
+        P_TX = zeros(size(obj.agents));
+        BW = zeros(size(obj.agents));
+        f_c = zeros(size(obj.agents));
+        G_RX_dBi = zeros(size(obj.agents));
+        beamwidthExponent = zeros(size(obj.agents));
+        
+    elseif isprop(obj.agents{1}.sensorModel, "P_TX")
+        % rfSensor parameters
+        lossExponent = cellfun(@(x) x.sensorModel.lossExponent, obj.agents);
+        P_TX = cellfun(@(x) x.sensorModel.P_TX, obj.agents);
+        BW = cellfun(@(x) x.sensorModel.BW, obj.agents);
+        f_c = cellfun(@(x) x.sensorModel.f_c, obj.agents);
+        G_RX_dBi = cellfun(@(x) x.sensorModel.G_RX_dBi, obj.agents);
+        beamwidthExponent = cellfun(@(x) x.sensorModel.beamwidthExponent, obj.agents);
+
+        % others to zero
+        alphaDist = zeros(size(obj.agents));
+        betaDist = zeros(size(obj.agents));
+        alphaTilt = zeros(size(obj.agents));
+        betaTilt = zeros(size(obj.agents));
+    end
+    % joint parameters
+    tilt = cellfun(@(x) x.sensorModel.tilt, obj.agents);
+    azimuth = cellfun(@(x) x.sensorModel.azimuth, obj.agents);
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
@@ -30,7 +59,9 @@ function writeInits(obj)
                     "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ...
                     "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
                     "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
-                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    "tilt", tilt, "azimuth", azimuth, ... % joint sensor parameters
+                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... % sigmoid sensor parameters
+                    "lossExponent", lossExponent, "P_TX", P_TX, "BW", BW, "f_c", f_c, "G_RX_dBi", G_RX_dBi, "beamwidthExponent", beamwidthExponent, ... % RF sensor parameters
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
                     "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
                     "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...

@rfSensor/RSS.m

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

@rfSensor/clearRssCache.m

@@ -0,0 +1,11 @@
+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

@@ -0,0 +1,6 @@
+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

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

@rfSensor/initialize.m

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

@rfSensor/pathLoss.m

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

@rfSensor/plot.m

@@ -0,0 +1,125 @@
+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

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

@rfSensor/plotPerformance.m

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

@rfSensor/rfSensor.m

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

@rfSensor/sensorPerformance.m

@@ -0,0 +1,34 @@
+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

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

@sensingObjective/plot.m

@@ -11,19 +11,26 @@ 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)
-        hold(f.CurrentAxes, "on");
+        ax = f.CurrentAxes;
-        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        hold(ax, "on");
+        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        hold(f.CurrentAxes, "off");
+        clim(ax, cRange);
+        hold(ax, "off");
     else
-        hold(f.Children(1).Children(ind(1)), "on");
+        ax = f.Children(1).Children(ind(1));
-        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        hold(ax, "on");
+        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        hold(f.Children(1).Children(ind(1)), "off");
+        clim(ax, cRange);
+        hold(ax, "off");
     end
 
     % Add to other perspectives

@sigmoidSensor/halfAngle.m

@@ -0,0 +1,9 @@
+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,17 +1,24 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth)
     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/sensorPerformance.m

@@ -8,16 +8,20 @@ function value = sensorPerformance(obj, agentPos, targetPos)
         value (:, 1) double;
     end
 
-    % compute direct distance and distance projected onto the ground
+    % Unit vectors from agent to each target
-    d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
+    diffs = targetPos - agentPos;
-    x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
+    d = vecnorm(diffs, 2, 2);
+    dirs = diffs ./ d;
 
-    % compute tilt angle
+    % Boresight unit vector: tilt=0 → nadir [0,0,-1]; azimuth 0=+Y, 90=+X clockwise
-    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
+    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');
 
     % Membership functions
     mu_d = obj.distanceMembership(d);
-    mu_t = obj.tiltMembership(tiltAngle);
+    mu_t = obj.tiltMembership(angularOffset);
 
     value = mu_d .* mu_t; % assume pan membership is always 1
 end

@sigmoidSensor/sigmoidSensor.m

@@ -6,14 +6,20 @@ 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);
+        [obj]   = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth); % initialize sensor, define parameters
-        [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
+        [value] = sensorPerformance(obj, agentPos, targetPos); % determine sensor performance for a given single sensor and target geometry
-        [f] = plotParameters(obj);
+        [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
     end
     methods (Access = private)
-        x = distanceMembership(obj, d);
+        x = distanceMembership(obj, d); % used in computing distance factor of sensor performance
-        x = tiltMembership(obj, t);
+        x = tiltMembership(obj, t); % used in computing tilt factor of sensor performance
     end
 end

aerpaw/config/scenario_columns.csv

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

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 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
+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/guidance_step.m

@@ -198,8 +198,8 @@ else
     % 5. Unconstrained gradient-ascent step for each agent
     for ii = 1:size(sim.agents, 1)
         sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, sim.agents, ...
+                                             sim.timestepIndex, ii, ...
-                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep);
+                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

aerpaw/results/controllerAnalysis.m

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

@@ -1,9 +1,12 @@
-function [f, R] = plotRadioLogs(resultsPath)
+function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
     arguments (Input)
         resultsPath (1, 1) string;
+        G cell = {};
+        tLim (1, 2) datetime = [datetime(-Inf, 'ConvertFrom', 'datenum'), datetime(Inf, 'ConvertFrom', 'datenum')];
     end
     arguments (Output)
         f (1, 1) matlab.ui.Figure;
+        fDist (1, 1) matlab.ui.Figure;
         R cell;
     end
 
@@ -40,11 +43,44 @@ function [f, R] = plotRadioLogs(resultsPath)
 
     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(numel(metricNames), 1, 'TileSpacing', 'compact', 'Padding', 'compact');
+    tl = tiledlayout(nMetrics + 1, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
 
-    for mi = 1:numel(metricNames)
+    % Distance vs time tile (first)
+    ax = nexttile(tl);
+    hold(ax, 'on'); grid(ax, 'on');
+    legendEntries = string.empty;
+    ci = 1;
+    if ~isempty(G)
+        for rxIdx = 1:nUAV
+            tbl = R{rxIdx};
+            txIDs = unique(tbl.TxUAVID);
+            for ti = 1:numel(txIDs)
+                txID = txIDs(ti);
+                rows = tbl(tbl.TxUAVID == txID, :);
+                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
+                if isempty(rows), continue; end
+                [~, ia] = unique(rows.Timestamp);
+                [radioPt, dist] = pairDist(rows(ia, :), G);
+                if isempty(dist) || all(isnan(dist)), continue; end
+                valid = ~isnan(dist);
+                si = mod(ci - 1, numel(styles)) + 1;
+                plot(ax, datetime(radioPt(valid), 'ConvertFrom', 'posixtime'), dist(valid), ...
+                    styles(si), 'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
+                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
+                ci = ci + 1;
+            end
+        end
+    end
+    ylabel(ax, 'Distance (m)');
+    xlabel(ax, 'Time');
+    legend(ax, legendEntries, 'Location', 'best');
+    hold(ax, 'off');
+
+    for mi = 1:nMetrics
         ax = nexttile(tl);
         hold(ax, 'on');
         grid(ax, 'on');
@@ -57,23 +93,32 @@ function [f, R] = plotRadioLogs(resultsPath)
             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);
 
-                % Skip if all NaN for this metric
+                if isempty(rows)
-                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', 1);
+                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
                 legendEntries(end+1) = sprintf("TX %d → RX %d", txID, tbl.RxUAVID(1)); %#ok<AGROW>
+
+                % Median per 1/3-second time bin
+                [t_med, v_med] = timeBinMedian(posixtime(rows.Timestamp), vals, 1/3);
+                plot(ax, datetime(t_med, 'ConvertFrom', 'posixtime'), v_med, '-', ...
+                    'Color', 'r', 'LineWidth', 2);
+                legendEntries(end+1) = sprintf("TX %d → RX %d (median)", txID, tbl.RxUAVID(1)); %#ok<AGROW>
                 ci = ci + 1;
             end
         end
 
         ylabel(ax, yLabels(mi));
-        if mi == numel(metricNames)
+        if mi == nMetrics
             xlabel(ax, 'Time');
         end
         legend(ax, legendEntries, 'Location', 'best');
@@ -81,4 +126,134 @@ function [f, R] = plotRadioLogs(resultsPath)
     end
 
     title(tl, 'Radio Channel Metrics');
-end
+
+    % --- 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/readRadioLogs.m

@@ -70,6 +70,40 @@ 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,33 +1,16 @@
 %% Plot AERPAW logs (trajectory, radio)
 resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
 
-% Measure intervals between issuing commands from the controller 
+% Check timeline in controller logs
-% (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
+controller = controllerAnalysis(resultsPath);
-r = dir(resultsPath);
-controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
-controllerPath = dir(controllerPath);
-controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
-controller = readControllerLogs(controllerPath);
-rpIdx = startsWith(controller.message, "Iteration duration: ");
-s = split(controller.message(rpIdx), "Iteration duration: ");
-s = split(s(:, 2), ' s');
-s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
-s.Format = "mm:ss.SSS";
-fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
-fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
-fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
-fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
-if seconds(min(s)) < 4 
-    warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
-end
 
 % Plot GPS logged data and scenario information (domain, objective, obstacles)
 seaToGroundLevel = 110; % measured approximately from USGS national map viewer
 plotWholeFlight = true; % do not attempt to automatically trim initial and final positioning and landing from flight plot (buggy)
 [fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel, true);
 
-% Plot radio statistics
+% Plot radio statistics (time-based and distance-based)
-[fRadio, R] = plotRadioLogs(resultsPath);
+[fRadio, fRadioDist, R] = plotRadioLogs(resultsPath, G, controller.timestamp([1, end]));
 
 %% Run simulation
 % Run miSim using same AERPAW scenario definition CSV
@@ -54,7 +37,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, sprintf("Agent %d", ii), plotCommsGeometry);
+    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, 5.0, sprintf("Agent %d", ii), plotCommsGeometry);
 end
 
 % Create obstacles
@@ -81,9 +64,12 @@ 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, G{ii}.North, G{ii}.Up + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
+        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East(inRange), G{ii}.North(inRange), G{ii}.Up(inRange) + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
         hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "off");
     end
 end

aerpaw/results/timeBinMedian.m

@@ -0,0 +1,29 @@
+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,9 +6,11 @@ classdef cone
         label = "";
 
         % Spatial
-        center = NaN;
+        center  = NaN;
-        radius = NaN;
+        radius  = NaN;
-        height = NaN;
+        height  = NaN;
+        tilt    = 0;   % degrees, 0=nadir 90=horizon
+        azimuth = 0;   % degrees, 0=+Y 90=+X clockwise
 
         % Plotting
         surface;

geometries/@cone/initialize.m

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

geometries/@cone/plot.m

@@ -20,7 +20,18 @@ 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);

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/BsVG5tuGBaruV5jvy4e3yG1jm4Qd.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/BsVG5tuGBaruV5jvy4e3yG1jm4Qp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/3zY8iV4IlWXtpru_5KSo-mgBjoEd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/3zY8iV4IlWXtpru_5KSo-mgBjoEp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/7wWFVcqPEpBA2edOtCoXYRUzhTAd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/7wWFVcqPEpBA2edOtCoXYRUzhTAp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnop.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/bflJ_m_JEPly6uLay0BQMV_1uAUd.xml

@@ -0,0 +1,6 @@
+<?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

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/jvRgOU4zI43Zz6soLWzgDAjnvmod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/jvRgOU4zI43Zz6soLWzgDAjnvmop.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/ugpKFhW2eBP_pSSkV0NyOiJaMEod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/ugpKFhW2eBP_pSSkV0NyOiJaMEop.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/vKw7EZP5Lkw7wHPSRNjXYJUWEOId.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/vKw7EZP5Lkw7wHPSRNjXYJUWEOIp.xml

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

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkd.xml

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

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkp.xml

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

resources/project/o6vRLbVRBKxltajotIPvuRfoO58/gP3qjg8gQH7Si3Dbi4abPbO-xfwd.xml

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

resources/project/o6vRLbVRBKxltajotIPvuRfoO58/gP3qjg8gQH7Si3Dbi4abPbO-xfwp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/MoO8eFA7MNcwhsnIRzm_IDZfmpgd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/MoO8eFA7MNcwhsnIRzm_IDZfmpgp.xml

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

test/parametricTestSuite.m

@@ -53,7 +53,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
                 collisionGeometry = spherical;
                 collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
 
-                agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), tc.plotCommsGeometry);
+                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), tc.plotCommsGeometry);
             end
 
             % Create obstacles
@@ -112,7 +112,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
                 
                 % Initialize agent
                 collisionGeometry = collisionGeometry.initialize(agentPos, params.collisionRadius(ii, 1), REGION_TYPE.COLLISION, "Agent 1 Collision Region");
-                agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, 1), params.maxIter(ii), params.initialStepSize(ii), "Agent 1", tc.plotCommsGeometry);
+                agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, 1), params.maxIter(ii), params.initialStepSize(ii), 5.0, "Agent 1", tc.plotCommsGeometry);
 
                 % Set up remaining agents in random (valid) locations
                 for jj = 2:size(agents, 1)
@@ -154,7 +154,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
 
                     % Initialize agent
                     collisionGeometry = collisionGeometry.initialize(agentPos, params.collisionRadius(ii, jj), REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj));
-                    agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, jj), params.maxIter(ii), params.initialStepSize(ii), sprintf("Agent %d", jj), tc.plotCommsGeometry);
+                    agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, jj), params.maxIter(ii), params.initialStepSize(ii), 5.0, sprintf("Agent %d", jj), tc.plotCommsGeometry);
                 end
 
                 % randomly shuffle agents to make the network more interesting (probably)

test/test_miSim.m

@@ -9,8 +9,8 @@ classdef test_miSim < matlab.unittest.TestCase
         plotCommsGeometry = false; % disable plotting communications geometries
 
         % Sim
-        maxIter = 50;
+        maxIter = 250;
-        timestep = 0.05;
+        timestep = 0.1;
 
         % Domain
         domain = rectangularPrism; % domain geometry
@@ -31,6 +31,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
         % Agents
         initialStepSize = 0.2; % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter.
+        initialMaxAngleStepSize = 0.1; % angular step size (degrees) for tilt/azimuth gradient ascent per timestep.
         minAgents = 3; % Minimum number of agents to be randomly generated
         maxAgents = 4; % Maximum number of agents to be randomly generated
         useDoubleIntegrator = false;
@@ -43,6 +44,8 @@ classdef test_miSim < matlab.unittest.TestCase
         collisionRanges = NaN;
 
         % Sensing
+        sensor = sigmoidSensor;
+        % sigmoidSensor
         betaDistMin = 3;
         betaDistMax = 15;
         betaTiltMin = 3;
@@ -51,10 +54,19 @@ classdef test_miSim < matlab.unittest.TestCase
         alphaDistMax = 3;
         alphaTiltMin = 15; % degrees
         alphaTiltMax = 30; % degrees
-        sensor = sigmoidSensor;
+        opticalPartitioningMin = 1e-6;
+        % rfSensor
+        P_TX = 1e-3; % Transmit power (Watts)
+        BW = 20e6; % Bandwidth (Hz)
+        f_c = 3e9; % Center frequency (Hz)
+        G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+        beamwidthExponent = 16;
+        lossExponent = 2;
+        sinrPartitioningMin = 50;
 
         % Communications
         useFixedTopology = false;
+        optimizeSensorPointing = false;
         minCommsRange = 3; % Minimum randomly generated collision geometry size
         maxCommsRange = 5; % Maximum randomly generated collision geometry size
         commsRanges = NaN;
@@ -173,7 +185,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
 
                     % Initialize candidate agent
-                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize); 
+                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize); 
 
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -227,7 +239,155 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+        end
+        function miSim_run_rf_sensor(tc)
+            % randomly create obstacles
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
+            tc.obstacles = cell(nGeom, 1);
+
+            % Iterate over obstacles to initialize
+            for ii = 1:size(tc.obstacles, 1)
+                badCandidate = true;
+                while badCandidate
+                    % Instantiate a rectangular prism obstacle inside the domain
+                    tc.obstacles{ii} = rectangularPrism;
+                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
+    
+                    % Check if the obstacle collides with an existing obstacle
+                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
+                        badCandidate = false;
+                    end
+                end
+            end
+
+            % Add agents individually, ensuring that each addition does not
+            % invalidate the initialization setup
+            for ii = 1:size(tc.agents, 1)
+                initInvalid = true;
+                while initInvalid
+                    candidatePos = [tc.domain.objective.groundPos, 0];
+                    % Generate a random position for the agent based on
+                    % existing agent positions
+                    if ii == 1
+                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                            candidatePos = tc.domain.random();
+                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
+                        end
+                    else
+                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
+                        candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
+                    end
+
+                    % Make sure that the candidate position is within the
+                    % domain
+                    if ~tc.domain.contains(candidatePos)
+                        continue;
+                    end
+
+                    % Make sure that the candidate position does not crowd
+                    % the sensing objective and create boring scenarios
+                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                        continue;
+                    end
+
+                    % Make sure that there exist unobstructed lines of sight at
+                    % appropriate ranges to form a connected communications 
+                    % graph between the agents
+                    connections = false(1, ii - 1);
+                    for jj = 1:(ii - 1)
+                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
+                            % Check new agent position against all existing
+                            % agent positions for communications range
+                            connections(jj) = true;
+                            for kk = 1:size(tc.obstacles, 1)
+                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
+                                    connections(jj) = false;
+                                end
+                            end
+                        end
+                    end
+
+                    % New agent must be connected to an existing agent to
+                    % be valid
+                    if ii ~= 1 && ~any(connections)
+                        continue;
+                    end
+
+                    % Initialize candidate agent collision geometry
+                    % candidateGeometry = rectangularPrism;
+                    % candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
+                    candidateGeometry = spherical;
+                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
+
+                    % Initialize candidate agent sensor model
+                    tc.sensor = rfSensor;
+                    tilt = 0; azimuth = 0;
+                    tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);
+
+                    % Initialize candidate agent
+                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+                    
+                    % Make sure candidate agent doesn't collide with
+                    % domain
+                    violation = false;
+                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
+                        % Check if collision geometry exits domain
+                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure candidate doesn't collide with obstacles
+                    violation = false;
+                    for kk = 1:size(tc.obstacles, 1)
+                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure candidate doesn't collide with existing
+                    % agents
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+
+                    % Make sure candidate clears domain floor
+                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
+                        violation = true;
+                    end
+
+                    if violation
+                        continue;
+                    end
+
+                    % Candidate agent is valid, store to pass in to sim
+                    initInvalid = false;
+                    tc.agents{ii} = newAgent;
+                end
+            end
+
+            % Initialize the simulation
+            tc.optimizeSensorPointing = true;
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+
+            % Write out initialization state
+            tc.testClass.writeInits();
+
+            % Run simulation loop
+            tc.testClass = tc.testClass.run();
         end
         function miSim_run(tc)
             % randomly create obstacles
@@ -312,7 +472,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
 
                     % Initialize candidate agent
-                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -366,7 +526,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
 
             % Write out initialization state
             tc.testClass.writeInits();
@@ -392,15 +552,15 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize agents
             tc.commsRanges = 3 * d * ones(size(tc.agents));
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
         
             centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
             tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
@@ -419,13 +579,13 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
 
             % Initialize agents
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             close(tc.testClass.fPerf);
 
             tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
@@ -437,7 +597,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [7, 6]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
         
             % Initialize agent collision geometry
             tc.agents = {agent};
@@ -449,14 +609,129 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize agents
             tc.maxIter = 75;
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             
             % Run the simulation
-            tc.testClass = tc.testClass.run();end
+            tc.testClass = tc.testClass.run();
+        end
+        function test_single_agent_gradient_ascent_tilted(tc)
+            % make basic domain
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
+        
+            % Initialize agent collision geometry
+            tc.agents = {agent};
+            geometry1 = spherical;
+            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3, 25, 155);
+
+            % Initialize agents
+            tc.maxIter = 75;
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % Initialize the simulation
+            tc.obstacles = cell(0, 1);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+            
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
+        function test_single_agent_gradient_ascent_tilted_RF_sensor(tc)
+            % make basic domain
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
+        
+            % Initialize agent collision geometry
+            tc.agents = {agent};
+            geometry1 = spherical;
+            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+
+            tc.sensor = rfSensor;
+            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 45, 45, tc.lossExponent);
+
+            % Initialize agents
+            tc.maxIter = 75;
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % Initialize the simulation
+            tc.obstacles = cell(0, 1);
+            tc.minAlt = 0.5;
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+            
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
+        function test_single_agent_gradient_ascent_sensor_pointing(tc)
+            % make basic domain
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
+        
+            % Initialize agent collision geometry
+            tc.agents = {agent};
+            geometry1 = spherical;
+            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
+
+            % Initialize agents
+            tc.maxIter = 75;
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % Initialize the simulation
+            tc.optimizeSensorPointing = true;
+            tc.obstacles = cell(0, 1);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
+        function test_single_agent_gradient_ascent_rf_sensor_pointing(tc)
+            % make basic domain
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
+        
+            % Initialize agent collision geometry
+            tc.agents = {agent};
+            geometry1 = spherical;
+            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            
+            % Initialize agent sensor model
+
+            tc.sensor = rfSensor;
+            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 0, 0, tc.lossExponent);
+
+            % Initialize agents
+            tc.maxIter = 75;
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % Initialize the simulation
+            tc.optimizeSensorPointing = true;
+            tc.obstacles = cell(0, 1);
+            tc.minAlt = 0.5;
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+            
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
         function test_collision_avoidance(tc)
             % No obstacles
             % Fixed agent initial conditions
@@ -466,7 +741,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [3, 7]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [3, 7]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent};
@@ -483,12 +758,12 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agents
             tc.maxIter = 25;
             tc.commsRanges = 5 * ones(size(tc.agents));
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             
             % Run the simulation
             tc.testClass.run();
@@ -504,7 +779,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5.2195]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5.2195]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent;};
@@ -530,11 +805,11 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize agents
             tc.commsRanges = (2 * tc.collisionRanges(1) + obstacleLength) * 0.9 * ones(size(tc.agents)); % defined such that they cannot go around the obstacle on both sides
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2)  *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2)  *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
             
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             
             % Run the simulation
             tc.testClass.run();
@@ -570,11 +845,11 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agents
             tc.maxIter = 50;
             tc.commsRanges = 4 * ones(size(tc.agents)); % defined such that they cannot reach their objective without breaking connectivity
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
@@ -588,7 +863,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent;};
@@ -605,8 +880,8 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agents
             tc.maxIter = 125;
             tc.commsRanges = 5 * ones(size(tc.agents));
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
             
             % Initialize obstacles
             obstacleLength = 1.5;
@@ -617,7 +892,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
 
             % Communications link should be established
             tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
@@ -633,7 +908,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent; agent; agent; agent;};
@@ -652,17 +927,17 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agents
             tc.maxIter = 125;
             tc.commsRanges = ones(size(tc.agents));
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
+            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
+            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -683,7 +958,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [8, 5]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
         
             % Initialize agent collision geometry
             tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
@@ -704,19 +979,19 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agents
             tc.maxIter = 125;
             tc.commsRanges = d * ones(size(tc.agents));
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
+            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
+            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize);
+            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
-            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize);
+            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
             % Initialize the simulation
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -796,7 +1071,7 @@ classdef test_miSim < matlab.unittest.TestCase
                         tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), ...
                         tc.betaTiltMin  + rand * (tc.betaTiltMax  - tc.betaTiltMin));
                     newAgent = agent;
-                    newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+                    newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
 
                     % Domain / obstacle / agent collision checks
                     violation = false;
@@ -831,7 +1106,7 @@ classdef test_miSim < matlab.unittest.TestCase
             sim1 = miSim;
             sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ...
                 tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ...
-                tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+                tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
 
             % Write inits and build file path
             sim1.writeInits();

test/test_rfSensor.m

@@ -0,0 +1,170 @@
+classdef test_rfSensor < matlab.unittest.TestCase
+    properties (Access = private)
+        % System under test
+        testClass = sigmoidSensor;
+    end
+
+    methods (TestMethodSetup)
+        function tc = setup(tc)
+            % Reinitialize sensor with random parameters
+            tc.testClass = rfSensor;
+        end
+    end
+
+    methods (Test)
+        function plot_RSS(tc)
+            % Plot sensor performance with no sources of interference
+            P_TX = 1e-3; % Transmit power (Watts)
+            BW = 20e6; % Bandwidth (Hz)
+            f_c = 2e9; % Center frequency (Hz)
+            G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+            beamwidthExponent = 6;
+            lossExponent = 2;
+
+            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+
+            tc.testClass.plotParameters();
+        end
+        function plot_SNR(tc)
+            % Plot sensor performance with no sources of interference
+            P_TX = 1e-3; % Transmit power (Watts)
+            BW = 20e6; % Bandwidth (Hz)
+            f_c = 2e9; % Center frequency (Hz)
+            G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+            beamwidthExponent = 6;
+            lossExponent = 2;
+
+            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 30, 135, lossExponent);
+
+            altitude = 30;
+
+            % Boresight azimuth=135° (between +X at 90° and -Y at 180°) → hotspot at +X,-Y.
+            % SNR at (5,-5) should be higher than at (5,+5).
+            agentPos = [0, 0, altitude];
+            [~, snrA] = tc.testClass.sensorPerformance(agentPos, [5, -5, 0]);
+            % tc.testClass = tc.testClass.clearRssCache();
+            [~, snrB] = tc.testClass.sensorPerformance(agentPos, [5,  5, 0]);
+            tc.assertGreaterThan(snrA, snrB, "SNR should be higher toward boresight (+X,-Y) than away from it (+X,+Y)");
+
+            tc.testClass.plotPerformance(altitude);
+        end
+        function plot_SINR_one_interferer(tc)
+            % Plot sensor performance with no sources of interference
+            P_TX = 1e-3; % Transmit power (Watts)
+            BW = 20e6; % Bandwidth (Hz)
+            f_c = 2e9; % Center frequency (Hz)
+            G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+            beamwidthExponent = 6;
+            lossExponent = 2;
+
+            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+
+            altitude = 30;
+            otherSensorsPos = [6, -4, -1]; % relative to main sensor
+            otherSensors = cell(1, 1);
+            otherSensors{1} = tc.testClass; % One interfering sensor, identical to the main sensor
+
+            tc.testClass.plotPerformance(altitude, otherSensorsPos, otherSensors);
+        end
+
+        function plot_SINR_heterogenous_interferers(tc)
+            % Plot sensor performance with no sources of interference
+            P_TX = 1e-3; % Transmit power (Watts)
+            BW = 20e6; % Bandwidth (Hz)
+            f_c = 2e9; % Center frequency (Hz)
+            G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+            beamwidthExponent = 6;
+            lossExponent = 2;
+
+            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+
+            altitude = 30;
+            otherSensorsPos = [6, -4, -1; -2, 6, 0]; % relative to main sensor
+            otherSensors = cell(2, 1);
+            otherSensors{1} = rfSensor; % two heterogenous interfering sensors
+            otherSensors{2} = rfSensor;
+
+            % Must use same center frequency and bandwidth for interference sources
+            otherSensors{1} = otherSensors{1}.initialize(10 * P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            otherSensors{2} = otherSensors{2}.initialize(10 * P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+
+            tc.testClass.plotPerformance(altitude, otherSensorsPos, otherSensors);
+        end
+        function plot_SINR_heterogenous_interferers_efficiently(tc)
+            P_TX = 1e-3;
+            BW = 20e6;
+            f_c = 2e9;
+            G_RX_dBi = 3;
+            altitude = 30;
+            beamwidthExponent = [6, 4, 10];
+            lossExponent = 2;
+
+            sensor1 = rfSensor;
+            sensor1 = sensor1.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
+            sensor2 = rfSensor;
+            sensor2 = sensor2.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
+            sensor3 = rfSensor;
+            sensor3 = sensor3.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
+
+            pos1 = [0,  0,  altitude];
+            pos2 = [6, -4,  altitude - 1];
+            pos3 = [-2, 6,  altitude];
+
+            % Build a shared target grid
+            distances = -15:0.25:15;
+            [Xg, Yg] = meshgrid(distances, distances);
+            targetPos = [Xg(:), Yg(:), zeros(numel(Xg), 1)];
+
+            % Call 1: cache empty, does all computations for this timestep
+            [~, ~, sensor1, others] = sensor1.sensorPerformance(pos1, targetPos, [pos2; pos3], {sensor2; sensor3});
+            sensor2 = others{1};
+            sensor3 = others{2};
+
+            % Calls 2 and 3 use cached data
+            [~, ~, sensor2, others] = sensor2.sensorPerformance(pos2, targetPos, [pos1; pos3], {sensor1; sensor3});
+            sensor1 = others{1};
+            sensor3 = others{2};
+
+            [~, ~, sensor3, ~] = sensor3.sensorPerformance(pos3, targetPos, [pos1; pos2], {sensor1; sensor2});
+
+            % All caches should be populated after the three calls
+            tc.assertNotEmpty(sensor1.rssCache);
+            tc.assertNotEmpty(sensor2.rssCache);
+            tc.assertNotEmpty(sensor3.rssCache);
+
+            % Plot SINR from each UAV's perspective.
+            % otherSensorsPos for plotPerformance: XY = offset from calling sensor, Z = absolute_alt - calling_alt.
+            % This is exactly posOther - posSelf for each row.
+            sensor1.plotPerformance(pos1(3), [pos2 - pos1; pos3 - pos1], {sensor2; sensor3});
+            sensor2.plotPerformance(pos2(3), [pos1 - pos2; pos3 - pos2], {sensor1; sensor3});
+            sensor3.plotPerformance(pos3(3), [pos1 - pos3; pos2 - pos3], {sensor1; sensor2});
+        end
+        function plot_SINR_heterogenous_interferers_3d(tc)
+            P_TX = 1e-3;
+            BW = 20e6;
+            f_c = 2e9;
+            G_RX_dBi = 3;
+            altitude = 30;
+            beamwidthExponent = [50, 20, 200];
+            lossExponent = 2;
+
+            sensor1 = rfSensor;
+            sensor1 = sensor1.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
+            sensor2 = rfSensor;
+            sensor2 = sensor2.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
+            sensor3 = rfSensor;
+            sensor3 = sensor3.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
+
+            pos1 = [0,  0,  altitude];
+            pos2 = [6, -4,  altitude - 5];
+            pos3 = [-2, 6,  altitude + 10];
+
+            % Plot SINR from each UAV's perspective.
+            % otherSensorsPos for plotPerformance: XY = offset from calling sensor, Z = absolute_alt - calling_alt.
+            % This is exactly posOther - posSelf for each row.
+            sensor1.plot(pos1(3), [pos2 - pos1; pos3 - pos1], {sensor2; sensor3});
+            sensor2.plot(pos2(3), [pos1 - pos2; pos3 - pos2], {sensor1; sensor3});
+            sensor3.plot(pos3(3), [pos1 - pos3; pos2 - pos3], {sensor1; sensor2});
+        end
+    end
+end

util/validators/arguments/mustBeSensor.m

@@ -1,11 +1,11 @@
 function mustBeSensor(sensorModel)
     if isa(sensorModel, 'cell')
         for ii = 1:size(sensorModel, 1)
-            assert(isa(sensorModel{ii}, 'sigmoidSensor'), ...
+            assert(isa(sensorModel{ii}, 'sigmoidSensor', 'rfSensor'), ...
                    'Sensor in index %d is not a valid sensor class', ii);
         end
     else
-        assert(isa(sensorModel, 'sigmoidSensor'), ...
+        assert(isa(sensorModel, 'sigmoidSensor') || isa(sensorModel, 'rfSensor'), ...
                'Sensor is not a valid sensor class');
     end
 end