fixed test cases

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "aerpaw/aerpawlib"]
-	path = aerpaw/aerpawlib
-	url = https://github.com/morzack/aerpawlib-vehicle-control.git

@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;

@agent/initialize.m

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

@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,6 +6,9 @@ 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")};
@@ -75,19 +78,26 @@ 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;
-    else
-        pNext = obj.pos + (targetRate / gradNorm) * gradC;
-    end
-
-    % Move to next position
+    % Compute unconstrained next state
     obj.lastPos = obj.pos;
-    obj.pos = pNext;
+    if useDoubleIntegrator
+        % Double-integrator: gradient produces desired acceleration with damping
+        obj.lastVel = obj.vel;
+        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
+        % Single-integrator: gradient directly sets position step
+        if gradNorm >= 1e-100
+            obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
+        end
+    end
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;

@miSim/constrainMotion.m

@@ -8,31 +8,31 @@ 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);
@@ -60,6 +60,10 @@ 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
@@ -80,6 +84,9 @@ 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
@@ -120,6 +127,9 @@ function [obj] = constrainMotion(obj)
         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}.pos - obj.agents{jj}.pos)^2;
 
                 A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
                 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

@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();
 

@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

@miSim/miSim.m

@@ -18,11 +18,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 +45,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

@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

@miSim/teardown.m

@@ -6,10 +6,33 @@ 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;
@@ -25,6 +48,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

@miSim/writeInits.m

@@ -15,16 +15,16 @@ function writeInits(obj)
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
 
-
     % Combine with simulation parameters
     inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
                     "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); % still needs obstacle states and objective state
 
     % Save all parameters to output file
     initsFile = strcat(obj.artifactName, "_miSimInits");

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

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

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
+5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1

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>

aerpaw/impl/controller_main.cpp

@@ -3,10 +3,15 @@
 #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);

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

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 "$@"

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 "$@"

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/.
+
+# 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"

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);

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>

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

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

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 -

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 -

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 -

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

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/YoWkWDdbwwiRDjC6Inll7Thh1K8d.xml

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

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

resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYd.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/7zvVfoOGOecwrnesPxOvEZRcJNYp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYd.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/LU7BdyAbq1VWsP1tcW099qs0_cYp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsd.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/M4B1x4nVEuLmUODrHewQHG0CDHsp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewd.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/Wf9PEeTALvEX83cew4fZfavy2ewp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAd.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/bRP3MIwZ4PcWx9PloJbVyWC00JAp.xml

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

resources/project/p7b0T5lVDU-D8JiROZNSX56qOhc/oczvKa9PTzIQRYD1e6ccXbiMz_wd.xml

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

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

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

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( ...