test case cleanup

@agent/agent.m

@@ -3,42 +3,40 @@ classdef agent
         % Identifiers
         label = "";
 
-        % Sensor
-        sensorModel;
-        sensingLength = 0.05; % length parameter used by sensing function
-
         % State
         lastPos = NaN(1, 3); % position from previous timestep
         pos = NaN(1, 3); % current position
-        vel = NaN(1, 3); % current velocity
-        pan = NaN; % pan angle
-        tilt = NaN; % tilt angle
+
+        % Sensor
+        sensorModel;
 
         % Collision
         collisionGeometry;
-        barrierFunction;
-        dBarrierFunction;
 
         % FOV cone
         fovGeometry;
 
         % Communication
-        comRange = NaN;
         commsGeometry = spherical;
         lesserNeighbors = [];
 
+        % Performance
         performance = 0;
 
         % Plotting
         scatterPoints;
-        debug = false;
-        debugFig;
         plotCommsGeometry = true;
     end
 
+    properties (SetAccess = private, GetAccess = public)
+        initialStepSize = NaN;
+        stepDecayRate = NaN;
+    end
+
     methods (Access = public)
-        [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index);
+        [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
+        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [partitioning] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,15 +1,13 @@
-function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, comRange, label, debug, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         pos (1, 3) double;
-        vel (1, 3) double;
-        pan (1, 1) double;
-        tilt (1, 1) double;
         collisionGeometry (1, 1) {mustBeGeometry};
         sensorModel (1, 1) {mustBeSensor};
         comRange (1, 1) double;
+        maxIter (1, 1) double;
+        initialStepSize (1, 1) double = 0.2;
         label (1, 1) string = "";
-        debug (1, 1) logical = false;
         plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
@@ -17,64 +15,20 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     end
 
     obj.pos = pos;
-    obj.vel = vel;
-    obj.pan = pan;
-    obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
     obj.label = label;
-    obj.debug = debug;
     obj.plotCommsGeometry = plotCommsGeometry;
+    obj.initialStepSize = initialStepSize;
+    obj.stepDecayRate = obj.initialStepSize / maxIter;
+
+    % Initialize performance vector
+    obj.performance = [0, NaN(1, maxIter), 0];
 
     % Add spherical geometry based on com range
     obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
-    if obj.debug
-        obj.debugFig = figure;
-        tiledlayout(obj.debugFig, "TileSpacing", "tight", "Padding", "compact");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance x Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance x Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Agent Performance (C)");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Agent Performance (del C)");
-    end
-
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], 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.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@agent/partition.m

@@ -0,0 +1,35 @@
+function [partitioning] = partition(obj, agents, objective)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        agents (:, 1) {mustBeA(agents, 'cell')};
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+    end
+    arguments (Output)
+        partitioning (:, :) double;
+    end
+    
+    % Assess sensing performance of each agent at each sample point
+    % in the domain
+    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
+    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    agentPerformances = cat(3, agentPerformances{:});
+
+    % Get highest performance value at each point
+    [~, idx] = max(agentPerformances, [], 3);
+
+    % Collect agent indices in the same way as performance
+    indices = 1:size(agents, 1);
+    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+    if size(agentInds, 1) ~= size(agents, 1)
+        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
+    end
+    agentInds = num2cell(agentInds, 2:3);
+    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
+    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
+    agentInds = cat(3, agentInds{:});
+
+    % Use highest performing agent's index to form partitions
+    [m, n, ~] = size(agentInds);
+    [jj, kk] = ndgrid(1:m, 1:n);
+    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+end

@agent/plot.m

@@ -36,5 +36,6 @@ function [obj, f] = plot(obj, ind, f)
     end
 
     % Plot FOV geometry
-    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
+    maxAlt = f.Children(1).Children(end).ZLim(2); % to avoid scaling the FOV geometry as the sim runs, let's just make it really big and hide the excess under the floor of the domain. Check the domain altitude to figure out how big it needs to be to achieve this deception.
+    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f, maxAlt);
 end

@agent/run.m

@@ -1,10 +1,11 @@
-function obj = run(obj, domain, partitioning, t, index)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
-        t (1, 1) double;
+        timestepIndex (1, 1) double;
         index (1, 1) double;
+        agents (:, 1) {mustBeA(agents, 'cell')};
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
@@ -12,136 +13,72 @@ function obj = run(obj, domain, partitioning, t, index)
 
     % Collect objective function values across partition
     partitionMask = partitioning == index;
-    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
-
-    % Compute sensor performance across partition
-    maskedX = domain.objective.X(partitionMask);
-    maskedY = domain.objective.Y(partitionMask);
-    zFactor = 1;
-    sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-    sensorValuesLower = obj.sensorModel.sensorPerformance(obj.pos - [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
-    sensorValuesHigher = obj.sensorModel.sensorPerformance(obj.pos + [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
-
-    % Put the values back into the form of the partition to enable basic operations on this data
-    F = NaN(size(partitionMask));
-    F(partitionMask) = objectiveValues;
-    S = NaN(size(partitionMask));
-    Slower = S;
-    Shigher = S;
-    S(partitionMask) = sensorValues;
-    Slower(partitionMask) = sensorValuesLower;
-    Shigher(partitionMask) = sensorValuesHigher;
-
-    % Find agent's performance
-    C = S .* F;
-    obj.performance = [obj.performance, sum(C(~isnan(C)))]; % at current Z only
-    C = cat(3, Shigher, S, Slower) .* F;
-
-    % Compute gradient on agent's performance
-    [gradCX, gradCY, gradCZ] = gradient(C, domain.objective.discretizationStep); % grad C
-    gradC = cat(4, gradCX, gradCY, gradCZ);
-    nGradC = vecnorm(gradC, 2, 4);
-
-    if obj.debug
-        % Compute additional component-level values for diagnosing issues
-        [gradSensorPerformanceX, gradSensorPerformanceY] = gradient(S, domain.objective.discretizationStep); % grad S_n
-        [gradObjectiveX, gradObjectiveY] = gradient(F, domain.objective.discretizationStep); % grad f
-        gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
-        gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
-
-        ii = 8;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F./max(F, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S./max(S, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S .* vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all')./(max(F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'))));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), C./max(C, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), nGradC./max(nGradC, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        [x, y] = find(nGradC == max(nGradC, [], "all"));
-
-        % just pick one
-        r = randi([1, size(x, 1)]);
-        x = x(r); y = y(r);
-
-        % switch them
-        temp = x;
-        x = y;
-        y = temp;
-        
-        % find objective location in discrete domain
-        [~, xIdx] = find(domain.objective.groundPos(1) == domain.objective.X);
-        xIdx = unique(xIdx);
-        [yIdx, ~] = find(domain.objective.groundPos(2) == domain.objective.Y);
-        yIdx = unique(yIdx);
-        for ii = 8:-1:1
-            hold(obj.debugFig.Children(1).Children(ii), "on");
-            % plot GA selection
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'go');
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'g+');
-            % plot objective center
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'ro');
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'r+');
-            hold(obj.debugFig.Children(1).Children(ii), "off");
-        end
-    end
-
-    % return now if there is no data to work with, and do not move
-    if all(isnan(nGradC), 'all')
+    if ~unique(partitionMask)
+        % This agent has no partition, maintain current state
         return;
     end
+    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-    % Use largest grad(C) value to find the direction of the next position
-    [xNextIdx, yNextIdx, zNextIdx] = ind2sub(size(nGradC), find(nGradC == max(nGradC, [], 'all')));
-    % switch them
-    temp = xNextIdx;
-    xNextIdx = yNextIdx;
-    yNextIdx = temp;
+    % Compute sensor performance on partition
+    maskedX = domain.objective.X(partitionMask);
+    maskedY = domain.objective.Y(partitionMask);
 
-    roundingScale = 10^-log10(domain.objective.discretizationStep);
-    zKey = zFactor * [1; 0; -1];
-    pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3) + zKey(zNextIdx)]; % have to do some unfortunate rounding here sometimes
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+    deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
+    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+    for ii = 1:7
+        % Apply delta to position
+        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+        
+        % Compute performance values on partition
+        if ii < 5
+            % 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.
+            % Those values are computed once before the loop and are only
+            % recomputed when +/- Z steps are applied
+        else
+            % Redo partitioning for Z stepping only
+            partitioning = obj.partition(agents, domain.objective);
 
-    % Determine next position
-    vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
-    rate = 0.1 - 0.0004 * t; % slow down as you get closer, coming to a stop by the end
-    nextPos = obj.pos + vDir * rate;
+            % 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
+        F = NaN(size(partitionMask));
+        F(partitionMask) = objectiveValues;
+        S = NaN(size(partitionMask));
+        S(partitionMask) = sensorValues;
+
+        % Compute agent performance
+        C = S .* F;
+        C_delta(ii) = sum(C(~isnan(C)));
+    end
+
+    % Store agent performance at current time and place
+    obj.performance(timestepIndex + 1) = C_delta(1);
+
+    % 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)];
+
+    % Compute scaling factor
+    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+    rateFactor = targetRate / norm(gradC);
+
+    % Compute unconstrained next position
+    pNext = obj.pos + rateFactor * gradC;
 
     % Move to next position
     obj.lastPos = obj.pos;
-    obj.pos = nextPos;
+    obj.pos = pNext;
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;

@agent/updatePlots.m

@@ -5,6 +5,13 @@ function updatePlots(obj)
     arguments (Output)
     end
 
+    % Find change in agent position since last timestep
+    deltaPos = obj.pos - obj.lastPos;
+    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+        % Agent did not move, so nothing has to move on the plots
+        return;
+    end
+
     % Scatterplot point positions
     for ii = 1:size(obj.scatterPoints, 1)
         obj.scatterPoints(ii).XData = obj.pos(1);
@@ -12,9 +19,6 @@ function updatePlots(obj)
         obj.scatterPoints(ii).ZData = obj.pos(3);
     end
 
-    % Find change in agent position since last timestep
-    deltaPos = obj.pos - obj.lastPos;
-
     % Collision geometry edges
     for jj = 1:size(obj.collisionGeometry.lines, 2)
         % Update plotting
@@ -39,6 +43,7 @@ function updatePlots(obj)
     % Update FOV geometry surfaces
     for jj = 1:size(obj.fovGeometry.surface, 2)
         % Update each plot
+        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
         obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
         obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
         obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);

@miSim/constrainMotion.m

@@ -14,6 +14,11 @@ function [obj] = constrainMotion(obj)
 
     agents = [obj.agents{:}];
     v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
+    if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all')
+        % Agents are not attempting to move, so there is no motion to be
+        % constrained
+        return;
+    end
 
     % Initialize QP based on number of agents and obstacles
     nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
@@ -34,7 +39,7 @@ function [obj] = constrainMotion(obj)
             
             A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (agents(ii).pos - agents(jj).pos);
             A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-            b(kk) = obj.barrierGain * h(ii, jj)^3;
+            b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent;
             kk = kk + 1;
         end
     end
@@ -49,7 +54,7 @@ function [obj] = constrainMotion(obj)
             hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
 
             A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
-            b(kk) = obj.barrierGain * hObs(ii, jj)^3;
+            b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
            
             kk = kk + 1;
         end
@@ -62,37 +67,37 @@ function [obj] = constrainMotion(obj)
         % X minimum
         h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMin^3;
+        b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent;
         kk = kk + 1;
     
         % X maximum
         h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMax^3;
+        b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent;
         kk = kk + 1;
     
         % Y minimum
         h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
-        b(kk) = obj.barrierGain * h_yMin^3;
+        b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent;
         kk = kk + 1;
     
         % Y maximum
         h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
-        b(kk) = obj.barrierGain * h_yMax^3;
+        b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent;
         kk = kk + 1;
     
         % Z minimum
         h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
-        b(kk) = obj.barrierGain * h_zMin^3;
+        b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
         kk = kk + 1;
     
         % Z maximum
         h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
-        b(kk) = obj.barrierGain * h_zMax^3;
+        b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent;
         kk = kk + 1;
     end
 
@@ -109,11 +114,7 @@ function [obj] = constrainMotion(obj)
 
                 A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-                b(kk) = obj.barrierGain * hComms(ii, jj);
-
-                % dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities
-                % h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal';
-                % b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3;
+                b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent;
 
                 kk = kk + 1; 
             end

@miSim/initialize.m

@@ -1,12 +1,12 @@
-function obj = initialize(obj, domain, objective, agents, minAlt, timestep, partitoningFreq, maxIter, obstacles, makePlots, makeVideo)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
-        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
         agents (:, 1) cell;
+        barrierGain (1, 1) double = 100;
+        barrierExponent (1, 1) double = 3;
         minAlt (1, 1) double = 1;
         timestep (:, 1) double = 0.05;
-        partitoningFreq (:, 1) double = 0.25
         maxIter (:, 1) double = 1000;
         obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
         makePlots(1, 1) logical = true;
@@ -26,6 +26,9 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     end
     obj.makeVideo = makeVideo;
 
+    % Generate artifact(s) name
+    obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
+
     % Define simulation time parameters
     obj.timestep = timestep;
     obj.timestepIndex = 0;
@@ -33,22 +36,17 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
 
     % Define domain
     obj.domain = domain;
-    obj.partitioningFreq = partitoningFreq;
 
     % Add geometries representing obstacles within the domain
     obj.obstacles = obstacles;
 
     % Add an additional obstacle spanning the domain's footprint to 
     % represent the minimum allowable altitude
-    obj.minAlt = minAlt;
-    if obj.minAlt > 0
+    if minAlt > 0
         obj.obstacles{end + 1, 1} = rectangularPrism;
-        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
     end
 
-    % Define objective
-    obj.objective = objective;
-
     % Define agents
     obj.agents = agents;
     obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
@@ -67,22 +65,25 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
         end
     end
 
+    % Set CBF parameters
+    obj.barrierGain = barrierGain;
+    obj.barrierExponent = barrierExponent;
+
     % Compute adjacency matrix and lesser neighbors
     obj = obj.updateAdjacency();
     obj = obj.lesserNeighbor();
 
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
-    obj.partitioningTimes = obj.times(obj.partitioningFreq:obj.partitioningFreq:size(obj.times, 1));
 
     % Prepare performance data store (at t = 0, all have 0 performance)
     obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
     % Prepare h function data store
-    obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1) - 1);
+    obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
 
     % Create initial partitioning
-    obj = obj.partition();
+    obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
     % Initialize variable that will store agent positions for trail plots
     obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
@@ -90,4 +91,7 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
 
     % Set up plots showing initialized state
     obj = obj.plot();
+
+    % Run validations
+    obj.validate();
 end

@miSim/miSim.m

@@ -5,7 +5,6 @@ classdef miSim
     properties (SetAccess = private, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
-        partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
         maxIter = NaN; % maximum number of simulation iterations
         domain = rectangularPrism;
         objective = sensingObjective;
@@ -13,13 +12,12 @@ classdef miSim
         agents = cell(0, 1); % agents that move within the domain
         adjacency = NaN; % Adjacency matrix representing communications network graph
         constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
-        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
         perf; % sensor performance timeseries array
         performance = 0; % simulation performance timeseries vector
-        barrierGain = 100; % collision avoidance parameter
-        minAlt = 1; % minimum allowed altitude constraint
-
+        barrierGain = 100; % CBF gain parameter
+        barrierExponent = 3; % CBF exponent parameter
+        artifactName = "";
         fPerf; % performance plot figure
     end
 
@@ -56,7 +54,7 @@ classdef miSim
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
         [obj] = constrainMotion(obj);
@@ -68,8 +66,9 @@ classdef miSim
         [obj] = plotGraph(obj);
         [obj] = plotTrails(obj);
         [obj] = plotH(obj);
-        [obj] = updatePlots(obj, updatePartitions);
+        [obj] = updatePlots(obj);
         validate(obj);
+        teardown(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/partition.m

@@ -1,33 +0,0 @@
-function obj = partition(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    % Assess sensing performance of each agent at each sample point
-    % in the domain
-    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
-    agentPerformances = cat(3, agentPerformances{:});
-    
-    % Get highest performance value at each point
-    [~, idx] = max(agentPerformances, [], 3);
-
-    % Collect agent indices in the same way as performance
-    indices = 1:size(obj.agents, 1);
-    agentInds = squeeze(tensorprod(indices, ones(size(obj.objective.X))));
-    if size(agentInds, 1) ~= size(obj.agents, 1)
-        agentInds = reshape(agentInds, [size(obj.agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
-    end
-    agentInds = num2cell(agentInds, 2:3);
-    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
-    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
-    agentInds = cat(3, agentInds{:});
-
-    % Use highest performing agent's index to form partitions
-    [m, n, ~] = size(agentInds);
-    [jj, kk] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-end

@miSim/plot.m

@@ -51,4 +51,7 @@ function obj = plot(obj)
 
     % Plot h functions
     obj = obj.plotH();
+
+    % Switch back to primary figure
+    figure(obj.f);
 end

@miSim/plotConnections.m

@@ -32,10 +32,8 @@ function obj = plotConnections(obj)
     end
 
     % Copy to other plots
-    if size(obj.spatialPlotIndices, 2) > 1
-        for ii = 2:size(obj.spatialPlotIndices, 2)
-            o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
-        end 
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
     end
 
     obj.connectionsPlot = o;

@miSim/plotTrails.m

@@ -19,8 +19,12 @@ function obj = plotTrails(obj)
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
     end
 
-    % Copy trails to other figures?
-    obj.trailPlot = o;
+    % Copy to other plots
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    end 
 
     % Add legend?
+
+    obj.trailPlot = o;
 end

@miSim/run.m

@@ -18,41 +18,39 @@ function [obj] = run(obj)
         obj.timestepIndex = ii;
         fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
+        % Before moving
         % Validate current simulation configuration
         obj.validate();
 
-        % Check if it's time for new partitions
-        updatePartitions = false;
-        if ismember(obj.t, obj.partitioningTimes)
-            updatePartitions = true;
-            obj = obj.partition();
-        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);
 
         % Determine desired communications links
         obj = obj.lesserNeighbor();
 
+        % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t, jj);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
         end
 
         % Adjust motion determined by unconstrained gradient ascent using
         % CBF constraints solved by QP
         obj = constrainMotion(obj);
 
-        % Finished simulation for this timestep, do accounting
-
+        % After moving
         % Update agent position history array
         obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
 
         % Update total performance
-        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];
+        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
 
         % Update adjacency matrix
         obj = obj.updateAdjacency();
 
         % Update plots
-        obj = obj.updatePlots(updatePartitions);
+        obj = obj.updatePlots();
 
         % Write frame in to video
         if obj.makeVideo

@miSim/setupVideoWriter.m

@@ -7,9 +7,9 @@ function v = setupVideoWriter(obj)
     end
 
     if ispc || ismac
-        v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'MPEG-4');
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
     elseif isunix
-        v = VideoWriter(fullfile('.', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
     end
     
     v.FrameRate = 1 / obj.timestep;

@miSim/teardown.m

@@ -0,0 +1,13 @@
+function teardown(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    % Close plots
+    close(obj.hf);
+    close(obj.fPerf);
+    close(obj.f);
+
+end

@miSim/updateAdjacency.m

@@ -17,19 +17,8 @@ function obj = updateAdjacency(obj)
                 A(ii, jj) = false; % comm range violation
                 continue;
             end
-
-            % % Check that agents do not have their line of sight obstructed
-            % for kk = 1:size(obj.obstacles, 1)
-            %     if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos)
-            %         A(ii, jj) = false;
-            %     end
-            % end
         end
     end
 
     obj.adjacency = A & A';
-
-    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
-        warning("Eliminated network connections that were necessary");
-    end
 end

@miSim/updatePlots.m

@@ -1,7 +1,6 @@
-function [obj] = updatePlots(obj, updatePartitions)
+function [obj] = updatePlots(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        updatePartitions (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
@@ -30,10 +29,8 @@ function [obj] = updatePlots(obj, updatePartitions)
     obj = obj.plotGraph();
 
     % Update partitioning plot
-    if updatePartitions
-        delete(obj.partitionPlot);
-        obj = obj.plotPartitions();
-    end
+    delete(obj.partitionPlot);
+    obj = obj.plotPartitions();
 
     % reset plot limits to fit domain
     for ii = 1:size(obj.spatialPlotIndices, 2)
@@ -43,22 +40,24 @@ function [obj] = updatePlots(obj, updatePartitions)
     end
 
     % Update agent trails
-    for ii = 1:size(obj.agents, 1)
-        obj.trailPlot(ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
-        obj.trailPlot(ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
-        obj.trailPlot(ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+    for jj = 1:size(obj.spatialPlotIndices, 2)
+        for ii = 1:size(obj.agents, 1)
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+        end
     end
 
     drawnow;
     
     % Update performance plot
     % Re-normalize performance plot
-    normalizingFactor = 1/max(obj.performance(end));
-    obj.performancePlot(1).YData(1:length(obj.performance)) = obj.performance * normalizingFactor;
-    obj.performancePlot(1).XData(obj.timestepIndex) = obj.t;
-    for ii = 2:(size(obj.agents, 1) + 1)
-        obj.performancePlot(ii).YData(1:length(obj.performance)) = obj.agents{ii - 1}.performance * normalizingFactor;
-        obj.performancePlot(ii).XData(obj.timestepIndex) = obj.t;
+    normalizingFactor = 1/max(obj.performance);
+    obj.performancePlot(1).YData(1:(length(obj.performance) + 1)) = [obj.performance, 0] * normalizingFactor;
+    obj.performancePlot(1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
+    for ii = 1:(size(obj.agents, 1))
+        obj.performancePlot(ii + 1).YData(1:(length(obj.performance) + 1)) = [obj.agents{ii}.performance(1:length(obj.performance)), 0] * normalizingFactor;
+        obj.performancePlot(ii + 1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
     end
 
     % Update h function plots

@miSim/validate.m

@@ -5,8 +5,23 @@ function validate(obj)
     arguments (Output)
     end
 
+    %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
         warning("Network is not connected");
     end
 
+    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
+        warning("Eliminated network connections that were necessary");
+    end
+
+    %% Obstacle Validators
+    AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
+    AO_collisions = vertcat(AO_collisions{:});
+    if any(AO_collisions)
+        [idx, idy] = find(AO_collisions);
+        for ii = 1:size(idx, 1)
+            error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
+        end
+    end
+
 end

@miSim/writeParams.m

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

@sensingObjective/initialize.m

@@ -1,10 +1,11 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
@@ -12,6 +13,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
 
     obj.discretizationStep = discretizationStep;
 
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+
     obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 

@sensingObjective/initializeRandomMvnpdf.m

@@ -14,13 +14,12 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     while domain.distance(mu) < protectedRange
         mu = domain.random();
     end
-    mu = mu(1:2);
 
     % Set random distribution parameters
     sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
 
     % Set up random bivariate normal distribution function
-    objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig);
+    objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
     obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);

@sensingObjective/sensingObjective.m

@@ -2,18 +2,19 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundAlt = 0;
-        groundPos = [0, 0];
-        discretizationStep = 1;
-        objectiveFunction = @(x, y) 0; % define objective functions over a grid in this manner
+        groundAlt = NaN;
+        groundPos = [NaN, NaN];
+        discretizationStep = NaN;
+        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
         X = [];
         Y = [];
         values = [];
-        protectedRange = 1; % keep obstacles from crowding objective
+        protectedRange = NaN; % keep obstacles from crowding objective
+        sensorPerformanceMinimum = NaN; % minimum sensor performance to allow assignment of a point in the domain to a partition
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange);
+        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
         [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end

@sigmoidSensor/initialize.m

@@ -1,10 +1,8 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
-        alphaPan (1, 1) double;
-        betaPan (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
     end
@@ -14,8 +12,6 @@ function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt
     
     obj.alphaDist = alphaDist;
     obj.betaDist = betaDist;
-    obj.alphaPan = alphaPan;
-    obj.betaPan = betaPan;
     obj.alphaTilt = alphaTilt;
     obj.betaTilt = betaTilt;
 end

@sigmoidSensor/sensorPerformance.m

@@ -1,9 +1,7 @@
-function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
+function value = sensorPerformance(obj, agentPos, targetPos)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
         agentPos (1, 3) double;
-        agentPan (1, 1) double;
-        agentTilt (1, 1) double;
         targetPos (:, 3) double;
     end
     arguments (Output)
@@ -15,7 +13,7 @@ function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos
     x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
 
     % compute tilt angle
-    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))) - agentTilt; % degrees
+    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
 
     % Membership functions
     mu_d = obj.distanceMembership(d);

@sigmoidSensor/sigmoidSensor.m

@@ -3,15 +3,12 @@ classdef sigmoidSensor
         % Sensor parameters
         alphaDist = NaN;
         betaDist = NaN;
-        alphaPan = NaN;
-        betaPan = NaN;
         alphaTilt = NaN; % degrees
         betaTilt = NaN;
     end
 
     methods (Access = public)
-        [obj]               = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt);
-        [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning);
+        [obj]               = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt);
         [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
         [f] = plotParameters(obj);
     end

geometries/@cone/cone.m

@@ -17,6 +17,6 @@ classdef cone
 
     methods (Access = public)
         [obj   ] = initialize(obj, center, radius, height, tag, label);
-        [obj, f] = plot(obj, ind, f);
+        [obj, f] = plot(obj, ind, f, maxAlt);
     end
 end

geometries/@cone/plot.m

@@ -1,8 +1,9 @@
-function [obj, f] = plot(obj, ind, f)
+function [obj, f] = plot(obj, ind, f, maxAlt)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'cone')};
         ind (1, :) double = NaN;
         f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        maxAlt (1, 1) = 10;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'cone')};
@@ -12,16 +13,18 @@ function [obj, f] = plot(obj, ind, f)
     % Create axes if they don't already exist
     f = firstPlotSetup(f);
     
+    scalingFactor = (maxAlt / obj.height);
+
     % Plot cone
-    [X, Y, Z] = cylinder([obj.radius, 0], obj.n);
+    [X, Y, Z] = cylinder([scalingFactor * obj.radius, 0], obj.n);
     
     % Scale to match height
-    Z = Z * obj.height;
+    Z = Z * maxAlt;
     
     % Move to center location
     X = X + obj.center(1);
     Y = Y + obj.center(2);
-    Z = Z + obj.center(3);
+    Z = Z + obj.center(3) - maxAlt;
     
     % Plot
     if isnan(ind)

geometries/@rectangularPrism/initialize.m

@@ -48,13 +48,4 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     if tag == REGION_TYPE.DOMAIN
         obj.objective = sensingObjective;
     end
-
-    % Initialize CBF
-    % first part evaluates to +/-1 if the point is outside/inside the collision geometry
-    % Second part determines the distance from the point to the boundary of the collision geometry
-    obj.barrierFunction = @(x) (1 - 2 * obj.collisionGeometry.contains(x)) * obj.collisionGeometry.distance(x); % x is 1x3
-    % gradient of barrier function 
-    obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
-    % as long as the collisionGeometry object is updated during runtime,
-    % these functions never have to be updated again
 end

geometries/@rectangularPrism/initializeRandom.m

@@ -6,7 +6,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d
         minDimension (1, 1) double = 10;
         maxDimension (1, 1) double = 20;
         domain (1, 1) {mustBeGeometry} = rectangularPrism;
-        minAlt (1, 1) double = 0;
+        minAlt (1, 1) double = 1;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};

geometries/@rectangularPrism/rectangularPrism.m

@@ -20,10 +20,6 @@ classdef rectangularPrism
 
         % Plotting
         lines;
-
-        % collision
-        barrierFunction;
-        dBarrierFunction;
     end
     properties (SetAccess = public, GetAccess = public)
         label = "";

geometries/@spherical/initialize.m

@@ -18,11 +18,6 @@ function obj = initialize(obj, center, radius, tag, label)
     obj.radius = radius;
     obj.diameter = 2 * obj.radius;
 
-    % Initialize CBF
-    obj.barrierFunction = @(x) NaN;
-    % gradient of barrier function 
-    obj.dBarrierFunction = @(x) NaN;
-
     % fake vertices in a cross pattern
     obj.vertices = [obj.center + [obj.radius, 0, 0]; ...
                     obj.center - [obj.radius, 0, 0]; ...

geometries/@spherical/plotWireframe.m

@@ -28,7 +28,7 @@ function [obj, f] = plotWireframe(obj, ind, f)
         o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 1);
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

geometries/@spherical/spherical.m

@@ -11,10 +11,6 @@ classdef spherical
 
         % Plotting
         lines;
-
-        % collision
-        barrierFunction;
-        dBarrierFunction;
     end
     properties (SetAccess = public, GetAccess = public)
         % Meta

resources/project/9ucwbDs2BkA9ZqIHQI_XVIGSH5A/r541J80LBY7p-VdF-2cmu2pJyd4p.xml

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

resources/project/9ucwbDs2BkA9ZqIHQI_XVIGSH5A/svhNcplUM7n23Qjwql1uZ5L6c6sd.xml

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

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/QQkomTANNGDauE43PAdpZJlxaiQp.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="d143c27d-6824-4569-9093-8150b60976cb" type="Reference"/>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/jMxqi1-UdFyFMXqowBRZrZ8JeFEd.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/jMxqi1-UdFyFMXqowBRZrZ8JeFEp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/c0mYkONNU4mIjcCgfDyvbU49RuMp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/lJsL2R-bNQunxq-01eTS9mZYaLsp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/uMGDwEZ0yJ74zW0K0aO7UX4-C-Yp.xml

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

resources/project/o6vRLbVRBKxltajotIPvuRfoO58/s62GUzlA81yhDNi9jlY63dGGTVId.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/9ucwbDs2BkA9ZqIHQI_XVIGSH5Ap.xml

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

resources/project/r541J80LBY7p-VdF-2cmu2pJyd4/KQTHy3u41hZ5fp_zyPTt34VbI4Yp.xml

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

resources/project/r541J80LBY7p-VdF-2cmu2pJyd4/nQxSe2SDiKQ4IIJQlHCi9SxD7Vsp.xml

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

resources/project/r541J80LBY7p-VdF-2cmu2pJyd4/peMR3cfX5HUpN7z1gUid-46m5V8d.xml

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

resources/project/r541J80LBY7p-VdF-2cmu2pJyd4/peMR3cfX5HUpN7z1gUid-46m5V8p.xml

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

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/Jdb23yLfHsZsXlb_4WZeit3L6-Ud.xml

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

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/Jdb23yLfHsZsXlb_4WZeit3L6-Up.xml

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

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/a6WBlPgSaAg9MeMZ9a3VNFUwwHYp.xml

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

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/lA9hUETTjsTN6BFBFM_Djjvh2CIp.xml

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

sensorModels/@fixedCardinalSensor/fixedCardinalSensor.m

@@ -1,13 +0,0 @@
-classdef fixedCardinalSensor
-    % Senses in the +/-x, +/- y directions at some specified fixed length
-    properties
-        alphaTilt = NaN;
-        r = 0.1; % fixed sensing length
-    end
-
-    methods (Access = public)
-        [obj] = initialize(obj, r);
-        [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning);
-        [value] = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
-    end
-end

sensorModels/@fixedCardinalSensor/initialize.m

@@ -1,10 +0,0 @@
-function obj = initialize(obj, r)
-    arguments(Input)
-        obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
-        r (1, 1) double;
-    end
-    arguments(Output)
-        obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
-    end
-    obj.r = r;
-end

sensorModels/@fixedCardinalSensor/sense.m

@@ -1,45 +0,0 @@
-function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
-        agent (1, 1) {mustBeA(agent, 'agent')};
-        sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
-        domain (1, 1) {mustBeGeometry};
-        partitioning (:, :) double = NaN;
-    end
-    arguments (Output)
-        neighborValues (4, 1) double;
-        neighborPos (4, 3) double;
-    end
-
-    % Set alphaTilt to produce an FOV cone with radius 'r' on the ground
-    obj.alphaTilt = atan2(obj.r, agent.pos(3));
-
-    % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
-    currentPos = agent.pos(1:2);
-    neighborPos = [currentPos(1) + obj.r, currentPos(2); ... % (+x)
-                   currentPos(1), currentPos(2) + obj.r; ... % (+y)
-                   currentPos(1) - obj.r, currentPos(2); ... % (-x)
-                   currentPos(1), currentPos(2) - obj.r; ... % (-y)
-                  ];
-
-    % Check for neighbor positions that fall outside of the domain
-    outOfBounds = false(size(neighborPos, 1), 1);
-    for ii = 1:size(neighborPos, 1)
-        if ~domain.contains([neighborPos(ii, :), 0])
-            outOfBounds(ii) = true;
-        end
-    end
-
-    % Replace out of bounds positions with inoffensive in-bounds positions
-    neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1);
-
-    % Sense values at selected positions
-    neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
-                      sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
-                      sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
-                      sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
-                     ];
-
-    % Prevent out of bounds locations from ever possibly being selected
-    neighborValues(outOfBounds) = 0;
-end

sensorModels/@fixedCardinalSensor/sensorPerformance.m

@@ -1,14 +0,0 @@
-function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
-        agentPos (1, 3) double;
-        agentPan (1, 1) double;
-        agentTilt (1, 1) double;
-        targetPos (:, 3) double;
-    end
-    arguments (Output)
-        value (:, 1) double;
-    end
-
-    value = 0.5 * ones(size(targetPos, 1), 1);
-end

test/parametricTestSuite.m

@@ -0,0 +1,77 @@
+classdef parametricTestSuite < matlab.unittest.TestCase
+    properties (Access = private)
+        % System under test
+        testClass = miSim;
+        domain = rectangularPrism;
+        obstacles = cell(1, 0);
+
+        %% Diagnostic Parameters
+        % No effect on simulation dynamics
+        makeVideo = true; % disable video writing for big performance increase
+        makePlots = true; % disable plotting for big performance increase (also disables video)
+        plotCommsGeometry = false; % disable plotting communications geometries
+        protectedRange = 0;
+    end
+    properties (TestParameter)
+        %% Simulation Parameters
+        timestep = num2cell([1]); % duration of one simulation timestep
+        maxIter = num2cell([25]); % number of timesteps to run
+
+        % Domain parameters
+        minAlt = num2cell([1]); % minimum allowed agent altitude, make sure test cases don't conflict with this
+
+        % Constraint parameters
+        barrierGain = num2cell([100]);
+        barrierExponent = num2cell([3]);
+
+        % Sensing Objective Parameters
+        sensorPerformanceMinimum = num2cell([1e-6]); % sensor performance threshhold for partition assignment
+        discretizationStep = num2cell([0.01]); % sensing objective discretization step size
+        % this value goes on to determine central differences used in
+        % gradient ascent and partitioning element sizes
+
+        % Agent Parameters
+        collisionRadius = num2cell([0.1]);
+        initialStepSize = num2cell([0.2]); % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter.
+
+        % Sensor Model Parameters
+        alphaDist = num2cell([2.5, 5]);
+        betaDist = num2cell([3, 15]);
+        alphaTilt = num2cell([15, 30]); % (degrees)methods
+        betaTilt = num2cell([3, 15]);
+
+        % Communications Parameters
+        comRange = num2cell([3]);
+    end
+
+    methods (Test, ParameterCombination = "exhaustive")
+        % Test cases
+        function single_agent_gradient_ascent(tc, timestep, maxIter, barrierGain, barrierExponent, minAlt, sensorPerformanceMinimum, discretizationStep, collisionRadius, initialStepSize, alphaDist, betaDist, alphaTilt, betaTilt, comRange)
+            % Set up square domain
+            l = 10;
+            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, discretizationStep, tc.protectedRange, sensorPerformanceMinimum);
+            
+            % Set up agent
+            sensorModel = sigmoidSensor;
+            sensorModel = sensorModel.initialize(alphaDist, betaDist, alphaTilt, betaTilt);
+            agentPos = [l/4, l/4, l/4];
+            collisionGeometry = spherical;
+            collisionGeometry = collisionGeometry.initialize(agentPos, collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Region");
+            agents = {agent};
+            agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, "Agent 1", tc.plotCommsGeometry);
+
+            % Set up simulation
+            tc.testClass = tc.testClass.initialize(tc.domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+
+            % Save simulation parameters to output file
+            tc.testClass.writeParams();
+
+            % Run
+            tc.testClass = tc.testClass.run();
+
+            % Cleanup
+            tc.testClass.teardown();
+        end
+    end
+end

test/test_miSim.m

@@ -9,9 +9,8 @@ classdef test_miSim < matlab.unittest.TestCase
         plotCommsGeometry = false; % disable plotting communications geometries
 
         % Sim
-        maxIter = 250;
-        timestep = 0.05
-        partitoningFreq = 5;
+        maxIter = 50;
+        timestep = 0.05;
 
         % Domain
         domain = rectangularPrism; % domain geometry
@@ -31,9 +30,9 @@ classdef test_miSim < matlab.unittest.TestCase
         objective = sensingObjective;
 
         % Agents
-        minAgents = 4; % Minimum number of agents to be randomly generated
-        maxAgents = 6; % Maximum number of agents to be randomly generated
-        sensingLength = 0.05; % length parameter used by sensing function
+        initialStepSize = 0.2; % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter.
+        minAgents = 3; % Minimum number of agents to be randomly generated
+        maxAgents = 4; % Maximum number of agents to be randomly generated
         agents = cell(0, 1);
         
         % Collision
@@ -50,9 +49,16 @@ classdef test_miSim < matlab.unittest.TestCase
         alphaDistMax = 3;
         alphaTiltMin = 15; % degrees
         alphaTiltMax = 30; % degrees
+        sensor = sigmoidSensor;
 
         % Communications
-        comRange = 8; % Maximum range between agents that forms a communications link
+        minCommsRange = 3; % Minimum randomly generated collision geometry size
+        maxCommsRange = 5; % Maximum randomly generated collision geometry size
+        commsRanges = NaN;
+
+        % Constraints
+        barrierGain = 100;
+        barrierExponent = 3;
     end
 
     % Setup for each test
@@ -72,8 +78,11 @@ classdef test_miSim < matlab.unittest.TestCase
                 tc.agents{ii, 1} = agent;
             end
 
-            % Define random collision ranges for each agent
+            % Random collision ranges for each agent
             tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
+        
+            % Random commuunications ranges for each agent
+            tc.commsRanges = tc.minCommsRange + rand(size(tc.agents, 1), 1) * (tc.maxCommsRange - tc.minCommsRange);
         end
     end
 
@@ -158,11 +167,10 @@ classdef test_miSim < matlab.unittest.TestCase
                     candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
 
                     % Initialize candidate agent sensor model
-                    sensor = sigmoidSensor;
-                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+                    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, zeros(1,3), 0, 0, candidateGeometry, sensor, tc.comRange); 
+                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize); 
 
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -210,7 +218,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            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);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -292,11 +300,10 @@ classdef test_miSim < matlab.unittest.TestCase
                     candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
 
                     % Initialize candidate agent sensor model
-                    sensor = sigmoidSensor;
-                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+                    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, zeros(1,3), 0, 0, candidateGeometry, sensor, tc.comRange);
+                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.comRange, tc.maxIter, tc.initialStepSize);
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -344,7 +351,10 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            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);
+
+            % Write out parameters
+            tc.testClass.writeParams();
 
             % Run simulation loop
             tc.testClass = tc.testClass.run();
@@ -353,187 +363,150 @@ classdef test_miSim < matlab.unittest.TestCase
             % place agents a fixed distance +/- X from the domain's center
             d = 1;
 
-            % make basic domain
-            tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
-
-            % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
             % Initialize agent collision geometry
-            dh = [0,0,-1]; % bias agent altitude from domain center
-            geometry1 = rectangularPrism;
+            tc.agents = {agent; agent; agent};
+            geometry1 = spherical;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize([tc.domain.center + dh + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize([tc.domain.center + dh - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
+            geometry3 = geometry1;
+            geometry1 = geometry1.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
+            geometry3 = geometry3.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
             
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            sensor = sensor.initialize(2.75, 9, NaN, NaN, 22.5, 9);
-            % Heterogeneous sensor model parameters
-            % sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
-
-            % Plot sensor parameters (optional)
-            % f = sensor.plotParameters();
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.domain.maxCorner(3) / 2, 9, 22.5, 9);
 
             % Initialize agents
-            tc.agents = {agent; agent};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, 3*d);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, 3*d);
-
-            % Optional third agent along the +Y axis
-            geometry3 = rectangularPrism;
-            geometry3 = geometry3.initialize([tc.domain.center + dh - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
-            tc.agents{3} = agent;
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, 3*d);
+            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{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), 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);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false);
+            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.verifyEqual(tc.testClass.partitioning(500, 500:502), [2, 3, 1]); % all three near center
+            centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
+            tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
             tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all')); % more non-assignments than partition 1 assignments
             tc.verifyLessThan(sum(tc.testClass.partitioning == 2, 'all'), sum(tc.testClass.partitioning == 1, 'all')); % more partition 1 assignments than partition 2 assignments
             tc.verifyLessThan(sum(tc.testClass.partitioning == 3, 'all'), sum(tc.testClass.partitioning == 2, 'all')); % more partition 3 assignments than partition 2 assignments
             tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
         end
         function test_single_partition(tc)
-            % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
-
-            % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2) + rand(1, 2) * 6 - 3), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
             % Initialize agent collision geometry
-            geometry1 = rectangularPrism;
-            geometry1 = geometry1.initialize([[tc.domain.center(1:2), 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2), 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
+            tc.agents = {agent};
+            geometry1 = spherical;
+            geometry1 = geometry1.initialize([tc.domain.center(1:2), 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
             
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
-
-            % Plot sensor parameters (optional)
-            % f = sensor.plotParameters();
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
 
             % Initialize agents
-            tc.agents = {agent};
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], zeros(1,3), 0, 0, geometry1, sensor, 3);
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false);
+            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);
             close(tc.testClass.fPerf);
 
             tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
             tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all'));
         end
-        function test_single_partition_basic_GA(tc)
+        function test_single_agent_gradient_ascent(tc)
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [2, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
+            tc.agents = {agent};
             geometry1 = rectangularPrism;
-            geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
             
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
-
-            % Plot sensor parameters (optional)
-            % f = sensor.plotParameters();
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
 
             % Initialize agents
-            tc.agents = {agent};
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, "", false);
+            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);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), true, false);
+            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);
             
             % Run the simulation
-            tc.testClass = tc.testClass.run();
-            if isgraphics(tc.testClass.agents{1}.debugFig)
-                close(tc.testClass.agents{1}.debugFig);
-            end
-
-            % tc.verifyGreaterThan(tc.testClass.performance(end)/max(tc.testClass.performance), 0.99); % ends up very near a relative maximum
-        end
+            tc.testClass = tc.testClass.run();end
         function test_collision_avoidance(tc)
             % No obstacles
             % Fixed agent initial conditions
             % Exaggerated large collision geometries to test CA
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = 1.5;
+            tc.agents = {agent; agent};
+            tc.collisionRanges = 1.5 * ones(size(tc.agents));
             d = [2.5, 0, 0];
             geometry1 = spherical;
             geometry2 = spherical;
-            geometry1 = geometry1.initialize(tc.domain.center + d, radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
             
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
 
             % Initialize agents
-            tc.agents = {agent; agent};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, zeros(1,3), 0, 0, geometry1, sensor, 5);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry2, sensor, 5);
+            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{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 50, cell(0, 1), tc.makeVideo, tc.makePlots);
+            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);
             
             % Run the simulation
             tc.testClass.run();
         end
         function test_obstacle_avoidance(tc)
-            % Right now this seems to prove that the communications
-            % constraints are working, but the result is dissatisfying
+            % Right now, the communications constraint is violated here
 
             % Fixed single obstacle
             % Fixed two agents initial conditions
             % Exaggerated large collision geometries
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = 1.1;
+            tc.agents = {agent; agent;};
+            tc.collisionRanges = 1.1 * ones(size(tc.agents));
             d = [3, 0, 0];
 
-            yOffset = 0;
+            yOffset = 1;
             % choice of 0 leads to the agents getting stuck attempting to go around the obstacle on both sides
             % choice of 1 leads to one agent easily going around while the other gets stuck and the communications link is broken
 
             geometry1 = spherical;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1 + yOffset, 0], radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) * 1.1 + yOffset, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
             
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            % Initialize agent sensor model with fixed parameters
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
             
             % Initialize obstacles
             obstacleLength = 1;
@@ -541,13 +514,12 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
 
             % Initialize agents
-            commsRadius = (2*radius + obstacleLength) * 0.9; % defined such that they cannot go around the obstacle on both sides
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius  *1.1 + yOffset, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-
+            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{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);
+            
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            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);
             
             % Run the simulation
             tc.testClass.run();
@@ -558,69 +530,68 @@ classdef test_miSim < matlab.unittest.TestCase
             % Negligible collision geometries
             % Non-standard domain with two objectives that will try to pull the
             % agents apart
-            l = 10; % domain size
-            dom = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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
-            dom.objective = dom.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [2, 8]) + mvnpdf([x(:), y(:)], [8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = 0.1;
+            tc.agents = {agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
             d = [1, 0, 0];
             geometry1 = spherical;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize(dom.center + d, radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(dom.center - d, radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            tc.sensor = sigmoidSensor;
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
             
             % Initialize obstacles
             tc.obstacles = {};
             
             % Initialize agents
-            commsRadius = 4; % defined such that they cannot reach their objective without breaking connectivity
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(dom.center + d, zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
+            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{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 75, tc.obstacles, true, 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);
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
         end
-        function test_obstacle_blocks_comms_LOS(tc)
+        function test_obstacle_permits_comms_LOS(tc)
             % Fixed single obstacle
             % Fixed two agents initial conditions
             % Exaggerated large communications radius
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = .25;
+            tc.agents = {agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
             d = 2;
             geometry1 = spherical;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
 
             % Initialize agents
-            commsRadius = 5;
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
+            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{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
             
             % Initialize obstacles
             obstacleLength = 1.5;
@@ -628,50 +599,55 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
+            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);
 
-            % No communications link should be established
-            tc.assertEqual(tc.testClass.adjacency, logical(eye(2)));
+            % Communications link should be established
+            tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
         end
         function test_LNA_case_1(tc)
             % based on example in meeting 
-            % No obstacles
-            % Fixed 5 agents initial conditions
-            % unitary communicaitons radius
+            % no obstacles
+            % fixed 5 agents initial conditions
+            % unit communicaitons radius
             % negligible collision radius
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = .01;
+            tc.agents = {agent; agent; agent; agent; agent;};
+            tc.collisionRanges = .01 * ones(size(tc.agents));
             d = 1;
             geometry5 = spherical;
-            geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry5.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry5.initialize(tc.domain.center, tc.collisionRanges(2), REGION_TYPE.COLLISION);
+            geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
+            geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
+            geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
 
             % Initialize agents
-            commsRadius = d;
-            tc.agents = {agent; agent; agent; agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius);
+            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{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), 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.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{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
+            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);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -688,42 +664,44 @@ classdef test_miSim < matlab.unittest.TestCase
             % unitary communicaitons radius
             % negligible collision radius
             % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "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(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
-            radius = .01;
+            tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
+            tc.collisionRanges = .01 * ones(size(tc.agents));
             d = 1;
             geometry7 = spherical;
-            geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], radius, REGION_TYPE.COLLISION);
-            geometry6 = geometry7.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], radius, REGION_TYPE.COLLISION);
+            geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
+            geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
+            geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
+            geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
+            geometry6 = geometry7.initialize(tc.domain.center, tc.collisionRanges(6), REGION_TYPE.COLLISION);
+            geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], tc.collisionRanges(7), REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
 
             % Initialize agents
-            commsRadius = d;
-            tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius);
-            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry6, sensor, commsRadius);
-            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius);
+            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{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{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{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{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{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), 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);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
+            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);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...

test/test_sigmoidSensor.m

@@ -21,7 +21,7 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
         function tc = setup(tc)
             % Reinitialize sensor with random parameters
             tc.testClass = sigmoidSensor;
-            tc.testClass = tc.testClass.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+            tc.testClass = tc.testClass.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));
         end
     end
 
@@ -34,28 +34,28 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
             alphaTilt = 15; % degrees
             betaTilt = 3;
             h = 1e-6;
-            tc.testClass = tc.testClass.initialize(alphaDist, betaDist, NaN, NaN, alphaTilt, betaTilt);
+            tc.testClass = tc.testClass.initialize(alphaDist, betaDist, alphaTilt, betaTilt);
             
             % Plot (optional)
             % tc.testClass.plotParameters();
 
             % Anticipate perfect performance for a point directly below and
             % extremely close
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [0, 0, 0]), 1, 'RelTol', 1e-3); 
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [0, 0, 0]), 1, 'RelTol', 1e-3); 
             % It looks like mu_t can max out at really low values like 0.37
             % when alphaTilt and betaTilt are small, which seems wrong
 
             % Performance at nadir point, distance alphaDist should be 1/2 exactly
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], NaN, 0, [0, 0, 0]), 1/2);
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], 0, [0, 0, 0]), 1/2);
 
             % Performance at (almost) 0 distance, alphaTilt should be 1/2
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [tand(alphaTilt)*h, 0, 0]), 1/2, 'RelTol', 1e-3);
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [tand(alphaTilt)*h, 0, 0]), 1/2, 'RelTol', 1e-3);
 
             % Performance at great distance should be 0
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], NaN, 0, [0, 0, 0]), 0, 'AbsTol', 1e-9);
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], 0, [0, 0, 0]), 0, 'AbsTol', 1e-9);
 
             % Performance at great tilt should be 0
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [5, 5, 0]), 0, 'AbsTol', 1e-9);
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], 0, [5, 5, 0]), 0, 'AbsTol', 1e-9);
         end
     end
 

util/objectiveFunctionWrapper.m

@@ -0,0 +1,15 @@
+function f = objectiveFunctionWrapper(center, sigma)
+    % Convenience function to generate MVNPDFs at a point
+    % Makes it look a lot neater to instantiate and sum these to make
+    % composite objectives in particular
+    arguments (Input)
+        center (:, 2) double;
+        sigma (2, 2) double = eye(2);
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, 'function_handle')};
+    end
+    
+    f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), 'UniformOutput', false)), 2);
+
+end