full simulation with RF sensors

@agent/agent.m

@@ -50,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/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,4 +1,4 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing)
+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};
@@ -9,6 +9,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleInt
         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,6 +34,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleInt
     maskedX = domain.objective.X(partitionMask);
     maskedY = domain.objective.Y(partitionMask);
 
+    if isa(obj.sensorModel, "rfSensor")
+        % Extract other agents' sensor models and positions once, outside the delta loop.
+        % Mask the full-grid RSS caches (filled by partition()) down to this agent's
+        % partition subset so sensorPerformance can reuse them for all perturbations.
+        otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherAgents, "UniformOutput", false));
+        otherSensors    = cellfun(@(x) x.sensorModel, otherAgents, "UniformOutput", false);
+        partitionIndices = find(partitionMask);
+        for kk = 1:numel(otherSensors)
+            if ~isempty(otherSensors{kk}.rssCache)
+                otherSensors{kk}.rssCache = otherSensors{kk}.rssCache(partitionIndices);
+            end
+        end
+        % Pre-mask this agent's own full-grid cache to the partition subset.
+        % Used for ii==1 (current position, no perturbation) to avoid recomputing.
+        baseSensorModel = obj.sensorModel;
+        if ~isempty(obj.sensorModel.rssCache)
+            baseSensorModel.rssCache = obj.sensorModel.rssCache(partitionIndices);
+        end
+    end
+
     if optimizeSensorPointing
         % Stash actual current sensor model tilt/azimuth before messing with it
         % in these following hypotheticals
@@ -58,7 +79,18 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleInt
         end
         
         % Compute performance values on partition
-        sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+        if isa(obj.sensorModel, "sigmoidSensor")
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+        elseif isa(obj.sensorModel, "rfSensor")
+            if ii == 1
+                sensorModelForDelta = baseSensorModel; % reuse partition-step cache; no recompute needed
+            else
+                sensorModelForDelta = obj.sensorModel.clearRssCache;
+            end
+            [sensorValues, ~, ~, ~] = sensorModelForDelta.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))], otherSensorsPos, otherSensors);
+        else
+            error("?");
+        end
 
         % Rearrange data into image arrays
         F = NaN(size(partitionMask));

@miSim/run.m

@@ -25,9 +25,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
@@ -42,7 +49,7 @@ function [obj] = run(obj)
         % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep, obj.optimizeSensorPointing);
+            obj.agents{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

@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

@@ -1,15 +1,24 @@
-function value = RSS(obj, d, t, a)
+function value = RSS(obj, d, dx, dy, dz)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "rfSensor")};
-        d (:, 1) double; % distance from agent to target
+        d  (:, 1) double;
-        t (:, 1) double; % LOS tilt angle
+        dx (:, 1) double;
-        a (:, 1) double; % LOS azimuth angle
+        dy (:, 1) double;
+        dz (:, 1) double;
     end
     arguments (Output)
         value (:, 1) double
     end
-    assert(size(d, 1) == size(t, 1), "Mismatch in number of distances (%d) and tilts (%d) provided", size(d, 1), size(t, 1));
+    % Boresight unit vector: [st*sa, st*ca, -ct]
-
+    % Target direction unit vector: [dx, dy, dz] / d
-    % RSS (dBm) = TX Power (dBm) + TX Antenna Gain (dBi) + RX Antenna Gain (dBi) - Path Loss (dB)
+    % cos_theta = dot product of the two, computed without per-point trig.
-    value = obj.P_TX_dBm + obj.transmitterGain(t, a) + obj.G_RX_dBi - obj.pathLoss(d);
+    st = sind(obj.tilt);
-end
+    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/computePointToPoints.m

@@ -1,24 +1,6 @@
-function [d, t, a] = computePointToPoints(obj, agentPos, targetPos)
+function [d, dx, dy, dz] = computePointToPoints(~, agentPos, targetPos)
-    arguments (Input)
+    dx = targetPos(:,1) - agentPos(1);
-        obj (1, 1) {mustBeA(obj, "rfSensor")};
+    dy = targetPos(:,2) - agentPos(2);
-        agentPos (1, 3) double;
+    dz = targetPos(:,3) - agentPos(3);
-        targetPos (:, 3) double;
+    d  = sqrt(dx.^2 + dy.^2 + dz.^2);
-    end
+end
-    arguments (Output)
-        d (:, 1) double;
-        t (:, 1) double;
-        a (:, 1) double;
-    end
-
-    % distance from sensor to target
-    d = vecnorm(agentPos - targetPos, 2, 2);
-
-    % distance from sensor nadir to target nadir (i.e. distance ignoring altitude)
-    x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2);
-    
-    % tilt angle (degrees) (0 (nadir), 180 (zenith))
-    t = (180 - atan2d(x, targetPos(:, 3) - agentPos(3)));
-
-    % azimuth angle (degrees) (0 (+y) clockwise to 360)
-    a = mod(atan2d(targetPos(:,1) - agentPos(1), targetPos(:,2) - agentPos(2)), 360);
-end

@rfSensor/rfSensor.m

@@ -15,17 +15,17 @@ classdef rfSensor
         P_TX_dBm = NaN; % Transmit power (dBm)
         N = NaN; % Thermal noise
         % Cached state (per timestep)
-        rssCache (:,1) double = double.empty(0,1); % linear-scale RSS to last ground targets grid
     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, t, a] = computePointToPoints(obj, agentPos, targetPos);
+        [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
@@ -33,7 +33,7 @@ classdef rfSensor
         obj = clearRssCache(obj);
     end
     methods (Access = private)
-        x = RSS(obj, d, t, a); % Received signal strength (function of distance and tilt angle)
+        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

@rfSensor/sensorPerformance.m

@@ -14,22 +14,21 @@ function [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targe
     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));
 
-    [d, t, a] = obj.computePointToPoints(agentPos, targetPos);
-
     if isempty(obj.rssCache)
-        obj.rssCache = 1e-3 .* 10 .^ (0.1 .* obj.RSS(d, t, a));  % dBm → W
+        [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(d));
+    I = zeros(size(S));
     for ii = 1:size(otherSensors, 1)
         if isempty(otherSensors{ii}.rssCache)
-            [d_other, t_other, a_other] = otherSensors{ii}.computePointToPoints(otherSensorsPos(ii, 1:3), targetPos);
+            [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_other, t_other, a_other));  % dBm → W
+            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);
+    SNR  = 10*log10(S ./ obj.N);
 end

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

test/test_miSim.m

@@ -44,6 +44,8 @@ classdef test_miSim < matlab.unittest.TestCase
         collisionRanges = NaN;
 
         % Sensing
+        sensor = sigmoidSensor;
+        % sigmoidSensor
         betaDistMin = 3;
         betaDistMax = 15;
         betaTiltMin = 3;
@@ -52,7 +54,15 @@ 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;
@@ -231,6 +241,154 @@ classdef test_miSim < matlab.unittest.TestCase
             % 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.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
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
@@ -439,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};
@@ -466,7 +624,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};
@@ -493,24 +651,15 @@ 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
-            minimumSINR = 50; % (dB)
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, minimumSINR, [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
-            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.sensor = rfSensor;
-            tc.sensor = tc.sensor.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 45, 45, lossExponent);
+            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;
@@ -530,7 +679,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};
@@ -558,24 +707,17 @@ 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
-            minimumSINR = 50; % (dB)
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, minimumSINR, [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
+            % Initialize agent sensor model
-            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.sensor = rfSensor;
-            tc.sensor = tc.sensor.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            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;
@@ -599,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};
@@ -637,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;};
@@ -721,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;};
@@ -766,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;};
@@ -816,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;};