results

plot1 for multiple trials
plots 3 and 4
2026-03-17 22:15:42 -07:00 · 2026-03-17 12:21:14 -07:00 · 2026-03-16 19:22:31 -07:00 · 2026-03-16 16:19:38 -07:00 · 2026-03-16 14:35:52 -07:00 · 2026-03-15 17:43:45 -07:00
80 changed files with 1337 additions and 140 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -48,6 +48,7 @@ sandbox/*

 # Figures
 *.fig
+*.png

 # Python Virtual Environment
 aerpaw/venv/
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +0,0 @@
-[submodule "aerpaw/aerpawlib"]
-	path = aerpaw/aerpawlib
-	url = https://github.com/morzack/aerpawlib-vehicle-control.git
--- a/@agent/agent.m
+++ b/@agent/agent.m
@@ -6,6 +6,8 @@ classdef agent
        % State
        lastPos = NaN(1, 3); % position from previous timestep
        pos = NaN(1, 3); % current position
+        vel = zeros(1, 3); % velocity (double-integrator mode)
+        lastVel = zeros(1, 3); % pre-step velocity (double-integrator mode)

        % Sensor
        sensorModel;
--- a/@agent/initialize.m
+++ b/@agent/initialize.m
@@ -15,6 +15,9 @@ 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;
    obj.sensorModel = sensorModel;
    obj.label = label;
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -1,4 +1,4 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
    arguments (Input)
        obj (1, 1) {mustBeA(obj, "agent")};
        domain (1, 1) {mustBeGeometry};
@@ -6,11 +6,21 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
        timestepIndex (1, 1) double;
        index (1, 1) double;
        agents (:, 1) {mustBeA(agents, "cell")};
+        useDoubleIntegrator (1, 1) logical = false;
+        dampingCoeff (1, 1) double = 2.0;
+        dt (1, 1) double = 1.0;
    end
    arguments (Output)
        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(:))
@@ -75,20 +85,25 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
    gradNorm = norm(gradC);

-    % Compute unconstrained next position.
-    % Guard against near-zero gradient: when sensor performance is saturated
-    % or near-zero across the whole partition, rateFactor -> Inf and pNext
-    % explodes. Stay put instead.
-    if gradNorm < 1e-100
-        pNext = obj.pos;
+    % Compute unconstrained next state
+    if useDoubleIntegrator
+        % Double-integrator: gradient produces desired acceleration with damping
+        if gradNorm < 1e-100
+            a_gradient = zeros(1, 3);
+        else
+            % Scale so steady-state step ≈ targetRate (matching SI behavior)
+            a_gradient = (targetRate * dampingCoeff / (gradNorm * dt)) * gradC;
+        end
+        % Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
+        obj.vel = (obj.vel + a_gradient * dt) / (1 + dampingCoeff * dt);
+        obj.pos = obj.lastPos + obj.vel * dt;
    else
-        pNext = obj.pos + (targetRate / gradNorm) * gradC;
+        % Single-integrator: gradient directly sets position step
+        if gradNorm >= 1e-100
+            obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
+        end
    end

-    % Move to next position
-    obj.lastPos = obj.pos;
-    obj.pos = pNext;
-
    % Reinitialize collision geometry in the new position
    d = obj.pos - obj.collisionGeometry.center;
    if isa(obj.collisionGeometry, "rectangularPrism")
--- a/@miSim/constrainMotion.m
+++ b/@miSim/constrainMotion.m
@@ -8,41 +8,41 @@ function [obj] = constrainMotion(obj)

    nAgents = size(obj.agents, 1);

-    if nAgents < 2
-        nAAPairs = 0;
-    else
-        nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
-    end
-
-    % Compute velocity matrix from unconstrained gradient-ascent step
-    v = zeros(nAgents, 3);
+    % Compute current velocity and desired control input
+    v = zeros(nAgents, 3);        % current velocity (for drift term in DI mode)
+    u_desired = zeros(nAgents, 3); % desired control: velocity (SI) or acceleration (DI)
    for ii = 1:nAgents
-        v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
+        if obj.useDoubleIntegrator
+            v(ii, :) = obj.agents{ii}.lastVel;
+            u_desired(ii, :) = (obj.agents{ii}.vel - obj.agents{ii}.lastVel) / obj.timestep;
+        else
+            v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
+            u_desired(ii, :) = v(ii, :);
+        end
    end
-    if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all")
-        % Agents are not attempting to move, so there is no motion to be
-        % constrained
+    if ~obj.useDoubleIntegrator && (all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all"))
+        % Single-integrator: agents are not attempting to move
+        return;
+    end
+    if obj.useDoubleIntegrator && all(u_desired == 0, "all") && all(v == 0, "all")
+        % Double-integrator: no desired acceleration and no existing velocity
        return;
    end

    % Initialize QP based on number of agents and obstacles
-    nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
-    nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
-    nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
-    total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
    kk = 1;
-    A = zeros(total, 3 * nAgents);
-    b = zeros(total, 1);
+    A = zeros(obj.numBarriers, 3 * nAgents);
+    b = zeros(obj.numBarriers, 1);

    % Set up collision avoidance constraints
    h = NaN(nAgents, nAgents);
    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
@@ -60,16 +60,20 @@ function [obj] = constrainMotion(obj)
        end
    end

+    idx = length(h(triu(true(size(h)), 1)));
+    obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
+    idx = idx + 1;
+
    hObs = NaN(nAgents, size(obj.obstacles, 1));
    % Set up obstacle avoidance constraints
    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;
@@ -80,46 +84,52 @@ function [obj] = constrainMotion(obj)
        end
    end

+    obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+    idx = idx + numel(hObs);
+
    % Set up domain constraints (walls and ceiling only)
    % Floor constraint is implicit with an obstacle corresponding to the
    % minimum allowed altitude, but I included it anyways
    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;
+
+        obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
+        idx = idx + 6;
    end

    if coder.target('MATLAB')
@@ -133,21 +143,41 @@ function [obj] = constrainMotion(obj)
    for ii = 1:(nAgents - 1)
        for jj = (ii + 1):nAgents
            if obj.constraintAdjacencyMatrix(ii, jj)
-                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
+                paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
+                r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
+                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;

-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.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));
-                b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent;
+
+                % One-step forward invariance: b = h/dt ensures h cannot
+                % go negative in a single timestep (linear approximation)
+                v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
+                hMin = -4 * r_comms * v_max_ij * obj.timestep;
+                if norm(A(kk, :)) < 1e-9
+                    b(kk) = 0;
+                else
+                    b(kk) = max(hMin, hComms(ii, jj)) / obj.timestep;
+                end

                kk = kk + 1;
            end
        end
    end
+    obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));

-    % Solve QP program generated earlier
-    vhat = reshape(v', 3 * nAgents, 1);
+    % Double-integrator: transform QP from velocity to acceleration space.
+    % Single-integrator constraint: A * v <= b
+    % Double-integrator: A * a <= (b - A * v_current) / dt
+    if obj.useDoubleIntegrator
+        v_flat = reshape(v', 3 * nAgents, 1);
+        b = (b - A * v_flat) / obj.timestep;
+    end
+
+    % Solve QP: minimize ||u - u_desired||²
+    uhat = reshape(u_desired', 3 * nAgents, 1);
    H = 2 * eye(3 * nAgents);
-    f = -2 * vhat;
+    f = -2 * uhat;

    % Update solution based on constraints
    if coder.target('MATLAB')
@@ -157,8 +187,8 @@ function [obj] = constrainMotion(obj)
    end
    opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
    x0 = zeros(size(H, 1), 1);
-    [vNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
-    vNew = reshape(vNew, 3, nAgents)';
+    [uNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
+    uNew = reshape(uNew, 3, nAgents)';

    if exitflag < 0
        % Infeasible or other hard failure: hold all agents at current positions
@@ -167,9 +197,9 @@ function [obj] = constrainMotion(obj)
        else
            fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
        end
-        vNew = zeros(nAgents, 3);
+        uNew = zeros(nAgents, 3);
    elseif exitflag == 0
-        % Max iterations exceeded: use suboptimal solution already in vNew
+        % Max iterations exceeded: use suboptimal solution already in uNew
        if coder.target('MATLAB')
            warning("QP max iterations exceeded, using suboptimal solution.");
        else
@@ -177,10 +207,16 @@ function [obj] = constrainMotion(obj)
        end
    end

-    % Update the "next position" that was previously set by unconstrained
-    % GA using the constrained solution produced here
-    for ii = 1:size(vNew, 1)
-        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
+    % Update agent state using the constrained control input
+    for ii = 1:size(uNew, 1)
+        if obj.useDoubleIntegrator
+            % uNew is constrained acceleration
+            obj.agents{ii}.vel = obj.agents{ii}.lastVel + uNew(ii, :) * obj.timestep;
+            obj.agents{ii}.pos = obj.agents{ii}.lastPos + obj.agents{ii}.vel * obj.timestep;
+        else
+            % uNew is constrained velocity
+            obj.agents{ii}.pos = obj.agents{ii}.lastPos + uNew(ii, :) * obj.timestep;
+        end
    end

    % Here we run this at the simulation level, but in reality there is no
--- a/@miSim/initialize.m
+++ b/@miSim/initialize.m
@@ -1,4 +1,4 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
+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};
@@ -11,6 +11,9 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
        obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
        makePlots(1, 1) logical = true;
        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")};
@@ -86,9 +89,18 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
    obj.barrierExponent = barrierExponent;
    obj.minAlt = minAlt;

-    % Compute adjacency matrix and lesser neighbors
+    % Set dynamics model
+    obj.useDoubleIntegrator = useDoubleIntegrator;
+    obj.dampingCoeff = dampingCoeff;
+    obj.useFixedTopology = useFixedTopology;
+
+    % 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)';
@@ -104,11 +116,28 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
    % Create initial partitioning
    obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);

+    % Determine number of barrier functions that will be necessary
+    if size(obj.agents, 1) < 2
+        nAAPairs = 0;
+    else
+        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
+    end
+    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
+    nADPairs = size(obj.agents, 1) * 6; % agents x (4 walls + 1 floor + 1 ceiling)
+    nLNAPairs = sum(triu(obj.constraintAdjacencyMatrix, 1), "all");
+    obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
+
    if coder.target('MATLAB')
        % Initialize variable that will store agent positions for trail plots
        obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
        obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);

+        % Initialize velocity history (zeros at t=0, all agents start at rest)
+        obj.velHist = zeros(size(obj.agents, 1), obj.maxIter + 1, 3);
+
+        % Initialize variable that will store barrier function values per timestep for analysis purposes
+        obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
+
        % Set up plots showing initialized state
        obj = obj.plot();

--- a/@miSim/initializeFromCsv.m
+++ b/@miSim/initializeFromCsv.m
@@ -79,6 +79,23 @@ assert(numel(BETA_TILT_VEC) == numAgents, ...

 numObstacles = scenario.numObstacles;

+% Dynamics model (optional columns — backward compatible with older CSVs)
+if isfield(scenario, 'useDoubleIntegrator')
+    USE_DOUBLE_INTEGRATOR = logical(scenario.useDoubleIntegrator);
+else
+    USE_DOUBLE_INTEGRATOR = false;
+end
+if isfield(scenario, 'dampingCoeff')
+    DAMPING_COEFF = 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;
 dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
@@ -124,6 +141,7 @@ end

 % ---- Initialise simulation (plots and video disabled) --------------------
 obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
-                     MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
+                     MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
+                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);

 end
--- a/@miSim/miSim.m
+++ b/@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
@@ -18,11 +17,16 @@ classdef miSim
        barrierGain = NaN; % CBF gain parameter
        barrierExponent = NaN; % CBF exponent parameter
        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
        % Indicies for various plot types in the main tiled layout figure
        spatialPlotIndices = [6, 4, 3, 2];
+        numBarriers = 0; % Number of barrier functions needed
+        barriers = []; % log barrier function values at each timestep for analysis
    end

    properties (Access = private)
@@ -40,6 +44,7 @@ classdef miSim
        performancePlot; % objects for sensor performance plot

        posHist; % data for trail plot
+        velHist; % velocity history (double-integrator mode)
        trailPlot; % objects for agent trail plot

        % Indicies for various plot types in the main tiled layout figure
@@ -61,7 +66,6 @@ classdef miSim
                obj (1, 1) miSim
            end
            obj.domain = rectangularPrism;
-            obj.objective = sensingObjective;
            obj.obstacles = {rectangularPrism};
            obj.agents = {agent};
        end
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -30,12 +30,14 @@ 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
        for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep);
        end

        % Adjust motion determined by unconstrained gradient ascent using
@@ -43,8 +45,9 @@ function [obj] = run(obj)
        obj = constrainMotion(obj);

        if coder.target('MATLAB')
-            % Update agent position history array
+            % Update agent position and velocity history arrays
            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);
+            obj.velHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.vel, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);

            % Update total performance
            obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
@@ -63,10 +66,12 @@ function [obj] = run(obj)
        end
    end

+    % Close video
    if coder.target('MATLAB')
        if obj.makeVideo
            % Close video file
            v.close();
        end
    end
+    
 end
--- a/@miSim/teardown.m
+++ b/@miSim/teardown.m
@@ -6,17 +6,39 @@ function obj = teardown(obj)
        obj (1, 1) {mustBeA(obj, "miSim")};
    end

-    % Close plots
-    close(obj.hf);
-    close(obj.fPerf);
-    close(obj.f);
+    % % Close plots
+    % close(obj.hf);
+    % close(obj.fPerf);
+    % close(obj.f);
+
+    % 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", [], "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));
+        out.agent(ii).perf = obj.agents{ii}.performance(1:(end - 2));
+        out.agent(ii).sensor.alphaDist = obj.agents{ii}.sensorModel.alphaDist;
+        out.agent(ii).sensor.betaDist = obj.agents{ii}.sensorModel.betaDist;
+        out.agent(ii).sensor.alphaTilt = obj.agents{ii}.sensorModel.alphaTilt;
+        out.agent(ii).sensor.betaTilt = obj.agents{ii}.sensorModel.betaTilt;
+        out.agent(ii).collisionRadius = obj.agents{ii}.collisionGeometry.radius;
+        out.agent(ii).commsRadius = obj.agents{ii}.commsGeometry.radius;
+    end
+
+    save(histPath, "out");

    % reset parameters
    obj.timestep = NaN;
    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;
@@ -25,6 +47,9 @@ function obj = teardown(obj)
    obj.performance = 0;
    obj.barrierGain = NaN;
    obj.barrierExponent = NaN;
+    obj.useDoubleIntegrator = false;
+    obj.dampingCoeff = 2.0;
+    obj.useFixedTopology = false;
    obj.artifactName = "";

 end
--- a/@miSim/validate.m
+++ b/@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
--- a/@miSim/writeInits.m
+++ b/@miSim/writeInits.m
@@ -14,6 +14,8 @@ 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
@@ -21,10 +23,13 @@ function writeInits(obj)
                    "discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ...
                    "sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ...
                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
-                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, "alphaDist", alphaDist, ...
-                    "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
+                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
+                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
                    ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
-                    "pos", pos);
+                    "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");
--- a/@sensingObjective/initialize.m
+++ b/@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
--- a/@sensingObjective/initializeRandomMvnpdf.m
+++ b/@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

--- a/@sensingObjective/sensingObjective.m
+++ b/@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 = [];
--- a/aerpaw/aerpawlib
+++ b/aerpaw/aerpawlib
--- a/aerpaw/config/client1.yaml
+++ b/aerpaw/config/client1.yaml
@@ -12,8 +12,8 @@ tdm:

 # ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
 origin:
-  lat: 35.72550610629396
-  lon: -78.70019657805574
+  lat: 35.72595214250436
+  lon: -78.69917609299937
  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
 # Environment-specific settings
 environments:
@@ -32,7 +32,7 @@ environments:
    mavlink:
      ip: "192.168.32.26"
      port: 14550
-    # Controller runs on host machine (192.168.122.1 from E-VM perspective)
+    # Controller runs on host machine (192.168.109.1 from E-VM perspective)
    controller:
-      ip: "192.168.122.1"
+      ip: "192.168.109.1"
      port: 5000
--- a/aerpaw/config/client2.yaml
+++ b/aerpaw/config/client2.yaml
@@ -12,8 +12,8 @@ tdm:

 # ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
 origin:
-  lat: 35.72550610629396
-  lon: -78.70019657805574
+  lat: 35.72595214250436
+  lon: -78.69917609299937
  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
 # Environment-specific settings
 environments:
@@ -32,7 +32,7 @@ environments:
    mavlink:
      ip: "192.168.32.26"
      port: 14550
-    # Controller runs on host machine (192.168.122.1 from E-VM perspective)
+    # Controller runs on host machine (192.168.109.1 from E-VM perspective)
    controller:
-      ip: "192.168.122.1"
+      ip: "192.168.109.1"
      port: 5000
--- a/aerpaw/config/scenario.csv
+++ b/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
-5, 120, 30.0, 0.1, 1.0, 2.0, 100, 3, "3.0, 3.0", "30.0, 30.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "50.0, 50.0, 80.0", "35.0, 35.0", "10, 5, 5, 10", 0.15, "5.0, 10.0, 45.0, 15.0, 10.0, 35.0", 1, "2.0, 15.0, 0.0", "25.0, 25.0, 50.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
--- a/aerpaw/controller.coderprj
+++ b/aerpaw/controller.coderprj
@@ -133,6 +133,11 @@
                  <Size type="coderapp.internal.codertype.Dimension"/>
                  <Size type="coderapp.internal.codertype.Dimension"/>
                </Types>
+                <Types id="27" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
              </Types>
            </coderapp.internal.interface.project.Interface>
          </MF0>
@@ -1094,7 +1099,7 @@
                </Artifacts>
                <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                <Success>true</Success>
-                <Timestamp>2026-03-03T19:58:03</Timestamp>
+                <Timestamp>2026-03-11T17:11:03</Timestamp>
              </MainBuildResult>
            </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
          </MF0>
--- a/aerpaw/impl/controller_main.cpp
+++ b/aerpaw/impl/controller_main.cpp
@@ -1,12 +1,17 @@
 #include <iostream>
-#include "controller.h" 
+#include "controller.h"
 #include "controller_impl.h" // TCP implementation header

 int main() {
-    // Number of clients to handle
-    int numClients = 2;  // for now
-
-    std::cout << "Initializing TCP server...\n";
+    // Derive numClients from initialPositions in scenario.csv
+    double targets[MAX_CLIENTS_PER_PARAM * 3];
+    int numClients = loadInitialPositions("config/scenario.csv",
+                                          targets, MAX_CLIENTS_PER_PARAM);
+    if (numClients < 1) {
+        std::cerr << "Failed to parse numClients from scenario.csv\n";
+        return 1;
+    }
+    std::cout << "Parsed " << numClients << " UAV(s) from scenario.csv\n";

    // Call MATLAB-generated server function
    controller(numClients);
--- a/aerpaw/radio/CSwSNRRX.py
+++ b/aerpaw/radio/CSwSNRRX.py
@@ -267,6 +267,10 @@ class CSwSNRRX(gr.top_block):
            '/root/Quality', num_uavs, slot_duration, guard_interval)
        self.blocks_file_sink_0 = TdmTaggedFileSink(
            '/root/Power', num_uavs, slot_duration, guard_interval)
+        self.blocks_file_sink_noisefloor = TdmTaggedFileSink(
+            '/root/NoiseFloor', num_uavs, slot_duration, guard_interval)
+        self._freqoffset_file = open('/root/FreqOffset', 'w')
+        self._freqoffset_file.write('tx_uav_id,value\n')
        self.blocks_divide_xx_0 = blocks.divide_ff(1)
        self.blocks_complex_to_real_0_0 = blocks.complex_to_real(1)
        self.blocks_complex_to_real_0 = blocks.complex_to_real(1)
@@ -310,6 +314,7 @@ class CSwSNRRX(gr.top_block):
        self.connect((self.blocks_nlog10_ff_0_0, 0), (self.blocks_add_const_vxx_0, 0))
        self.connect((self.blocks_nlog10_ff_0_0, 0), (self.blocks_sub_xx_0, 0))
        self.connect((self.blocks_nlog10_ff_0_0_0, 0), (self.blocks_sub_xx_0, 1))
+        self.connect((self.blocks_nlog10_ff_0_0_0, 0), (self.blocks_file_sink_noisefloor, 0))
        self.connect((self.blocks_stream_to_vector_0_0, 0), (self.epy_block_0, 0))
        self.connect((self.blocks_sub_xx_0, 0), (self.blocks_file_sink_0_0_0, 0))
        self.connect((self.blocks_vector_to_stream_0_0, 0), (self.blocks_keep_m_in_n_0, 0))
@@ -321,6 +326,26 @@ class CSwSNRRX(gr.top_block):
        self.connect((self.freq_xlating_fft_filter_ccc_0_0, 0), (self.blocks_stream_to_vector_0_0, 0))
        self.connect((self.uhd_usrp_source_0, 0), (self.blocks_multiply_xx_0, 0))

+        ##################################################
+        # Frequency offset polling thread
+        ##################################################
+        def _freq_offset_probe():
+            frame_dur = slot_duration * num_uavs
+            while True:
+                val = self.digital_fll_band_edge_cc_0_0.get_frequency()
+                freq_hz = val * samp_rate / (2 * math.pi)
+                now = time.time()
+                slot_time = now % frame_dur
+                current_slot = int(slot_time / slot_duration)
+                time_into_slot = slot_time - current_slot * slot_duration
+                tx_id = -1 if time_into_slot < guard_interval else current_slot
+                self._freqoffset_file.write(f'{tx_id},{freq_hz}\n')
+                self._freqoffset_file.flush()
+                time.sleep(0.01)
+        _freq_offset_thread = threading.Thread(target=_freq_offset_probe)
+        _freq_offset_thread.daemon = True
+        _freq_offset_thread.start()
+

    def get_args(self):
        return self.args
--- a/aerpaw/radio/startchannelsounderRXGRC.sh
+++ b/aerpaw/radio/startchannelsounderRXGRC.sh
@@ -20,4 +20,4 @@ else
 fi

 cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
-python3 CSwSNRRX.py --freq $RX_FREQ --gainrx $GAIN_RX --noise 0 --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS
+python3 CSwSNRRX.py --freq $RX_FREQ --gainrx $GAIN_RX --noise 0 --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS "$@"
--- a/aerpaw/radio/startchannelsounderTXGRC.sh
+++ b/aerpaw/radio/startchannelsounderTXGRC.sh
@@ -20,4 +20,4 @@ else
 fi

 cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
-python3 CSwSNRTX.py --freq $TX_FREQ --gaintx $GAIN_TX --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS
+python3 CSwSNRTX.py --freq $TX_FREQ --gaintx $GAIN_TX --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS "$@"
--- a/aerpaw/radio/updateScripts.sh
+++ b/aerpaw/radio/updateScripts.sh
@@ -1,7 +1,15 @@
 #!/bin/bash

+# Drop in replacements for channel sounder scripts
 cp startchannelsounderRXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
 cp startchannelsounderTXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
-
 cp CSwSNRRX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
-cp CSwSNRTX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
+cp CSwSNRTX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
+
+# Replace start scripts
+cp ../scripts/startexperiment.sh /root/.
+cp ../scripts/startRadio.sh /root/Profiles/ProfileScripts/Radio/.
+cp ../scripts/startVehicle.sh /root/Profiles/ProfileScripts/Vehicle/.
+
+echo "REMEMBER! Manually edit startexperiment.sh to point to the correct client.yaml"
+echo "REMEMBER! Manually copy startexperiment_controller.sh to startexperiment.sh on the fixed node"
--- a/aerpaw/results/plotRadioLogs.m
+++ b/aerpaw/results/plotRadioLogs.m
@@ -24,16 +24,25 @@ function [f, R] = plotRadioLogs(resultsPath)
        R{ii}(bad, :) = [];
    end

+    % Compute path loss from Power (post-processing)
+    % Power = 20*log10(peak_mag) - rxGain; path loss = txGain - rxGain - Power
+    txGain_dB = 76;   % from startchannelsounderTXGRC.sh GAIN_TX
+    rxGain_dB = 30;   % from startchannelsounderRXGRC.sh GAIN_RX
+    for ii = 1:numel(R)
+        R{ii}.PathLoss = txGain_dB - rxGain_dB - R{ii}.Power;
+        R{ii}.FreqOffset = R{ii}.FreqOffset / 1e6; % Hz to MHz
+    end
+
    % Build legend labels and color map for up to 4 UAVs
    nUAV = numel(R);
    colors = lines(nUAV * nUAV);
    styles = ["-o", "-s", "-^", "-d", "-v", "-p", "-h", "-<", "->", "-+", "-x", "-*"];

-    metricNames = ["SNR", "Power", "Quality"];
-    yLabels     = ["SNR (dB)", "Power (dB)", "Quality"];
+    metricNames = ["SNR", "Power", "Quality", "PathLoss", "NoiseFloor", "FreqOffset"];
+    yLabels     = ["SNR (dB)", "Power (dB)", "Quality", "Path Loss (dB)", "Noise Floor (dB)", "Freq Offset (MHz)"];

    f = figure;
-    tl = tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
+    tl = tiledlayout(numel(metricNames), 1, 'TileSpacing', 'compact', 'Padding', 'compact');

    for mi = 1:numel(metricNames)
        ax = nexttile(tl);
--- a/aerpaw/results/readRadioLogs.m
+++ b/aerpaw/results/readRadioLogs.m
@@ -4,19 +4,19 @@ function R = readRadioLogs(logPath)
    end

    arguments (Output)
-        R (:, 6) table;
+        R (:, 8) table;
    end

    % Extract receiving UAV ID from directory name (e.g. "uav0_..." → 0)
    [~, dirName] = fileparts(logPath);
    rxID = int32(sscanf(dirName, 'uav%d'));

-    metrics = ["quality", "snr", "power"];
+    metrics = ["quality", "snr", "power", "noisefloor", "freqoffset"];
    logs = dir(logPath);
    logs = logs(endsWith({logs(:).name}, metrics + "_log.txt"));

-    R = table(datetime.empty(0,1), zeros(0,1,'int32'), zeros(0,1,'int32'), zeros(0,1), zeros(0,1), zeros(0,1), ...
-        'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality"]);
+    R = table(datetime.empty(0,1), zeros(0,1,'int32'), zeros(0,1,'int32'), zeros(0,1), zeros(0,1), zeros(0,1), zeros(0,1), zeros(0,1), ...
+        'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality", "NoiseFloor", "FreqOffset"]);

    for ii = 1:numel(logs)
        filepath = fullfile(logs(ii).folder, logs(ii).name);
@@ -43,13 +43,15 @@ function R = readRadioLogs(logPath)
        val = data{3};

        n = numel(ts);
-        t = table(ts, txId, repmat(rxID, n, 1), NaN(n,1), NaN(n,1), NaN(n,1), ...
-            'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality"]);
+        t = table(ts, txId, repmat(rxID, n, 1), NaN(n,1), NaN(n,1), NaN(n,1), NaN(n,1), NaN(n,1), ...
+            'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality", "NoiseFloor", "FreqOffset"]);

        switch metric
-            case "snr",     t.SNR = val;
-            case "power",   t.Power = val;
-            case "quality", t.Quality = val;
+            case "snr",        t.SNR = val;
+            case "power",      t.Power = val;
+            case "quality",    t.Quality = val;
+            case "noisefloor", t.NoiseFloor = val;
+            case "freqoffset", t.FreqOffset = val;
        end

        R = [R; t]; %#ok<AGROW>
--- a/aerpaw/results/resultsAnalysis.m
+++ b/aerpaw/results/resultsAnalysis.m
@@ -1,5 +1,5 @@
 %% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "t1"); % Define path to results copied from AERPAW platform
+resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform

 % Plot GPS logged data and scenario information (domain, objective, obstacles)
 seaToGroundLevel = 110; % measured approximately from USGS national map viewer
--- a/aerpaw/run_uav.sh
+++ b/aerpaw/run_uav.sh
@@ -32,8 +32,8 @@ else
    exit 1
 fi

-# Client config file (optional 2nd argument)
-CONFIG_FILE="${2:-config/client.yaml}"
+# Client config file: 2nd argument > AERPAW_CLIENT_CONFIG env var > default
+CONFIG_FILE="${2:-${AERPAW_CLIENT_CONFIG:-config/client.yaml}}"
 if [ ! -f "$CONFIG_FILE" ]; then
    echo "Error: Config file not found: $CONFIG_FILE"
    exit 1
@@ -59,7 +59,7 @@ echo "[run_uav] MAVLink connection: $CONN"

 # Run via aerpawlib
 echo "[run_uav] Starting UAV runner..."
-python3 -m aerpawlib \
+python3 -u -m aerpawlib \
    --script client.uav_runner \
    --conn "$CONN" \
    --vehicle drone
--- a/aerpaw/scripts/startRadio.sh
+++ b/aerpaw/scripts/startRadio.sh
@@ -0,0 +1,100 @@
+#!/bin/bash
+
+# Derive number of UAVs from scenario.csv
+NUM_UAVS=$(python3 -c "
+import csv, os
+csv_path = '/root/miSim/aerpaw/config/scenario.csv'
+with open(csv_path, 'r') as f:
+    reader = csv.reader(f, skipinitialspace=True)
+    header = [h.strip() for h in next(reader)]
+    row = next(reader)
+col = header.index('initialPositions')
+vals = [v.strip() for v in row[col].strip().split(',') if v.strip()]
+print(len(vals) // 3)
+" 2>/dev/null || echo 0)
+
+cd $PROFILE_DIR"/ProfileScripts/Radio/Helpers"
+
+if [ "$NUM_UAVS" -eq 2 ]; then
+    # Direct 1-to-1 mode: UAV 0 = TX only, UAV 1 = RX only
+    echo "[Radio] 2-UAV direct mode: UAV_ID=$UAV_ID"
+
+    if [ "$UAV_ID" -eq 0 ]; then
+        # TX only (--num-uavs 1 disables TDM muting)
+        screen -S txGRC -dm \
+               bash -c "stdbuf -oL -eL ./startchannelsounderTXGRC.sh --num-uavs 1 \
+               2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsoundertxgrc_log.txt"
+    else
+        # RX only (--num-uavs 1 disables TDM tagging)
+        screen -S rxGRC -dm \
+               bash -c "stdbuf -oL -eL ./startchannelsounderRXGRC.sh --num-uavs 1 \
+               2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsounderrxgrc_log.txt"
+
+        screen -S power -dm \
+               bash -c "stdbuf -oL -eL tail -F /root/Power\
+                2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
+
+        screen -S quality -dm \
+               bash -c "stdbuf -oL -eL tail -F /root/Quality\
+                2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
+
+        screen -S snr -dm \
+               bash -c "stdbuf -oL -eL tail -F /root/SNR\
+                2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
+
+        screen -S noisefloor -dm \
+              bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
+                2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_noisefloor_log.txt"
+
+        screen -S freqoffset -dm \
+               bash -c "stdbuf -oL -eL tail -F /root/FreqOffset\
+                2>&1 | ts $TS_FORMAT \
+               | tee $RESULTS_DIR/$LOG_PREFIX\_freqoffset_log.txt"
+    fi
+else
+    # 3+ UAVs: full TDM mode — every node runs both TX and RX
+    echo "[Radio] TDM mode: $NUM_UAVS UAVs, UAV_ID=$UAV_ID"
+
+    screen -S rxGRC -dm \
+           bash -c "stdbuf -oL -eL ./startchannelsounderRXGRC.sh \
+           2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsounderrxgrc_log.txt"
+
+    screen -S power -dm \
+           bash -c "stdbuf -oL -eL tail -F /root/Power\
+            2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
+
+    screen -S quality -dm \
+           bash -c "stdbuf -oL -eL tail -F /root/Quality\
+            2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
+
+    screen -S snr -dm \
+           bash -c "stdbuf -oL -eL tail -F /root/SNR\
+            2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
+
+    screen -S noisefloor -dm \
+          bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
+            2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_noisefloor_log.txt"
+
+    screen -S freqoffset -dm \
+           bash -c "stdbuf -oL -eL tail -F /root/FreqOffset\
+            2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_freqoffset_log.txt"
+
+    screen -S txGRC -dm \
+           bash -c "stdbuf -oL -eL ./startchannelsounderTXGRC.sh \
+           2>&1 | ts $TS_FORMAT \
+           | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsoundertxgrc_log.txt"
+fi
+
+cd -
--- a/aerpaw/scripts/startVehicle.sh
+++ b/aerpaw/scripts/startVehicle.sh
@@ -0,0 +1,30 @@
+#!/bin/bash
+
+### Sample GPS logger portion
+# use vehicle type generic to skip the arming requirement
+export VEHICLE_TYPE="${VEHICLE_TYPE:-generic}"
+
+# GPS Logger sample application (this does not move the vehicle)
+
+#cd $PROFILE_DIR"/ProfileScripts/Vehicle/Helpers"
+#
+#screen -S vehicle -dm \
+#       bash -c "stdbuf -oL -eL ./gpsLoggerHelper.sh \
+#       2> >(ts $TS_FORMAT >> $RESULTS_DIR/${LOG_PREFIX}_vehicle_log_err.txt) \
+#       | ts $TS_FORMAT \
+#       | tee $RESULTS_DIR/$LOG_PREFIX\_vehicle_log.txt"
+#
+#cd -
+
+### Actual control portion (custom)
+export VEHICLE_TYPE="${VEHICLE_TYPE:-drone}" # out of rover, drone, generic
+
+cd /root/miSim/aerpaw
+
+# Use screen/ts/tee aerpawism from sample script
+screen -S vehicle -dm \
+       bash -c "stdbuf -oL -eL ./run_uav.sh testbed \
+       | ts $TS_FORMAT \
+       | tee $RESULTS_DIR/$LOG_PREFIX\_vehicle_log.txt"     
+
+cd -
--- a/aerpaw/scripts/startVehicle_controller.sh
+++ b/aerpaw/scripts/startVehicle_controller.sh
@@ -0,0 +1,11 @@
+#!/bin/bash
+
+cd /root/miSim/aerpaw
+
+# Compile controller
+/bin/bash compile.sh
+
+# Run controller
+./build/controller_app
+
+cd -
--- a/aerpaw/scripts/startexperiment.sh
+++ b/aerpaw/scripts/startexperiment.sh
@@ -0,0 +1,50 @@
+#!/bin/bash
+/root/stopexperiment.sh
+
+source /root/.ap-set-experiment-env.sh
+source /root/.bashrc
+
+# set path to client config YAML
+export AERPAW_CLIENT_CONFIG=/root/miSim/aerpaw/config/client1.yaml
+
+export AERPAW_REPO=${AERPAW_REPO:-/root/AERPAW-Dev}
+export AERPAW_PYTHON=${AERPAW_PYTHON:-python3}
+export PYTHONPATH=/usr/local/lib/python3/dist-packages/
+export EXP_NUMBER=${EXP_NUMBER:-1}
+
+if [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_uav" ]; then
+    export VEHICLE_TYPE=drone
+elif [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_ugv" ]; then
+    export VEHICLE_TYPE=rover
+else
+    export VEHICLE_TYPE=none
+fi
+
+if [ "$AP_EXPENV_SESSION_ENV" == "Virtual" ]; then
+    export LAUNCH_MODE=EMULATION
+elif [ "$AP_EXPENV_SESSION_ENV" == "Testbed" ]; then
+    export LAUNCH_MODE=TESTBED
+else
+    export LAUNCH_MODE=none
+fi
+
+# prepare results directory
+export UAV_ID=$(python3 -c "import yaml; print(yaml.safe_load(open('$AERPAW_CLIENT_CONFIG'))['uav_id'])")
+export RESULTS_DIR_TIMESTAMP=$(date +%Y-%m-%d_%H_%M_%S)
+export RESULTS_DIR="/root/Results/uav${UAV_ID}_${RESULTS_DIR_TIMESTAMP}"
+mkdir -p "$RESULTS_DIR"
+
+export TS_FORMAT="${TS_FORMAT:-'[%Y-%m-%d %H:%M:%.S]'}"
+export LOG_PREFIX="$(date +%Y-%m-%d_%H_%M_%S)"
+
+export TX_FREQ=3.32e9
+export RX_FREQ=3.32e9
+
+export PROFILE_DIR=$AERPAW_REPO"/AHN/E-VM/Profile_software"
+cd $PROFILE_DIR"/ProfileScripts"
+
+./Radio/startRadio.sh
+#./Traffic/startTraffic.sh
+./Vehicle/startVehicle.sh
+
+schedule_stop.sh 30
--- a/aerpaw/scripts/startexperiment_controller.sh
+++ b/aerpaw/scripts/startexperiment_controller.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+/root/stopexperiment.sh
+
+source /root/.ap-set-experiment-env.sh
+source /root/.bashrc
+
+export AERPAW_REPO=${AERPAW_REPO:-/root/AERPAW-Dev}
+export AERPAW_PYTHON=${AERPAW_PYTHON:-python3}
+export PYTHONPATH=/usr/local/lib/python3/dist-packages/
+export EXP_NUMBER=${EXP_NUMBER:-1}
+
+if [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_uav" ]; then
+    export VEHICLE_TYPE=drone
+elif [ "$AP_EXPENV_THIS_CONTAINER_NODE_VEHICLE" == "vehicle_ugv" ]; then
+    export VEHICLE_TYPE=rover
+else
+    export VEHICLE_TYPE=none
+fi
+
+if [ "$AP_EXPENV_SESSION_ENV" == "Virtual" ]; then
+    export LAUNCH_MODE=EMULATION
+elif [ "$AP_EXPENV_SESSION_ENV" == "Testbed" ]; then
+    export LAUNCH_MODE=TESTBED
+else
+    export LAUNCH_MODE=none
+fi
+
+# prepare results directory
+export RESULTS_DIR_TIMESTAMP=$(date +%Y-%m-%d_%H_%M_%S)
+export RESULTS_DIR="/root/Results/controller_${RESULTS_DIR_TIMESTAMP}"
+mkdir -p "$RESULTS_DIR"
+
+export TS_FORMAT="${TS_FORMAT:-'[%Y-%m-%d %H:%M:%.S]'}"
+export LOG_PREFIX="$(date +%Y-%m-%d_%H_%M_%S)"
+
+export TX_FREQ=3.32e9
+export RX_FREQ=3.32e9
+
+export PROFILE_DIR=$AERPAW_REPO"/AHN/E-VM/Profile_software"
+cd $PROFILE_DIR"/ProfileScripts"
+
+screen -S controller -dm \
+	bash -c "stdbuf -oL -eL ./Vehicle/startVehicle.sh \
+	| ts $TS_FORMAT \
+	| tee $RESULTS_DIR/$LOG_PREFIX\_controller_log.txt"
+
+schedule_stop.sh 30
--- a/plot_1.m
+++ b/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");
--- a/plot_3.m
+++ b/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");
--- a/resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8d.xml
+++ b/resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8d.xml
--- a/resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8p.xml
+++ b/resources/project/AYz988O6MmLDfGQa1POLekzFkGo/YCy7McFB8-ln__QHYP5j7X7nQI8p.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>
--- a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/YoWkWDdbwwiRDjC6Inll7Thh1K8d.xml
+++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/YoWkWDdbwwiRDjC6Inll7Thh1K8d.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Ref="aerpaw/scripts" Type="Relative"/>
--- a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/YoWkWDdbwwiRDjC6Inll7Thh1K8p.xml
+++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/YoWkWDdbwwiRDjC6Inll7Thh1K8p.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="6402cbb5-c767-4c8b-bd7c-b2d7cf1055fc" type="Reference"/>
--- a/resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/q1TwgWjEDIn-a93hPi1WGodL3NYd.xml
+++ b/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>
--- a/resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/q1TwgWjEDIn-a93hPi1WGodL3NYp.xml
+++ b/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"/>
--- a/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcd.xml
+++ b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcd.xml
@@ -1,2 +1,2 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<Info location="t1.zip" type="File"/>
+<Info/>
--- a/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcp.xml
+++ b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="scripts" type="File"/>
--- a/resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQId.xml
+++ b/resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQId.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQIp.xml
+++ b/resources/project/Xaysz7d0ZEEdz7yS61mKTAp17xo/GbYZ0TkPTXbbWS6cBc7HG5YmHQIp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>
--- a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml
+++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml
+++ b/resources/project/o8EfsliYZoi0Pg0hEr9bLYBd-k0/C05cVxQ9NvFXxKc5bQbtmN_GvSAp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="startexperiment_controller.sh" type="File"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="startRadio.sh" type="File"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="startexperiment.sh" type="File"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="startVehicle.sh" type="File"/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/oczvKa9PTzIQRYD1e6ccXbiMz_wd.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/oczvKa9PTzIQRYD1e6ccXbiMz_wd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/oczvKa9PTzIQRYD1e6ccXbiMz_wp.xml
+++ b/resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/oczvKa9PTzIQRYD1e6ccXbiMz_wp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="startVehicle_controller.sh" type="File"/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGod.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGod.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGop.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/AYz988O6MmLDfGQa1POLekzFkGop.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plot3" type="File"/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xod.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xod.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xop.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/Xaysz7d0ZEEdz7yS61mKTAp17xop.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plot1_2" type="File"/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0d.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/o8EfsliYZoi0Pg0hEr9bLYBd-k0p.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plot1" type="File"/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwd.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwp.xml
+++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/zpVmgHK5gbVcPgNcOE2ip90e6Dwp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plot1_3" type="File"/>
--- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/Dt-U7DFYhio-3hILJY9YWPnda6sd.xml
+++ b/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>
--- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/Dt-U7DFYhio-3hILJY9YWPnda6sp.xml
+++ b/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"/>
--- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/e-EiR3xDuOjis31Xf2_ZV1uoriEd.xml
+++ b/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>
--- a/resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/e-EiR3xDuOjis31Xf2_ZV1uoriEp.xml
+++ b/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"/>
--- a/resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysd.xml
+++ b/resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysd.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>
--- a/resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysp.xml
+++ b/resources/project/zpVmgHK5gbVcPgNcOE2ip90e6Dw/WwFoZBvO-jYGdGTuAqgFvzbHuysp.xml
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>
--- a/t1.zip
+++ b/t1.zip
--- a/test/parametricTestSuite.m
+++ b/test/parametricTestSuite.m
@@ -57,13 +57,16 @@ 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);
+            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();

            % Run
            tc.testClass = tc.testClass.run();
+
+            % Save results and clean up
+            tc.testClass = tc.testClass.teardown();
        end
        function csv_parametric_tests_random_agents(tc)
            % Read in parameters to iterate over
@@ -147,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);
--- a/test/results.m
+++ b/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
--- a/test/test_miSim.m
+++ b/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( ...
--- a/util/objectiveFunctionWrapper.m
+++ b/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
Author	SHA1	Message	Date
Kevin D	f23675a54c	results	2026-03-17 22:15:42 -07:00
Kevin D	8c3b853895	plot1 for multiple trials	2026-03-17 12:21:14 -07:00
Kevin D	e77b05bc0f	plots 3 and 4	2026-03-16 19:22:31 -07:00
Kevin D	6b74347411	started plot3 work	2026-03-16 16:19:38 -07:00
Kevin D	a3837a6ef4	second attempt at plot1	2026-03-16 14:35:52 -07:00
Kevin D	01f2af9102	added second plot - pairwise distances	2026-03-15 17:43:45 -07:00
Kevin D	0d02e5d1f5	plot1 kinda works	2026-03-15 15:15:48 -07:00
Kevin D	2a0e2e500f	results plot1 WIP	2026-03-15 13:42:35 -07:00
Kevin D	ca891a809f	fixed test cases	2026-03-13 16:58:23 -07:00
Kevin D	771575560f	added static network option	2026-03-13 16:18:12 -07:00
Kevin D	f003528a9c	double integrator dynamics	2026-03-13 15:54:43 -07:00
Kevin D	102f23316d	added logging to matfile	2026-03-13 10:55:46 -07:00
Kevin D	24113f282f	remove TDM for 2 UAV experiments	2026-03-12 16:33:19 -07:00
Kevin D	b4cd7613ec	new scenario	2026-03-11 17:13:09 -07:00
Kevin D	97e34264dd	vehicle runner fix	2026-03-11 12:51:27 -07:00
Kevin D	c5f1dcdb51	updated results analysis script	2026-03-11 12:46:29 -07:00
Kevin D	e5fa2fa827	small testbed convenience fixes	2026-03-11 12:30:36 -07:00
Kevin D	fdd9b49e34	scenario tweak	2026-03-11 12:05:06 -07:00
Kevin D	ea034dd748	communications constraint improvements, experiment 1 design	2026-03-11 12:02:17 -07:00
Kevin D	b09f882369	added more radio metrics	2026-03-09 21:23:31 -07:00
Kevin D	cdbfaebc17	added modified AERPAW scripts	2026-03-09 21:23:31 -07:00
Kevin D	1b4fec0f72	plot script fixes	2026-03-09 21:23:31 -07:00
Kevin D	cd3463d479	finalized plotting utility	2026-03-09 21:23:31 -07:00
Kevin D	624b2bdcb2	type error fix	2026-03-09 21:23:31 -07:00
Kevin D	6da0c97abf	added radio plotting tools	2026-03-09 21:23:31 -07:00
Kevin D	3c775cf814	plotting update	2026-03-09 21:23:31 -07:00
Kevin D	1562fdc351	fixed GPS log out path	2026-03-09 21:23:31 -07:00
Kevin D	a706857374	radio experiment TDM working	2026-03-09 21:23:31 -07:00
Kevin D	8c5811ff6a	seems to line up well again, constrainMotion updates	2026-03-09 21:23:31 -07:00
Kevin D	14201aff5d	scenario update, quadprog issue	2026-03-09 21:23:31 -07:00
Kevin D	532e37f133	obstacle respected now	2026-03-09 21:23:31 -07:00
Kevin D	986f4e2dcf	obstacles in but ignored	2026-03-09 21:23:31 -07:00
Kevin D	c18b470706	scenario - obstacle - one around, one over	2026-03-09 21:23:31 -07:00
Kevin D	438ebda388	per-UAV parameters	2026-03-09 21:23:31 -07:00
Kevin D	f40d2bfd84	moved reader out of miSim, went to event-based guidance	2026-03-09 21:23:31 -07:00
Kevin D	117d34590e	removed prompt to continue	2026-03-09 21:23:31 -07:00
Kevin D	7da35c5cda	results compare favorably	2026-03-09 21:23:31 -07:00
Kevin D	05ac8a6e97	scenario edits	2026-03-09 21:23:31 -07:00
Kevin D	813b124c47	improved globe plotting	2026-03-09 21:23:31 -07:00
Kevin D	5408a31d56	moved origin to get more space from geofence	2026-03-09 21:23:31 -07:00
Kevin D	1d4f59734b	scenario csv on both platforms	2026-03-09 21:23:31 -07:00
Kevin D	5e52292b71	added slack in collision avoidance constraint	2026-03-09 21:23:31 -07:00
Kevin D	f1c2df31d9	csv parse update	2026-03-09 21:23:31 -07:00
Kevin D	c19f65c3a1	testing fixes	2026-03-09 21:23:31 -07:00
Kevin D	dbba95c6a9	added constraint violation recovery mechanism	2026-03-09 21:23:31 -07:00
Kevin D	1ada914384	codegen fixes, bug fixes, gets running on testbed environment	2026-03-09 21:23:31 -07:00
Kevin D	58d87cd16f	gps log plotting	2026-03-09 21:23:31 -07:00
Kevin D	cec6458f7c	aerpaw gps csv reader	2026-03-09 21:23:31 -07:00
Kevin D	9385b9bd06	gps logging updates	2026-03-09 21:23:31 -07:00
Kevin D	d25287cdf9	respect geofence, move from socket to async/await	2026-03-09 21:23:31 -07:00
Kevin D	61e440b594	more config cleanup	2026-03-09 21:23:31 -07:00
Kevin D	dbb4ba178a	config cleanup	2026-03-09 21:23:31 -07:00
Kevin D	cde86065e9	project cleanup	2026-03-09 21:23:31 -07:00
Kevin D	87d925ba5c	logging consistency	2026-03-09 21:23:31 -07:00
Kevin D	0e9f494c50	message type updates	2026-03-09 21:23:31 -07:00
Kevin D	bcfaad1817	removed potentially faulty environment detection in favor of explicit setting	2026-03-09 21:23:31 -07:00
Kevin D	1475d9e7d1	refactor experiment config	2026-03-09 21:23:31 -07:00
Kevin D	ee238f239d	added parallel message receiving for previously implemented messaging where necessary	2026-03-09 21:23:31 -07:00
Kevin D	4cdcb16ee3	added RTL and LAND capabilities	2026-03-09 21:23:31 -07:00
Kevin D	9705c1e952	kinda working	2026-03-09 21:23:31 -07:00
Kevin D	8002336ba1	added real autopilot connection info	2026-03-09 21:23:31 -07:00
Kevin D	cb61ddb161	allowed connection to real autopilot	2026-03-09 21:23:31 -07:00
Kevin D	4d08e2c88a	added aerpawlib capabilities to uav script	2026-03-09 21:23:31 -07:00
Kevin D	c8b54a30aa	reorganized and added aerpawlib submodule	2026-03-09 21:23:31 -07:00
Kevin D	1ae617d5f7	sending starting positions to agents (not verified on AERPAW yet)	2026-03-09 21:23:31 -07:00
Kevin D	fa5d63361c	cleanup demo	2026-03-09 21:23:31 -07:00
Kevin D	8abd009aed	basic implementation of client/server for AERPAW, whole lot of mess included	2026-03-09 21:23:31 -07:00
Kevin D	20417f240c	experiment setup	2026-03-09 21:23:31 -07:00