results

.gitignore

@@ -48,6 +48,7 @@ sandbox/*
 
 # Figures
 *.fig
+*.png
 
 # Python Virtual Environment
 aerpaw/venv/

@agent/initialize.m

@@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     end
 
     obj.pos = pos;
+    obj.lastPos = pos;
     obj.vel = zeros(1, 3);
     obj.lastVel = zeros(1, 3);
     obj.collisionGeometry = collisionGeometry;

@agent/run.m

@@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
         obj (1, 1) {mustBeA(obj, "agent")};
     end
 
+    % Always update lastPos/lastVel so constrainMotion evaluates barriers at
+    % the correct (most recent) position, even when this agent has no partition.
+    obj.lastPos = obj.pos;
+    if useDoubleIntegrator
+        obj.lastVel = obj.vel;
+    end
+
     % Collect objective function values across partition
     partitionMask = partitioning == index;
     if ~any(partitionMask(:))
@@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
     gradNorm = norm(gradC);
 
     % Compute unconstrained next state
-    obj.lastPos = obj.pos;
     if useDoubleIntegrator
         % Double-integrator: gradient produces desired acceleration with damping
-        obj.lastVel = obj.vel;
         if gradNorm < 1e-100
             a_gradient = zeros(1, 3);
         else

@miSim/constrainMotion.m

@@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj)
     h(logical(eye(nAgents))) = 0; % self value is 0
     for ii = 1:(nAgents - 1)
         for jj = (ii + 1):nAgents
-            h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
+            h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
             h(jj, ii) = h(ii, jj);
 
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
             A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
             % Slack derived from existing params: recovery velocity = max gradient approach velocity.
             % Correction splits between 2 agents, so |A| = 2*r_sum
@@ -69,11 +69,11 @@ function [obj] = constrainMotion(obj)
     for ii = 1:nAgents
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
+            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
 
-            hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+            hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
 
-            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos);
             % Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
             r_i = obj.agents{ii}.collisionGeometry.radius;
             v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
@@ -93,37 +93,37 @@ function [obj] = constrainMotion(obj)
     h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
     for ii = 1:nAgents
         % X minimum
-        h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y minimum
-        h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
-        h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
         b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
         b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
@@ -145,9 +145,9 @@ function [obj] = constrainMotion(obj)
             if obj.constraintAdjacencyMatrix(ii, jj)
                 paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
                 r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
-                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
+                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
 
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
 
                 % One-step forward invariance: b = h/dt ensures h cannot

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
@@ -13,6 +13,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         makeVideo (1, 1) logical = true;
         useDoubleIntegrator (1, 1) logical = false;
         dampingCoeff (1, 1) double = 2.0;
+        useFixedTopology (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "miSim")};
@@ -91,10 +92,15 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Set dynamics model
     obj.useDoubleIntegrator = useDoubleIntegrator;
     obj.dampingCoeff = dampingCoeff;
+    obj.useFixedTopology = useFixedTopology;
 
-    % Compute adjacency matrix and lesser neighbors
+    % Compute adjacency matrix and network topology
     obj = obj.updateAdjacency();
-    obj = obj.lesserNeighbor();
+    if obj.useFixedTopology
+        obj.constraintAdjacencyMatrix = obj.adjacency;
+    else
+        obj = obj.lesserNeighbor();
+    end
 
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';

@miSim/initializeFromCsv.m

@@ -90,6 +90,11 @@ if isfield(scenario, 'dampingCoeff')
 else
     DAMPING_COEFF = 2.0;
 end
+if isfield(scenario, 'useFixedTopology')
+    USE_FIXED_TOPOLOGY = logical(scenario.useFixedTopology);
+else
+    USE_FIXED_TOPOLOGY = false;
+end
 
 % ---- Build domain --------------------------------------------------------
 dom = rectangularPrism;
@@ -137,6 +142,6 @@ end
 % ---- Initialise simulation (plots and video disabled) --------------------
 obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
                      MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF);
+                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
 
 end

@miSim/miSim.m

@@ -7,7 +7,6 @@ classdef miSim
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
         domain;
-        objective;
         obstacles; % geometries that define obstacles within the domain
         agents; % agents that move within the domain
         adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -20,6 +19,7 @@ classdef miSim
         minAlt = 0; % minimum allowable altitude (m)
         useDoubleIntegrator = false; % false = single-integrator, true = double-integrator dynamics
         dampingCoeff = 2.0; % velocity-proportional damping for double-integrator mode
+        useFixedTopology = false; % false = lesser neighbor (dynamic), true = fixed initial topology
         artifactName = "";
         f; % main plotting tiled layout figure
         fPerf; % performance plot figure
@@ -66,7 +66,6 @@ classdef miSim
                 obj (1, 1) miSim
             end
             obj.domain = rectangularPrism;
-            obj.objective = sensingObjective;
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end

@miSim/run.m

@@ -30,7 +30,9 @@ function [obj] = run(obj)
         obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
         % Determine desired communications links
-        obj = obj.lesserNeighbor();
+        if ~obj.useFixedTopology
+            obj = obj.lesserNeighbor();
+        end
 
         % Moving
         % Iterate over agents to simulate their unconstrained motion

@miSim/teardown.m

@@ -13,12 +13,13 @@ function obj = teardown(obj)
 
     % Log results into matfile
     histPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist.mat"));
-    out = struct("agent", repmat(struct("pos", [], "vel", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", [], "useDoubleIntegrator", [], "dampingCoeff", []);
+    out = struct("agent", repmat(struct("pos", [], "vel", [], "perf", [], "sensor", struct("alphaDist", [], "betaDist", [], "alphaTilt", [], "betaTilt", []), "collisionRadius", [], "commsRadius", []), size(obj.agents)), "perf", [], "barriers", [], "useDoubleIntegrator", [], "dampingCoeff", [], "useFixedTopology", []);
 
     out.perf = obj.performance(1:(end - 1));
     out.barriers = [zeros(size(obj.barriers(1:end, 1), 1), 1), obj.barriers(1:end, 1:(end - 1))];
     out.dampingCoeff = obj.dampingCoeff;
     out.useDoubleIntegrator = obj.useDoubleIntegrator;
+    out.useFixedTopology = obj.useFixedTopology;
     for ii = 1:size(obj.agents, 1)
         out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
         out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
@@ -38,7 +39,6 @@ function obj = teardown(obj)
     obj.timestepIndex = NaN;
     obj.maxIter = NaN;
     obj.domain = rectangularPrism;
-    obj.objective = sensingObjective;
     obj.obstacles = cell(0, 1);
     obj.agents = cell(0, 1);
     obj.adjacency = NaN;
@@ -49,6 +49,7 @@ function obj = teardown(obj)
     obj.barrierExponent = NaN;
     obj.useDoubleIntegrator = false;
     obj.dampingCoeff = 2.0;
+    obj.useFixedTopology = false;
     obj.artifactName = "";
 
 end

@miSim/validate.m

@@ -7,11 +7,11 @@ function validate(obj)
 
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
-        warning("Network is not connected");
+        error("Network is not connected");
     end
 
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
-        warning("Eliminated network connections that were necessary");
+        error("Eliminated network connections that were necessary");
     end
 
     %% Obstacle Validators
@@ -20,10 +20,9 @@ function validate(obj)
         for kk = 1:size(obj.agents, 1)
             P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
             d = obj.agents{kk}.pos - P;
-            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
-                warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
+            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
+                error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, - dot(d, d) + obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
             end
         end
     end
-
 end

@miSim/writeInits.m

@@ -14,6 +14,9 @@ function writeInits(obj)
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
+    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
+    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
+
 
     % Combine with simulation parameters
     inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
@@ -24,7 +27,9 @@ function writeInits(obj)
                     "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
                     "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
-                    "pos", pos); % still needs obstacle states and objective state
+                    "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
+                    "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
+                    "objectiveIntegral", sum(obj.domain.objective.values(:)));
 
     % Save all parameters to output file
     initsFile = strcat(obj.artifactName, "_miSimInits");

@sensingObjective/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
     arguments (Input)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
@@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
         sensorPerformanceMinimum (1, 1) double = 1e-6;
+        objectiveMu (:, 2) double = NaN(1, 2);
+        objectiveSigma (:, 2, 2) double = NaN(1, 2, 2);
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
@@ -37,8 +39,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
 
     % store ground position
     idx = obj.values == 1;
-    obj.groundPos = [obj.X(idx), obj.Y(idx)];
-    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+    if any(isnan(objectiveMu))
+        obj.groundPos = [obj.X(idx), obj.Y(idx)];
+        obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+    else
+        obj.groundPos = objectiveMu;
+    end
+    obj.objectiveSigma = objectiveSigma;
 
-    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -11,7 +11,7 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
 
     % Set random objective position
     mu = domain.minCorner;
-    while domain.distance(mu) < protectedRange
+    while domain.distance(mu) < protectedRange * 1.01
         mu = domain.random();
     end
 

@sensingObjective/sensingObjective.m

@@ -2,7 +2,8 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundPos = [NaN, NaN];
+        groundPos = NaN(1, 2);
+        objectiveSigma = NaN(1, 2, 2);
         discretizationStep = NaN;
         X = [];
         Y = [];

aerpaw/config/scenario.csv

@@ -1,2 +1,2 @@
-timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff
-5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1

plot_1.m

@@ -0,0 +1,189 @@
+clear;
+% Load data
+dataPath = fullfile('.', 'sandbox', 'plot1');
+simHists = dir(dataPath); simHists = simHists(3:end);
+simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat'));
+simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat'));
+assert(length(simHists) == length(simInits), "input data availability mismatch");
+
+% Initialize plotting data
+nRuns = length(simHists);
+Cfinal = NaN(nRuns, 1);
+n = NaN(nRuns, 1);
+doubleIntegrator = NaN(nRuns, 1);
+numObjective = NaN(nRuns, 1);
+positions = cell(6, nRuns);
+commsRadius = NaN(nRuns, 1);
+collisionRadius = NaN(nRuns, 1);
+
+% Aggregate relevant data
+for ii = 1:length(simHists)
+    initName = strrep(simInits(ii).name, "_miSimInits.mat", "");
+    histName = strrep(simHists(ii).name, "_miSimHist.mat", "");
+    assert(initName == histName);
+
+    init = load(fullfile(simInits(ii).folder, simInits(ii).name));
+    hist = load(fullfile(simHists(ii).folder, simHists(ii).name));
+
+    % Stash relevant data
+    Cfinal(ii) = hist.out.perf(end) / init.objectiveIntegral;
+    n(ii) = init.numAgents;
+    doubleIntegrator(ii) = init.useDoubleIntegrator;
+    numObjective(ii) = size(init.objectivePos, 1);
+    commsRadius(ii) = unique(init.comRange);
+    collisionRadius(ii) = unique(init.collisionRadius);
+    for jj = 1:length(hist.out.agent)
+        alphaDist(jj, ii) = hist.out.agent(jj).sensor.alphaDist;
+        positions{jj, ii} = hist.out.agent(jj).pos;
+        assert(hist.out.agent(jj).commsRadius == commsRadius(ii));
+        assert(hist.out.agent(jj).collisionRadius == collisionRadius(ii));
+    end
+
+    alphaDist2 = unique(alphaDist);
+    if length(alphaDist2) > 1
+        alphaDist2 = alphaDist2(1);
+    end
+    if doubleIntegrator(ii) && all(alphaDist(1:n(ii), ii) == alphaDist2) && numObjective(ii) == 1
+        a2betaIdx = ii;
+        a2beta = struct("init", init, "hist", hist.out);
+    end
+end
+
+commsRadius = unique(commsRadius); assert(isscalar(commsRadius));
+collisionRadius = unique(collisionRadius); assert(isscalar(collisionRadius));
+sensors = flip(unique(alphaDist(1, :)));
+n_unique = sort(unique(n));
+nGroups = length(n_unique);
+
+% Build config label for each run
+config = strings(nRuns, 1);
+baseConfig = strings(nRuns, 1);
+for ii = 1:nRuns
+    s = "";
+    if numObjective(ii) == 1
+        s = s + "A";
+    elseif numObjective(ii) == 2
+        s = s + "B";
+    end
+    if alphaDist(1, ii) == sensors(1)
+        s = s + "_I";
+    elseif alphaDist(1, ii) == sensors(2)
+        s = s + "_II";
+    end
+    if ~doubleIntegrator(ii)
+        s = s + "_alpha";
+    else
+        s = s + "_beta";
+    end
+    baseConfig(ii) = s;
+    config(ii) = n(ii) + "_" + s;
+end
+configOrder = unique(baseConfig(n == n_unique(1)), 'stable');
+nConfigsPerN = length(configOrder);
+
+%%
+close all;
+f1 = figure;
+x1 = axes;
+
+C_mean = NaN(nGroups, nConfigsPerN);
+C_var = NaN(nGroups, nConfigsPerN);
+for ii = 1:nGroups
+    for jj = 1:nConfigsPerN
+        mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj));
+        C_mean(ii, jj) = mean(Cfinal(mask));
+        C_var(ii, jj) = var(Cfinal(mask));
+    end
+end
+
+hBar = bar(x1, C_mean);
+hold(x1, 'on');
+for jj = 1:nConfigsPerN
+    xPos = hBar(jj).XEndPoints;
+    errorbar(x1, xPos, C_mean(:, jj), C_var(:, jj), 'k.', 'LineWidth', 1, 'HandleVisibility', 'off');  % disabled the error bars because they are small to the point of meaninglessness
+end
+hold(x1, 'off');
+set(x1, 'XTickLabel', string(n_unique));
+xlabel("Number of agents");
+ylabel("Final coverage (normalized)");
+title("Final performance of parameterizations");
+legend(["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex", "Location", "northwest");
+grid("on");
+ylim([0, 1/2]);
+
+savefig(f1, "plot1.fig");
+exportgraphics(f1, "plot1.png");
+
+%%
+f2 = figure;
+x2 = axes;
+
+% Compute pairwise distances between agents
+maxPairs = nchoosek(6, 2);
+pairDist = cell(maxPairs, nRuns);
+for ii = 1:nRuns
+    pp = 0;
+    for jj = 1:n(ii)-1
+        for kk = jj+1:n(ii)
+            pp = pp + 1;
+            pairDist{pp, ii} = vecnorm(positions{jj, ii} - positions{kk, ii}, 2, 2);
+        end
+    end
+end
+
+% Cap pairwise distances at communications range
+for ii = 1:nRuns
+    nPairs = nchoosek(n(ii), 2);
+    for pp = 1:nPairs
+        pairDist{pp, ii} = min(pairDist{pp, ii}, commsRadius);
+    end
+end
+
+% Compute mean, min, max pairwise distance across all pairs and timesteps per run
+meanPairDist = NaN(nRuns, 1);
+minPairDist = NaN(nRuns, 1);
+maxPairDist = NaN(nRuns, 1);
+for ii = 1:nRuns
+    nPairs = nchoosek(n(ii), 2);
+    D = vertcat(pairDist{1:nPairs, ii});
+    meanPairDist(ii) = mean(D);
+    minPairDist(ii) = min(D);
+    maxPairDist(ii) = max(D);
+end
+
+% Group pairwise distance stats by (n, config), aggregating across reps
+meanD = NaN(nGroups, nConfigsPerN);
+minD = NaN(nGroups, nConfigsPerN);
+maxD = NaN(nGroups, nConfigsPerN);
+for ii = 1:nGroups
+    for jj = 1:nConfigsPerN
+        mask = (n == n_unique(ii)) & (baseConfig == configOrder(jj));
+        meanD(ii, jj) = mean(meanPairDist(mask));
+        minD(ii, jj) = min(minPairDist(mask));
+        maxD(ii, jj) = max(maxPairDist(mask));
+    end
+end
+
+% Plot whiskers (min to max) with mean markers
+barWidth = 0.8;
+groupWidth = barWidth / nConfigsPerN;
+hold(x2, 'on');
+for jj = 1:nConfigsPerN
+    xPos = (1:nGroups) + (jj - (nConfigsPerN + 1) / 2) * groupWidth;
+    errorbar(x2, xPos, meanD(:, jj), meanD(:, jj) - minD(:, jj), maxD(:, jj) - meanD(:, jj), ...
+        'o', 'LineWidth', 1.5, 'MarkerSize', 6, 'CapSize', 10);
+end
+hold(x2, 'off');
+set(x2, 'XTick', 1:nGroups, 'XTickLabel', string(n_unique));
+xlabel(x2, "Number of agents");
+ylabel(x2, "Pairwise distance");
+title(x2, "Pairwise Agent Distances (min/mean/max)");
+legend(x2, ["$AI\alpha$"; "$AI\beta$"; "$AII\alpha$"; "$BI\beta$"], "Interpreter", "latex");
+grid(x2, "on");
+
+yline(collisionRadius, 'r--', "Label", "Collision Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
+yline(commsRadius, 'r--', "Label", "Communications Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
+ylim([0, commsRadius + 5]);
+
+savefig(f2, "plot2.fig");
+exportgraphics(f2, "plot2.png");

plot_3.m

@@ -0,0 +1,142 @@
+clear;
+% Load data
+dataPath = fullfile('.', 'sandbox', 'plot3');
+simHists = dir(dataPath); simHists = simHists(3:end);
+simInits = simHists(endsWith({simHists.name}, 'miSimInits.mat'));
+simHists = simHists(endsWith({simHists.name}, 'miSimHist.mat'));
+assert(length(simHists) == length(simInits), "input data availability mismatch");
+assert(isscalar(simHists));
+
+init = fullfile(simInits(1).folder, simInits(1).name);
+hist = fullfile(simHists(1).folder, simHists(1).name);
+
+init = load(init);
+hist = load(hist);
+hist = hist.out;
+
+f3 = figure;
+x3 = axes;
+assert(size(init.objectivePos, 1) == 1)
+assert(hist.useDoubleIntegrator);
+
+plot(hist.perf./init.objectiveIntegral, "LineWidth", 2);
+hold("on");
+for ii = 1:length(hist.agent)
+    plot(hist.agent(ii).perf./init.objectiveIntegral, "LineWidth", 2);
+end
+grid("on");
+ylabel("Performance (normalized)");
+xlabel("Timestep");
+legend(["Cumulative"; "Agent 1"; "Agent 2"; "Agent 3"; "Agent 4"], "Location", "northwest");
+title("$AII\beta$ Performance", "Interpreter", "latex");
+
+savefig(f3, "plot3.fig");
+exportgraphics(f3, "plot3.png");
+
+f4 = figure;
+x4 = axes;
+
+% Compute pairwise distances between agents over time
+nAgents = length(hist.agent);
+commsRadius = hist.agent(1).commsRadius;
+collisionRadius = hist.agent(1).collisionRadius;
+nPairs = nchoosek(nAgents, 2);
+T = size(hist.agent(1).pos, 1);
+pairDistMat = NaN(T, nPairs);
+pp = 0;
+for jj = 1:nAgents-1
+    for kk = jj+1:nAgents
+        pp = pp + 1;
+        pairDistMat(:, pp) = vecnorm(hist.agent(jj).pos - hist.agent(kk).pos, 2, 2);
+    end
+end
+
+% Cap at communications range
+% pairDistMat = min(pairDistMat, commsRadius);
+
+% Plot all pairwise distances over time
+hold(x4, 'on');
+hLeft = gobjects(nPairs, 1);
+for pp = 1:nPairs
+    hLeft(pp) = plot(x4, pairDistMat(:, pp), 'LineWidth', 2);
+end
+yline(x4, collisionRadius, 'r--', "Label", "Collision Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
+yline(x4, commsRadius, 'r--', "Label", "Communications Radius", "LabelHorizontalAlignment", "left", "HandleVisibility", "off");
+hold(x4, 'off');
+xlabel(x4, "Timestep");
+ylabel(x4, "Pairwise distance");
+title(x4, "$AII\beta$ Pairwise Agent Distances and Barrier Function Values", "Interpreter", "latex");
+grid(x4, "on");
+ylim(x4, [0, commsRadius + 5]);
+
+% Build legend labels
+pairLabels = strings(nPairs, 1);
+pp = 0;
+for jj = 1:nAgents-1
+    for kk = jj+1:nAgents
+        pp = pp + 1;
+        pairLabels(pp) = sprintf("Agents %d-%d Distance", jj, kk);
+    end
+end
+
+l = legend(hLeft(:), pairLabels(:), "Location", "northeast");
+
+savefig(f4, "plot4_distanceOnly.fig");
+exportgraphics(f4, "plot4_distanceOnly.png");
+
+% Plot all barrier function values on right Y-axis
+nObs = init.numObstacles;
+nAA = nchoosek(nAgents, 2);
+nAO = nAgents * nObs;
+nAD = nAgents * 6;
+nComms = size(hist.barriers, 1) - nAA - nAO - nAD;
+
+yyaxis(x4, 'right');
+hold(x4, 'on');
+
+% Color palettes: pairs share colors across collision/comms,
+% agents share colors across obstacle/domain
+pairColors = lines(nAA);
+agentColors = lines(nAgents);
+
+% Row offsets in hist.barriers
+colStart = 1;
+obsStart = colStart + nAA;
+domStart = obsStart + nAO;
+comStart = domStart + nAD;
+
+% Collision + Comms barriers grouped per pair (same color)
+hRight = gobjects(0, 1);
+rightLabels = strings(0, 1);
+for pp = 1:nAA
+    hRight(end+1) = plot(x4, hist.barriers(colStart + pp - 1, :), '--', 'LineWidth', 1.5, 'Color', pairColors(pp, :));
+    rightLabels(end+1) = sprintf('h_{col} %d', pp);
+end
+for pp = 1:nComms
+    hRight(end+1) = plot(x4, hist.barriers(comStart + pp - 1, :), '-.', 'LineWidth', 1.5, 'Color', pairColors(pp, :));
+    rightLabels(end+1) = sprintf('h_{com} %d', pp);
+end
+
+% Obstacle barriers — colored by agent
+% idx = obsStart;
+% for aa = 1:nAgents
+%     for oo = 1:nObs
+%         hRight(end+1) = plot(x4, hist.barriers(idx, :), ':', 'LineWidth', 1, 'Color', agentColors(aa, :));
+%         rightLabels(end+1) = sprintf('h_{obs} a%d-o%d', aa, oo);
+%         idx = idx + 1;
+%     end
+% end
+
+hold(x4, 'off');
+ylabel(x4, "Barrier function $h$", "Interpreter", "latex");
+
+% Clamp both Y-axes to start at 0
+yyaxis(x4, 'left');  ylim(x4, [0, 25]);
+yyaxis(x4, 'right'); ylim(x4, [0, 275]);
+x4.YAxis(2).Color = 'k';
+
+% Combined legend
+l = legend([hLeft(:); hRight(:)], [pairLabels(:); rightLabels(:)], "Location", "eastoutside");
+
+savefig(f4, "plot4.fig");
+exportgraphics(f4, "plot4.png");

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8d.xml

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

resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8p.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/q1TwgWjEDIn-a93hPi1WGodL3NYd.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/q1TwgWjEDIn-a93hPi1WGodL3NYp.xml

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

resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQId.xml

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

resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQIp.xml

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

resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml

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

resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGod.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGop.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xod.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xop.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwd.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/Dt-U7DFYhio-3hILJY9YWPnda6sd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/Dt-U7DFYhio-3hILJY9YWPnda6sp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/e-EiR3xDuOjis31Xf2_ZV1uoriEd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/e-EiR3xDuOjis31Xf2_ZV1uoriEp.xml

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

resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysd.xml

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

resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysp.xml

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

test/parametricTestSuite.m

@@ -57,7 +57,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
             end
 
             % Set up simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo, logical(params.useDoubleIntegrator), params.dampingCoeff);
+            tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo, logical(params.useDoubleIntegrator), params.dampingCoeff, logical(params.useFixedTopology));
 
             % Save simulation parameters to output file
             tc.testClass.writeInits();
@@ -150,7 +150,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
                 end
 
                 % randomly shuffle agents to make the network more interesting (probably)
-                agents = agents(randperm(numel(agents))); 
+                agents = agents(randperm(numel(agents)));
 
                 % Set up obstacles
                 obstacles = cell(params.numObstacles(ii), 1);

test/results.m

@@ -0,0 +1,338 @@
+classdef results < matlab.unittest.TestCase
+    properties (Constant, Access = private)
+        seed = 1;
+        domainSize = [150, 150, 100]; % fixed domain size [X, Y, Z]
+    end
+
+    properties (Access = private)
+        % System under test
+        testClass = miSim;
+
+        %% 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
+
+        %% Scenario Reinitialization
+        % Number of extra reinitializations per test case (3 n-values x 4 configs = 12).
+        % Order: n3/A_1_alpha, n3/A_1_beta, n3/A_2_alpha, n3/B_1_beta,
+        %        n5/A_1_alpha, ..., n6/B_1_beta
+        % Set inspectScenarios = true to pause after init for manual review.
+        % At the keyboard prompt, type the number of reinits needed into
+        % the variable 'reinitCount', then 'dbcont' to continue.
+        inspectScenarios = false;
+        reinit = zeros(1, 12);
+
+        %% Fixed Test Parameters
+        useFixedTopology = true; % No lesser neighbor, fixed network instead
+        discretizationStep = 0.5;
+        protectedRange = 5;
+        collisionRadius = 5;
+        sensorPerformanceMinimum = 0.005;
+        comRange = 20;
+        maxIter = 400;
+        initialStepSize = 1;
+        % Each row: [minX minY minZ maxX maxY maxZ]
+        obstacleCorners = [results.domainSize(1)/2, results.domainSize(2)*5/8, 0, results.domainSize(1)*5/8, results.domainSize(2), 35;
+                           results.domainSize(1)/3, 0, 0, results.domainSize(1)/2, results.domainSize(2)*3/8, 40];
+        barrierGain = 1;
+        barrierExponent = 1;
+        timestep = 0.5;
+        dampingCoeff = 2;
+    end
+
+    properties (TestParameter)
+        %% Test Iterations
+        % Specific parameter combos to run iterations on
+        n = struct('n3', 3, 'n5', 5, 'n6', 6); % number of agents
+        config = results.makeConfigs();
+    end
+
+    properties (MethodSetupParameter)
+        trials = struct('r1', 1, 'r2', 2, 'r3', 3, 'r4', 4, 'r5', 5);
+    end
+
+    methods (TestMethodSetup)
+        function setSeed(tc, trials)
+            rng(tc.seed + trials);
+        end
+    end
+
+    methods (Static, Access = public)
+        function c = makeConfigs()
+            rng(results.seed);
+            abMin = 6; % alpha*beta >= 6 ensures membership(0) = tanh(3) >= 0.995
+            alphaDist = rand(1, 2) .* [75, 45];
+            betaDist = abMin ./ alphaDist + rand(1, 2) .* [1, 1/8] .* (20 - abMin ./ alphaDist);
+            alphaTilt = 10 + rand(1, 2) .* [20, 20];
+            betaTilt = abMin ./ alphaTilt + rand(1, 2) .* (50 - abMin ./ alphaTilt);
+            sensors = struct('alphaDist', num2cell(alphaDist), 'alphaTilt', num2cell(alphaTilt), 'betaDist', num2cell(betaDist), 'betaTilt', num2cell(betaTilt));
+            sensor1 = sigmoidSensor;
+            sensor2 = sigmoidSensor;
+            sensor1 = sensor1.initialize(sensors(1).alphaDist, sensors(1).betaDist, sensors(1).alphaTilt, sensors(1).betaTilt);
+            sensor2 = sensor2.initialize(sensors(2).alphaDist, sensors(2).betaDist, sensors(2).alphaTilt, sensors(2).betaTilt);
+            sensor1.plotParameters;
+            sensor2.plotParameters;
+            c = struct('A_1_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', false), ...
+                        'A_1_beta',  struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(1), 'doubleIntegrator', true), ...
+                        'A_2_alpha', struct('objectivePos', [3, 1] / 4 .* results.domainSize(1:2), 'sensor', sensors(2), 'doubleIntegrator', false), ...
+                        'B_1_beta',  struct('objectivePos', [[3, 1] / 4 .* results.domainSize(1:2); [3, 1] / 4 .* results.domainSize(1:2) + 25 .* [-1, 1] ./ sqrt(2)], 'sensor', sensors(1), 'doubleIntegrator', true));
+        end
+    end
+
+    methods (Test)
+        function plot1_runs(tc, n, config)
+            % if n == 5 && config.doubleIntegrator == true
+            %     tc.makePlots = true;
+            % else
+            %     tc.makePlots = false;
+            % end
+            % Compute test case index for reinit lookup
+            nKeys = fieldnames(tc.n);
+            configKeys = fieldnames(tc.config);
+            nIdx = find(cellfun(@(k) tc.n.(k) == n, nKeys));
+            configIdx = find(cellfun(@(k) isequal(tc.config.(k), config), configKeys));
+            testIdx = (nIdx - 1) * numel(configKeys) + configIdx;
+
+            % Determine number of reinitializations
+            reinitCount = tc.reinit(testIdx);
+
+            for reroll = 0:reinitCount
+
+            % Set up fixed-size domain
+            minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3);
+            % Place sensing objective(s) at fixed positions from config
+            objectiveMu = config.objectivePos;
+            numDist = size(objectiveMu, 1);
+            objectiveSigma = [];
+            for ii = 1:numDist
+                sig = [200, 140; 140, 280];
+                if ~mod(ii, 2)
+                    sig = rot90(sig,2);
+                end
+                sig = reshape(sig, [1, 2, 2]);
+                objectiveSigma = cat(1, objectiveSigma, sig);
+            end
+            tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain");
+            tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
+            
+            % Initialize agents
+            agents = cell(n, 1);
+            [agents{:}] = deal(agent);
+
+            % Initialize sensor model
+            sensorModel = sigmoidSensor;
+            sensorModel = sensorModel.initialize(config.sensor.alphaDist, config.sensor.betaDist, config.sensor.alphaTilt, config.sensor.betaTilt);
+
+            % Initialize fixed obstacles from corner coordinates
+            nObs = size(tc.obstacleCorners, 1);
+            obstacles = cell(nObs, 1);
+            for jj = 1:nObs
+                corners = [tc.obstacleCorners(jj, 1:3); tc.obstacleCorners(jj, 4:6)];
+                obstacles{jj} = rectangularPrism;
+                obstacles{jj} = obstacles{jj}.initialize(corners, REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", jj));
+            end
+
+            % Place agents in small-x, large-y quadrant (opposite objectives)
+            % with chain topology: each agent connected only to its neighbors
+            midXY = (tc.testClass.domain.minCorner(1:2) + tc.testClass.domain.maxCorner(1:2)) / 2;
+            quadrantSize = tc.testClass.domain.maxCorner(1:2) / 2;
+            margin = quadrantSize / 6;
+            agentBounds = [tc.testClass.domain.minCorner(1) + margin(1), ...
+                           midXY(2) + margin(2); ...
+                           midXY(1) - margin(1), ...
+                           tc.testClass.domain.maxCorner(2) - margin(2)];
+            % Find a fixed altitude where sensor performance passes at ALL
+            % corners of the placement bounds (worst-case XY)
+            corners = [agentBounds(1,1), agentBounds(1,2);
+                       agentBounds(2,1), agentBounds(1,2);
+                       agentBounds(1,1), agentBounds(2,2);
+                       agentBounds(2,1), agentBounds(2,2)];
+            agentAlt = tc.testClass.domain.maxCorner(3) - tc.collisionRadius;
+            while agentAlt > minAlt + 2 * tc.collisionRadius
+                worstPerf = inf;
+                for cc = 1:4
+                    p = sensorModel.sensorPerformance([corners(cc,:), agentAlt], [corners(cc,:), 0]);
+                    worstPerf = min(worstPerf, p);
+                end
+                if worstPerf >= tc.sensorPerformanceMinimum * 10
+                    break;
+                end
+                agentAlt = agentAlt - 1;
+            end
+            chainSpacingMin = 0.7 * tc.comRange;
+            chainSpacingMax = 0.9 * tc.comRange;
+            collisionGeometry = spherical;
+            for jj = 1:n
+                retry = true;
+                while retry
+                    retry = false;
+
+                    if jj == 1
+                        % First agent: random XY within bounds, fixed altitude
+                        agentPos = [agentBounds(1, :) + (agentBounds(2, :) - agentBounds(1, :)) .* rand(1, 2), agentAlt];
+                    else
+                        % Place at 0.7-0.9 * comRange in XY from previous agent, same altitude
+                        dir = randn(1, 2);
+                        dir = dir / norm(dir);
+                        r = chainSpacingMin + rand * (chainSpacingMax - chainSpacingMin);
+                        agentPos = [agents{jj-1}.pos(1:2) + r * dir, agentAlt];
+                    end
+
+                    % Check within placement bounds (XY only, Z is fixed)
+                    if any(agentPos(1:2) <= agentBounds(1, :)) || any(agentPos(1:2) >= agentBounds(2, :))
+                        retry = true;
+                        continue;
+                    end
+
+                    % Check sensor performance threshold; lower altitude if it fails
+                    if sensorModel.sensorPerformance(agentPos, [agentPos(1:2), 0]) < tc.sensorPerformanceMinimum * 10
+                        agentAlt = max(agentAlt - tc.collisionRadius, minAlt + 1.1 * tc.collisionRadius);
+                        agentPos(3) = agentAlt;
+                        % If we've hit the floor and still failing, widen XY search
+                        if agentAlt <= minAlt + 2 * tc.collisionRadius
+                            agentBounds = [tc.testClass.domain.minCorner(1) + tc.collisionRadius, ...
+                                           tc.testClass.domain.minCorner(2) + tc.collisionRadius; ...
+                                           tc.testClass.domain.maxCorner(1) - tc.collisionRadius, ...
+                                           tc.testClass.domain.maxCorner(2) - tc.collisionRadius];
+                        end
+                        retry = true;
+                        continue;
+                    end
+
+                    % Must be within comRange of previous agent (chain link)
+                    if jj > 1 && norm(agents{jj-1}.pos - agentPos) >= tc.comRange
+                        retry = true;
+                        continue;
+                    end
+
+                    % Must be BEYOND comRange of all non-adjacent agents (sparsity)
+                    % for kk = 1:(jj - 2)
+                    %     if norm(agents{kk}.pos - agentPos) < tc.comRange
+                    %         retry = true;
+                    %         break;
+                    %     end
+                    % end
+                    % if retry, continue; end
+
+                    % No collision with any existing agent
+                    for kk = 1:(jj - 1)
+                        if norm(agents{kk}.pos - agentPos) < agents{kk}.collisionGeometry.radius + tc.collisionRadius
+                            retry = true;
+                            break;
+                        end
+                    end
+                    if retry, continue; end
+
+                    % No collision with any obstacle
+                    for kk = 1:nObs
+                        P = min(max(agentPos, obstacles{kk}.minCorner), obstacles{kk}.maxCorner);
+                        d = agentPos - P;
+                        if dot(d, d) <= tc.collisionRadius^2
+                            retry = true;
+                            break;
+                        end
+                    end
+                end
+
+                % Initialize agent
+                collisionGeometry = collisionGeometry.initialize(agentPos, tc.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj));
+                agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, tc.comRange, tc.maxIter, tc.initialStepSize, sprintf("Agent %d", jj), tc.plotCommsGeometry);
+            end
+
+            % Randomly shuffle agents to vary index-based topology
+            agents = agents(randperm(numel(agents)));
+
+            end % reroll loop
+
+            % Inspect scenario if enabled
+            if tc.inspectScenarios
+                tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+                fprintf("Test %d (n=%d, config=%s): reinit=%d. Inspect plot.\n", testIdx, n, configKeys{configIdx}, reinitCount);
+                fprintf("To add reinits, update tc.reinit(%d) and rerun.\n", testIdx);
+                keyboard;
+                tc.testClass = tc.testClass.teardown();
+                return;
+            end
+
+            % Set up simulation
+            tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, tc.maxIter, obstacles, tc.makePlots, tc.makeVideo, config.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            
+            % Save simulation parameters to output file
+            tc.testClass.writeInits();
+
+            % Run
+            tc.testClass = tc.testClass.run();
+
+            % Cleanup
+            tc.testClass = tc.testClass.teardown();
+            close all;
+        end
+        function AIIbeta_plots_3_4(tc)
+            % test-specific parameters
+            tc.makePlots = true;
+            tc.makeVideo = true;
+            maxIters = 400;
+
+            configs = results.makeConfigs();
+            c = configs.A_2_alpha;
+            c.doubleIntegrator = true; % make a2alpha into a2beta
+
+            % Set up fixed-size domain
+            minAlt = tc.domainSize(3)/10 + rand * 1/10 * tc.domainSize(3);
+            tc.testClass.domain = tc.testClass.domain.initialize([zeros(1, 3); tc.domainSize], REGION_TYPE.DOMAIN, "Domain");
+            
+            % Set objective
+            objectiveMu = [tc.domainSize(1) * 2 / 3, tc.domainSize(2) * 3 / 4];
+            objectiveSigma = reshape([215, 100; 100, 175], [1, 2, 2]);
+            tc.testClass.domain.objective = tc.testClass.domain.objective.initialize(objectiveFunctionWrapper(objectiveMu, objectiveSigma), tc.testClass.domain, tc.discretizationStep, tc.protectedRange, tc.sensorPerformanceMinimum, objectiveMu, objectiveSigma);
+            
+            % Set agent initial states (fully connected network of 4)
+            centerPos = [tc.domainSize(1) / 4, tc.domainSize(2) / 4];
+            d = tc.collisionRadius + (tc.comRange - tc.collisionRadius) / 4;
+            agentsPos = centerPos + [1, 1; 1, -1; -1, -1; -1, 1] / sqrt(2) * d;
+            agentAlt = minAlt * 1.5;
+            agentsPos = [agentsPos, agentAlt * ones(4, 1) + rand * 5 - 2.5];
+
+            agents = {agent, agent, agent, agent};
+            cg = spherical;
+            sensorModel = sigmoidSensor;
+            sensorModel = sensorModel.initialize(c.sensor.alphaDist, c.sensor.betaDist, c.sensor.alphaTilt, c.sensor.betaTilt);
+            agents{1} = agents{1}.initialize(agentsPos(1, :), cg.initialize(agentsPos(1, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 1 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 1", false);
+            agents{2} = agents{2}.initialize(agentsPos(2, :), cg.initialize(agentsPos(2, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 2 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 2", false);
+            agents{3} = agents{3}.initialize(agentsPos(3, :), cg.initialize(agentsPos(3, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 3 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 3", false);
+            agents{4} = agents{4}.initialize(agentsPos(4, :), cg.initialize(agentsPos(4, :), tc.collisionRadius, REGION_TYPE.COLLISION, "Agent 4 Collision Geometry"), sensorModel, tc.comRange, maxIters, tc.initialStepSize, "Agent 4", false);
+            
+            obstacles = cell(1, 1);
+            obstacles{1} = rectangularPrism;
+            obstacles{1} = obstacles{1}.initialize([0, tc.domainSize(2)/2, 0; tc.domainSize(1) * 0.4, tc.domainSize(2), 40],REGION_TYPE.OBSTACLE, "Obstacle 1");
+
+            % Set up simulation
+            tc.testClass = tc.testClass.initialize(tc.testClass.domain, agents, tc.barrierGain, tc.barrierExponent, minAlt, tc.timestep, maxIters, obstacles, tc.makePlots, tc.makeVideo, c.doubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+            
+            % Save simulation parameters to output file
+            tc.testClass.writeInits();
+
+            % Run
+            tc.testClass = tc.testClass.run();
+
+            % Cleanup
+            tc.testClass = tc.testClass.teardown();
+        end
+    end
+
+    methods
+        function c = obstacleCollisionCheck(~, obstacles, obstacle)
+            % Check if the obstacle intersects with any other obstacles
+            c = false;
+            for ii = 1:size(obstacles, 1)
+                if geometryIntersects(obstacles{ii}, obstacle)
+                    c = true;
+                    return;
+                end
+            end
+        end
+
+    end
+end

test/test_miSim.m

@@ -33,6 +33,8 @@ classdef test_miSim < matlab.unittest.TestCase
         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
+        useDoubleIntegrator = false;
+        dampingCoeff = 2;
         agents = cell(0, 1);
         
         % Collision
@@ -52,6 +54,7 @@ classdef test_miSim < matlab.unittest.TestCase
         sensor = sigmoidSensor;
 
         % Communications
+        useFixedTopology = false;
         minCommsRange = 3; % Minimum randomly generated collision geometry size
         maxCommsRange = 5; % Maximum randomly generated collision geometry size
         commsRanges = NaN;
@@ -224,7 +227,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
         end
         function miSim_run(tc)
             % randomly create obstacles
@@ -363,7 +366,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
 
             % Write out initialization state
             tc.testClass.writeInits();
@@ -397,7 +400,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
         
             centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
             tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
@@ -422,7 +425,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
             close(tc.testClass.fPerf);
 
             tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
@@ -450,7 +453,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
             
             % Run the simulation
             tc.testClass = tc.testClass.run();end
@@ -485,7 +488,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize the simulation
             tc.obstacles = cell(0, 1);
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
             
             % Run the simulation
             tc.testClass.run();
@@ -531,7 +534,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, 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, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
             
             % Run the simulation
             tc.testClass.run();
@@ -571,7 +574,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, 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, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
@@ -614,7 +617,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
 
             % Communications link should be established
             tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
@@ -659,7 +662,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -713,7 +716,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.minAlt = 0;
             tc.makePlots = false;
             tc.makeVideo = false;
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...

util/objectiveFunctionWrapper.m

@@ -4,12 +4,12 @@ function f = objectiveFunctionWrapper(center, sigma)
     % composite objectives in particular
     arguments (Input)
         center (:, 2) double;
-        sigma (2, 2) double = eye(2);
+        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);
+    assert(size(center, 1) == size(sigma, 1));
+    f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
 
 end