Merge branch 'main' into AERPAW-experiments

.gitignore

@@ -4,6 +4,7 @@
 *.autosave
 *.slx.r*
 *.mdl.r*
+*.bak
 
 # Derived content-obscured files
 *.p
@@ -48,6 +49,7 @@ sandbox/*
 
 # Figures
 *.fig
+*.png
 
 # Python Virtual Environment
 aerpaw/venv/

@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;
@@ -30,7 +32,9 @@ classdef agent
 
     properties (SetAccess = private, GetAccess = public)
         initialStepSize = NaN;
+        initialMaxAngleStepSize = NaN;
         stepDecayRate = NaN;
+        angleStepDecayRate = NaN;
     end
 
     methods (Access = public)
@@ -46,8 +50,8 @@ classdef agent
             obj.commsGeometry = spherical;
         end
         [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [obj] = run(obj, domain, partitioning, t, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents);
-        [partitioning] = partition(obj, agents, objective)
+        [partitioning, agents] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
@@ -7,6 +7,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
         comRange (1, 1) double;
         maxIter (1, 1) double;
         initialStepSize (1, 1) double = 0.2;
+        initialMaxAngleStepSize (1, 1) double = 5.0;
         label (1, 1) string = "";
         plotCommsGeometry (1, 1) logical = false;
     end
@@ -15,12 +16,17 @@ 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;
     obj.plotCommsGeometry = plotCommsGeometry;
     obj.initialStepSize = initialStepSize;
+    obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
     obj.stepDecayRate = obj.initialStepSize / maxIter;
+    obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
 
     % Initialize performance vector
     if coder.target('MATLAB')
@@ -32,5 +38,5 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.halfAngle()) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label), obj.sensorModel.tilt, obj.sensorModel.azimuth);
-end
+end

@agent/partition.m

@@ -1,4 +1,4 @@
-function [partitioning] = partition(obj, agents, objective)
+function [partitioning, agents] = partition(obj, agents, objective)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         agents (:, 1) {mustBeA(agents, "cell")};
@@ -6,6 +6,7 @@ function [partitioning] = partition(obj, agents, objective)
     end
     arguments (Output)
         partitioning (:, :) double;
+        agents (:, 1) cell;
     end
 
     nAgents = size(agents, 1);
@@ -18,8 +19,22 @@ function [partitioning] = partition(obj, agents, objective)
     % minimum threshold that must be exceeded for any assignment.
     agentPerf = zeros(nPoints, nAgents + 1);
     for aa = 1:nAgents
-        p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+        if isa(agents{aa}.sensorModel, "sigmoidSensor")
-            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
+            p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+                [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
+        elseif isa(agents{aa}.sensorModel, "rfSensor")
+            otherSensorsIdx = [1:(aa - 1), (aa + 1):size(agents, 1)];
+            otherSensors = agents(otherSensorsIdx);
+            otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherSensors, "UniformOutput", false));
+            otherSensors = cellfun(@(x) x.sensorModel, otherSensors, "UniformOutput", false);
+            [p, ~, agents{aa}.sensorModel, otherSensors] = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
+                [objective.X(:), objective.Y(:), zeros(nPoints, 1)], otherSensorsPos, otherSensors);
+            for k = 1:numel(otherSensorsIdx)
+                agents{otherSensorsIdx(k)}.sensorModel = otherSensors{k};
+            end
+        else
+            error("?");
+        end
         agentPerf(:, aa) = p(:);
     end
     agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;

@agent/run.m

@@ -1,16 +1,27 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
+function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
         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;
+        optimizeSensorPointing (1, 1) logical = false;
+        otherAgents (:, 1) cell = cell();
     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(:))
@@ -23,33 +34,62 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     maskedX = domain.objective.X(partitionMask);
     maskedY = domain.objective.Y(partitionMask);
 
-    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    if isa(obj.sensorModel, "rfSensor")
-    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+        % Extract other agents' sensor models and positions once, outside the delta loop.
-    deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
+        % Mask the full-grid RSS caches (filled by partition()) down to this agent's
-    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+        % partition subset so sensorPerformance can reuse them for all perturbations.
-    for ii = 1:7
+        otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherAgents, "UniformOutput", false));
+        otherSensors    = cellfun(@(x) x.sensorModel, otherAgents, "UniformOutput", false);
+        partitionIndices = find(partitionMask);
+        for kk = 1:numel(otherSensors)
+            if ~isempty(otherSensors{kk}.rssCache)
+                otherSensors{kk}.rssCache = otherSensors{kk}.rssCache(partitionIndices);
+            end
+        end
+        % Pre-mask this agent's own full-grid cache to the partition subset.
+        % Used for ii==1 (current position, no perturbation) to avoid recomputing.
+        baseSensorModel = obj.sensorModel;
+        if ~isempty(obj.sensorModel.rssCache)
+            baseSensorModel.rssCache = obj.sensorModel.rssCache(partitionIndices);
+        end
+    end
+
+    if optimizeSensorPointing
+        % Stash actual current sensor model tilt/azimuth before messing with it
+        % in these following hypotheticals
+        tilt = obj.sensorModel.tilt;
+        azimuth = obj.sensorModel.azimuth;
+    end
+
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z, tilt, azimuth
+    deltaPos = domain.objective.discretizationStep; % smallest possible step size that gets different results
+    if optimizeSensorPointing
+        deltaAngle = atan2d(domain.objective.discretizationStep, obj.pos(3)); % smallest possible angle derived from smallest possible step size and current height
+    end
+    deltaApplicator = [0, 0, 0, 0, 0; 1, 0, 0, 0, 0; -1, 0, 0, 0, 0; 0, 1, 0, 0, 0; 0, -1, 0, 0, 0; 0, 0, 1, 0, 0; 0, 0, -1, 0, 0; 0, 0, 0, 1, 0; 0, 0, 0, -1, 0; 0, 0, 0, 0, 1; 0, 0, 0, 0, -1;]; % none, +X, -X, +Y, -Y, +Z, -Z, +tilt, -tilt, +azimuth, -azimuth
+    C_delta = NaN(size(deltaApplicator, 1), 1); % agent performance at delta steps in each direction
+    for ii = 1:size(deltaApplicator, 1)
+        if ~optimizeSensorPointing && ii > 7; break; end
         % Apply delta to position
-        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+        pos = obj.pos + deltaPos * deltaApplicator(ii, 1:3);
+        if optimizeSensorPointing
+            % Apply delta to tilt and azimuth
+            obj.sensorModel.tilt = tilt + deltaAngle * deltaApplicator(ii, 4);
+            obj.sensorModel.azimuth = azimuth + deltaAngle * deltaApplicator(ii, 5);
+        end
         
         % Compute performance values on partition
-        if ii < 6
+        if isa(obj.sensorModel, "sigmoidSensor")
-            % Compute sensing performance
             sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-            % Objective performance does not change for 0, +/- X, +/- Y steps.
+        elseif isa(obj.sensorModel, "rfSensor")
-            % Those values are computed once before the loop and are only
+            if ii == 1
-            % recomputed when +/- Z steps are applied
+                sensorModelForDelta = baseSensorModel; % reuse partition-step cache; no recompute needed
+            else
+                sensorModelForDelta = obj.sensorModel.clearRssCache;
+            end
+            [sensorValues, ~, ~, ~] = sensorModelForDelta.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))], otherSensorsPos, otherSensors);
         else
-            % Redo partitioning for Z stepping only
+            error("?");
-            partitioning = obj.partition(agents, domain.objective);
-
-            % Recompute partiton-derived performance values for objective
-            partitionMask = partitioning == index;
-            objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
-
-            % Recompute partiton-derived performance values for sensing
-            maskedX = domain.objective.X(partitionMask);
-            maskedY = domain.objective.Y(partitionMask);
-            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
         end
 
         % Rearrange data into image arrays
@@ -63,31 +103,53 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
         C_delta(ii) = sum(C(~isnan(C)));
     end
 
+    if optimizeSensorPointing
+        % Reset sensor model to actual tilt and azimuth angles
+        obj.sensorModel.tilt = tilt;
+        obj.sensorModel.azimuth = azimuth;
+    end
+
     % Store agent performance at current time and place
     if coder.target('MATLAB')
         obj.performance(timestepIndex + 1) = C_delta(1);
     end
 
     % Compute gradient by finite central differences
-    gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
+    gradC = [(C_delta(2)-C_delta(3))/(2*deltaPos), (C_delta(4)-C_delta(5))/(2*deltaPos), (C_delta(6)-C_delta(7))/(2*deltaPos)];
-
+    if optimizeSensorPointing
-    % Compute scaling factor
+        gradC(4) = (C_delta(8) -C_delta(9)) /(2*deltaAngle);
-    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+        gradC(5) = (C_delta(10)-C_delta(11))/(2*deltaAngle);
-    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 scaling factor
-    obj.lastPos = obj.pos;
+    targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
-    obj.pos = pNext;
+    gradPosNorm = norm(gradC(1:3));
+
+    % Compute unconstrained next state
+    if useDoubleIntegrator
+        % Double-integrator: gradient produces desired acceleration with damping
+        if gradPosNorm < 1e-100
+            a_gradient = zeros(1, 5);
+        else
+            % Scale so steady-state step ≈ targetRate (matching SI behavior)
+            a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
+        end
+        % Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
+        obj.vel = (obj.vel + a_gradient(1:3) * dt) / (1 + dampingCoeff * dt);
+        obj.pos = obj.lastPos + obj.vel * dt;
+    else
+        % Single-integrator: gradient directly sets position step
+        if gradPosNorm >= 1e-100
+            obj.pos = obj.pos + (targetPosRate / gradPosNorm) * gradC(1:3);
+        end
+    end
+
+    % Update tilt and azimuth, saturating at the decaying maximum allowed step size
+    if optimizeSensorPointing
+        maxAngleStep = obj.initialMaxAngleStepSize - obj.angleStepDecayRate * timestepIndex;
+        obj.sensorModel.tilt    = obj.sensorModel.tilt    + sign(gradC(4)) * min(abs(gradC(4)), maxAngleStep);
+        obj.sensorModel.azimuth = obj.sensorModel.azimuth + sign(gradC(5)) * min(abs(gradC(5)), maxAngleStep);
+    end
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;

@agent/updatePlots.m

@@ -7,45 +7,68 @@ function updatePlots(obj)
 
     % Find change in agent position since last timestep
     deltaPos = obj.pos - obj.lastPos;
-    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+    posChanged    = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
-        % Agent did not move, so nothing has to move on the plots
+    orientChanged = obj.sensorModel.tilt    ~= obj.fovGeometry.tilt || ...
+                    obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
+
+    if ~posChanged && ~orientChanged
         return;
     end
 
-    % Scatterplot point positions
+    if posChanged
-    for ii = 1:size(obj.scatterPoints, 1)
+        % Scatterplot point positions
-        obj.scatterPoints(ii).XData = obj.pos(1);
+        for ii = 1:size(obj.scatterPoints, 1)
-        obj.scatterPoints(ii).YData = obj.pos(2);
+            obj.scatterPoints(ii).XData = obj.pos(1);
-        obj.scatterPoints(ii).ZData = obj.pos(3);
+            obj.scatterPoints(ii).YData = obj.pos(2);
-    end
+            obj.scatterPoints(ii).ZData = obj.pos(3);
-
-    % Collision geometry edges
-    for jj = 1:size(obj.collisionGeometry.lines, 2)
-        % Update plotting
-        for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
-            obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
-            obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
-            obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
         end
-    end
 
-    % Communications geometry edges
+        % Collision geometry edges
-    if obj.plotCommsGeometry
+        for jj = 1:size(obj.collisionGeometry.lines, 2)
-        for jj = 1:size(obj.commsGeometry.lines, 2)
             for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
                 obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
                 obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
                 obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
             end
         end
+
+        % Communications geometry edges
+        if obj.plotCommsGeometry
+            for jj = 1:size(obj.commsGeometry.lines, 2)
+                for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
+                    obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
+                    obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
+                    obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
+                end
+            end
+        end
     end
 
-    % Update FOV geometry surfaces
+    % FOV cone: recompute full mesh whenever position or orientation changes
-    for jj = 1:size(obj.fovGeometry.surface, 2)
+    if ~isempty(obj.fovGeometry.surface)
-        % Update each plot
+        % Sync fovGeometry state to current agent position and sensor orientation
-        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
+        obj.fovGeometry = obj.fovGeometry.initialize( ...
-        obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
+            obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
-        obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
+            obj.fovGeometry.tag, obj.fovGeometry.label, ...
-        obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
+            obj.sensorModel.tilt, obj.sensorModel.azimuth);
+
+        % Recompute cone mesh (mirrors cone.plot logic)
+        maxAlt = obj.fovGeometry.surface(1).Parent.ZLim(2);
+        scalingFactor = maxAlt / obj.fovGeometry.height;
+        [X, Y, Z] = cylinder([scalingFactor * obj.fovGeometry.radius, 0], obj.fovGeometry.n);
+        Z = Z * maxAlt;
+        Ry = [cosd(obj.fovGeometry.tilt), 0, -sind(obj.fovGeometry.tilt); 0, 1, 0; sind(obj.fovGeometry.tilt), 0, cosd(obj.fovGeometry.tilt)];
+        Rz = [sind(obj.fovGeometry.azimuth), -cosd(obj.fovGeometry.azimuth), 0; cosd(obj.fovGeometry.azimuth), sind(obj.fovGeometry.azimuth), 0; 0, 0, 1];
+        R  = Rz * Ry;
+        pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
+        X = reshape(pts(1, :), size(X)) + obj.pos(1);
+        Y = reshape(pts(2, :), size(Y)) + obj.pos(2);
+        Z = reshape(pts(3, :) + maxAlt, size(Z)) + obj.pos(3) - maxAlt;
+
+        for jj = 1:size(obj.fovGeometry.surface, 2)
+            obj.fovGeometry.surface(jj).XData = X;
+            obj.fovGeometry.surface(jj).YData = Y;
+            obj.fovGeometry.surface(jj).ZData = Z;
+        end
     end
-end
+end

@miSim/constrainMotion.m

@@ -8,41 +8,41 @@ function [obj] = constrainMotion(obj)
 
     nAgents = size(obj.agents, 1);
 
-    if nAgents < 2
+    % Compute current velocity and desired control input
-        nAAPairs = 0;
+    v = zeros(nAgents, 3);        % current velocity (for drift term in DI mode)
-    else
+    u_desired = zeros(nAgents, 3); % desired control: velocity (SI) or acceleration (DI)
-        nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
-    end
-
-    % Compute velocity matrix from unconstrained gradient-ascent step
-    v = zeros(nAgents, 3);
     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")
+    if ~obj.useDoubleIntegrator && (all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all"))
-        % Agents are not attempting to move, so there is no motion to be
+        % Single-integrator: agents are not attempting to move
-        % constrained
+        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);
+    A = zeros(obj.numBarriers, 3 * nAgents);
-    b = zeros(total, 1);
+    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,22 @@ function [obj] = constrainMotion(obj)
         end
     end
 
+    idx = length(h(triu(true(size(h)), 1)));    
+    if coder.target('MATLAB')
+	obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
+    end
+    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,51 +86,56 @@ function [obj] = constrainMotion(obj)
         end
     end
 
+    if coder.target('MATLAB')
+	obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
+    end
+    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;
-    end
 
-    if coder.target('MATLAB')
+        if coder.target('MATLAB')
-        % Save off h function values (logging only — not needed in compiled mode)
+	    obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
-        obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
+        end
+	idx = idx + 6;
     end
 
     % Add communication network constraints
@@ -133,21 +144,44 @@ 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
+    
+    if coder.target('MATLAB')
+	obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
+    end
 
-    % Solve QP program generated earlier
+    % Double-integrator: transform QP from velocity to acceleration space.
-    vhat = reshape(v', 3 * nAgents, 1);
+    % 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 +191,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);
+    [uNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
-    vNew = reshape(vNew, 3, nAgents)';
+    uNew = reshape(uNew, 3, nAgents)';
 
     if exitflag < 0
         % Infeasible or other hard failure: hold all agents at current positions
@@ -167,9 +201,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 +211,16 @@ function [obj] = constrainMotion(obj)
         end
     end
 
-    % Update the "next position" that was previously set by unconstrained
+    % Update agent state using the constrained control input
-    % GA using the constrained solution produced here
+    for ii = 1:size(uNew, 1)
-    for ii = 1:size(vNew, 1)
+        if obj.useDoubleIntegrator
-        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
+            % 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
@@ -188,4 +228,4 @@ function [obj] = constrainMotion(obj)
     % Running at the simulation level is just meant to simplify the
     % simulation
 
-end
+end

@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, optimizeSensorPointing)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
@@ -11,6 +11,10 @@ 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;
+        optimizeSensorPointing (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "miSim")};
@@ -68,16 +72,18 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
     % Set labels for agents and collision geometries in cases where they
-    % were not provieded at the time of their initialization
+    % were not provieded at the time of their initialization.
-    for ii = 1:size(obj.agents, 1)
+    % Guarded by coder.target: label is a fixed-size "" in codegen, and
-        % Agent
+    % sprintf returns variable-length — incompatible even in a dead branch.
-        if isempty(char(obj.agents{ii}.label))
+    % On the compiled path agents always receive explicit labels from the caller.
-            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
+    if coder.target('MATLAB')
-        end
+        for ii = 1:size(obj.agents, 1)
-
+            if isempty(char(obj.agents{ii}.label))
-        % Collision geometry
+                obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
-        if isempty(char(obj.agents{ii}.collisionGeometry.label))
+            end
-            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+            if isempty(char(obj.agents{ii}.collisionGeometry.label))
+                obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+            end
         end
     end
 
@@ -86,9 +92,19 @@ 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;
+    obj.optimizeSensorPointing = optimizeSensorPointing;
+
+    % 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)';
@@ -97,18 +113,39 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Prepare performance data store (at t = 0, all have 0 performance)
         obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
-        % Prepare h function data store
-        obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
     end
 
     % 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));
+
+        % Initialize constraint adjacency history (nAgents x nAgents x nTimesteps)
+        nAgents = size(obj.agents, 1);
+        obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
+        obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
+
+
         % Set up plots showing initialized state
         obj = obj.plot();
 

@miSim/initializeFromCsv.m

@@ -52,8 +52,19 @@ BETA_TILT_VEC        = scenario.betaTilt;           % 1×N
 
 DOMAIN_MIN                 = scenario.domainMin;                % 1×3
 DOMAIN_MAX                 = scenario.domainMax;                % 1×3
-OBJECTIVE_GROUND_POS       = scenario.objectivePos;             % 1×2
+
-OBJECTIVE_VAR              = reshape(scenario.objectiveVar, 2, 2); % 2×2 covariance matrix
+% objectivePos: 2 values per Gaussian component (1 or 2 components supported)
+nObjComponents = numel(scenario.objectivePos) / 2;
+assert(mod(numel(scenario.objectivePos), 2) == 0, ...
+    'objectivePos must have an even number of values (2 per Gaussian component)');
+assert(nObjComponents >= 1 && nObjComponents <= 2, ...
+    'At most 2 objective Gaussian components supported; got %d', nObjComponents);
+assert(numel(scenario.objectiveVar) == nObjComponents * 4, ...
+    'objectiveVar must have %d values for %d component(s); got %d', ...
+    nObjComponents * 4, nObjComponents, numel(scenario.objectiveVar));
+OBJECTIVE_GROUND_POS = reshape(scenario.objectivePos, 2, nObjComponents)';           % nObj×2
+OBJECTIVE_VAR        = permute(reshape(scenario.objectiveVar, 2, 2, nObjComponents), [3, 1, 2]); % nObj×2×2
+
 SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
 
 % Initial UAV positions: flat vector reshaped to N×3
@@ -79,6 +90,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 +152,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/initializeFromInits.m

@@ -0,0 +1,87 @@
+function obj = initializeFromInits(obj, initsPath)
+% INITIALIZEFROMINITS  Initialize miSim from a saved simInits matfile.
+%
+% Loads all simulation parameters and initial agent states written by
+% writeInits(), reconstructs domain, objective, agents, and obstacles, then
+% calls the standard obj.initialize() method.  Plots and video are disabled.
+%
+% Usage:
+%   sim = sim.initializeFromInits('sandbox/2025_01_01_12_00_00_miSimInits.mat');
+
+arguments (Input)
+    obj       (1, 1) {mustBeA(obj, 'miSim')};
+    initsPath (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+inits = load(initsPath);
+
+% ---- Build domain ------------------------------------------------------------
+dom = rectangularPrism;
+dom = dom.initialize([inits.domainMin; inits.domainMax], REGION_TYPE.DOMAIN, "Domain");
+
+% ---- Build sensing objective -------------------------------------------------
+dom.objective = sensingObjective;
+% reshape guards against MATLAB flattening the 1×2×2 singleton dimension on load
+objSigma = reshape(inits.objectiveSigma, [1 2 2]);
+objFcn = objectiveFunctionWrapper(inits.objectivePos, objSigma);
+dom.objective = dom.objective.initialize(objFcn, dom, ...
+    inits.discretizationStep, inits.protectedRange, inits.sensorPerformanceMinimum, ...
+    inits.objectivePos, objSigma);
+
+% ---- Build agents ------------------------------------------------------------
+numAgents = inits.numAgents;
+agentList = cell(numAgents, 1);
+for ii = 1:numAgents
+    pos = inits.pos(ii, :);
+
+    sensor = sigmoidSensor;
+    sensor = sensor.initialize(inits.alphaDist(ii), inits.betaDist(ii), ...
+                               inits.alphaTilt(ii), inits.betaTilt(ii));
+
+    geom = spherical;
+    geom = geom.initialize(pos, inits.collisionRadius(ii), REGION_TYPE.COLLISION, ...
+                           sprintf("UAV %d Collision", ii));
+    ag = agent;
+    ag = ag.initialize(pos, geom, sensor, inits.comRange(ii), inits.maxIter, ...
+                       inits.initialStepSize(ii), sprintf("UAV %d", ii));
+    agentList{ii} = ag;
+end
+
+% ---- Build obstacles ---------------------------------------------------------
+numObstacles = inits.numObstacles;
+obstacleList = cell(numObstacles, 1);
+if numObstacles > 0
+    for ii = 1:numObstacles
+        obs = rectangularPrism;
+        obs = obs.initialize([inits.obsMinCorners(ii, :); inits.obsMaxCorners(ii, :)], ...
+                             REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+        obstacleList{ii} = obs;
+    end
+end
+
+% ---- Optional backward-compat fields -----------------------------------------
+if isfield(inits, 'useDoubleIntegrator')
+    useDoubleIntegrator = logical(inits.useDoubleIntegrator);
+else
+    useDoubleIntegrator = false;
+end
+if isfield(inits, 'dampingCoeff')
+    dampingCoeff = inits.dampingCoeff;
+else
+    dampingCoeff = 2.0;
+end
+if isfield(inits, 'useFixedTopology')
+    useFixedTopology = logical(inits.useFixedTopology);
+else
+    useFixedTopology = false;
+end
+
+% ---- Initialize simulation (plots and video disabled) ------------------------
+obj = obj.initialize(dom, agentList, inits.barrierGain, inits.barrierExponent, ...
+                     inits.minAlt, inits.timestep, inits.maxIter, obstacleList, ...
+                     false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology);
+
+end

@miSim/miSim.m

@@ -7,7 +7,6 @@ classdef miSim
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
         domain;
-        objective;
         obstacles; % geometries that define obstacles within the domain
         agents; % agents that move within the domain
         adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -18,11 +17,18 @@ 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
+        optimizeSensorPointing = false; % false = fixed sensor tilt/azimuth, true = optimize tilt/azimuth via gradient ascent
         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
+        constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
     end
 
     properties (Access = private)
@@ -40,6 +46,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
@@ -48,7 +55,6 @@ classdef miSim
         partitionGraphIndex = 1;
 
         % CBF plotting
-        h; % h function values
         hf; % h function plotting figure
         caPlot; % objects for collision avoidance h function plot
         obsPlot; % objects for obstacle h function plot
@@ -61,12 +67,13 @@ classdef miSim
                 obj (1, 1) miSim
             end
             obj.domain = rectangularPrism;
-            obj.objective = sensingObjective;
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology);
         [obj] = initializeFromCsv(obj, csvPath);
+        [obj] = initializeFromInits(obj, initsPath);
+        [obj] = plotFromSimHist(obj, initsPath, histPath);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
         [obj] = constrainMotion(obj);

@miSim/plotFromSimHist.m

@@ -0,0 +1,93 @@
+function obj = plotFromSimHist(obj, initsPath, histPath)
+% PLOTFROMSIMHIST  Reconstruct all three miSim plots from saved matfiles.
+%
+% Loads the simInits matfile to rebuild domain/obstacle/objective/agent
+% geometry, then loads the simHist matfile to restore the full time-history
+% arrays.  Produces the same three figures that a live run would generate:
+%   1. Sensor performance vs. time  (obj.fPerf)
+%   2. Barrier function values vs. time  (obj.hf)
+%   3. 3-D spatial figure with domain, obstacles, objective, agent trails,
+%      and final-timestep communications topology  (obj.f)
+%
+% Usage:
+%   sim = miSim;
+%   sim = sim.plotFromSimHist( ...
+%       'sandbox/2025_01_01_12_00_00_miSimHist.mat', ...
+%       'sandbox/2025_01_01_12_00_00_miSimInits.mat');
+
+arguments (Input)
+    obj       (1, 1) {mustBeA(obj, 'miSim')};
+    initsPath (1, 1) string;
+    histPath  (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+% ---- Reconstruct geometry from inits (plots disabled) --------------------
+obj = obj.initializeFromInits(initsPath);
+nAgents = size(obj.agents, 1);
+
+% ---- Load history data ---------------------------------------------------
+data = load(histPath);
+out  = data.out;
+
+nHistTimesteps = size(out.barriers, 2);
+nPosTimesteps  = size(out.agent(1).pos, 1);
+
+% ---- Populate barrier history --------------------------------------------
+% out.barriers may be narrower than the pre-allocated obj.barriers if the
+% run was shorter than maxIter; fill what we have and leave the rest NaN.
+obj.barriers(:, 1:nHistTimesteps) = out.barriers;
+
+% ---- Populate position history and advance agents to final positions -----
+for ii = 1:nAgents
+    agentPos = out.agent(ii).pos;           % (nPosTimesteps × 3)
+    nPts = size(agentPos, 1);
+    obj.posHist(ii, 1:nPts, :) = reshape(agentPos, [1, nPts, 3]);
+    obj.agents{ii}.pos = agentPos(end, :);  % show final position in spatial plot
+end
+
+% ---- Set final constraint topology ---------------------------------------
+obj.constraintAdjacencyMatrix = out.constraintAdjacency(:, :, end);
+
+% ---- Recompute partitioning at final agent positions ---------------------
+obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+
+% ---- Enable plotting and produce spatial + barrier figures ---------------
+obj.makePlots = true;
+obj = obj.plot();
+
+% ---- Performance figure (built directly — live machinery is incremental) -
+nPerfTimesteps = numel(out.perf);
+times = (0:nPerfTimesteps - 1) * obj.timestep;
+normFactor = 1 / max(out.perf);
+
+obj.fPerf = figure;
+ax = axes(obj.fPerf);
+hold(ax, "on");
+title(ax,  "Sensor Performance");
+xlabel(ax, "Time (s)");
+ylabel(ax, "Sensor Performance");
+grid(ax, "on");
+
+legendStrings = strings(nAgents + 1, 1);
+legendStrings(1) = "Total";
+plot(ax, times, out.perf * normFactor, "LineWidth", 1.5);
+for ii = 1:nAgents
+    agentPerf  = out.agent(ii).perf;
+    agentTimes = times(1:numel(agentPerf));
+    plot(ax, agentTimes, agentPerf * normFactor);
+    if isfield(out.agent(ii), 'label')
+        legendStrings(ii + 1) = string(out.agent(ii).label);
+    else
+        legendStrings(ii + 1) = sprintf("Agent %d", ii);
+    end
+end
+legend(ax, legendStrings, "Location", "northwest");
+hold(ax, "off");
+
+% Bring spatial figure to the front
+figure(obj.f);
+
+end

@miSim/plotH.m

@@ -6,6 +6,10 @@ function obj = plotH(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
+    nCA  = size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2;
+    nObs = size(obj.agents, 1) * size(obj.obstacles, 1);
+    nDom = size(obj.agents, 1) * 6;
+
     obj.hf = figure;
     tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
 
@@ -15,7 +19,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
+    obj.caPlot = plot(obj.barriers(1:nCA, :)');
     legendStrings = [];
     for ii = 2:size(obj.agents, 1)
         for jj = 1:(ii - 1)
@@ -31,7 +35,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
+    obj.obsPlot = plot(obj.barriers((nCA + 1):(nCA + nObs), :)');
     legendStrings = [];
     for ii = 1:size(obj.obstacles, 1)
         for jj = 1:size(obj.agents, 1)
@@ -47,8 +51,13 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
+    obj.domPlot = plot(obj.barriers((nCA + nObs + 1):(nCA + nObs + nDom), :)');
-    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
+    domLabels = ["X Min", "X Max", "Y Min", "Y Max", "Z Min", "Z Max"];
+    legendStrings = strings(nDom, 1);
+    for ii = 1:size(obj.agents, 1)
+        legendStrings((ii - 1) * 6 + (1:6)) = sprintf("A%d ", ii) + domLabels;
+    end
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));

@miSim/run.m

@@ -10,7 +10,13 @@ function [obj] = run(obj)
         % Start video writer
         if obj.makeVideo
             v = obj.setupVideoWriter();
+            drawnow;
             v.open();
+            % Capture reference frame size; used to resize frames that deviate
+            % due to figure reflow during plot updates (e.g. in headless mode).
+            I_ref = getframe(obj.f);
+            v.writeVideo(I_ref);
+            videoFrameSize = [size(I_ref.cdata, 2), size(I_ref.cdata, 1)];
         end
     end
 
@@ -25,17 +31,31 @@ function [obj] = run(obj)
             obj.validate();
         end
 
+        % Clear RF sensor caches
+        if isa(obj.agents{1}.sensorModel, "rfSensor")
+            for ss = 1:size(obj.agents, 1)
+                obj.agents{ss}.sensorModel = obj.agents{ss}.sensorModel.clearRssCache;
+            end
+        end
+
         % Update partitioning before moving (this one is strictly for
         % plotting purposes, the real partitioning is done by the agents)
-        obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+        [obj.partitioning, obj.agents] = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
         % Determine desired communications links
-        obj = obj.lesserNeighbor();
+        if ~obj.useFixedTopology
+            obj = obj.lesserNeighbor();
+        end
+
+        % Log constraint adjacency for this timestep
+        if coder.target('MATLAB')
+            obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
+        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.useDoubleIntegrator, obj.dampingCoeff, obj.timestep, obj.optimizeSensorPointing, obj.agents([1:(jj - 1), (jj + 1):size(obj.agents, 1)]));
         end
 
         % Adjust motion determined by unconstrained gradient ascent using
@@ -43,8 +63,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))];
@@ -58,15 +79,20 @@ function [obj] = run(obj)
             % Write frame in to video
             if obj.makeVideo
                 I = getframe(obj.f);
+                if size(I.cdata, 2) ~= videoFrameSize(1) || size(I.cdata, 1) ~= videoFrameSize(2)
+                    I.cdata = imresize(I.cdata, [videoFrameSize(2), videoFrameSize(1)]);
+                end
                 v.writeVideo(I);
             end
         end
     end
 
+    % Close video
     if coder.target('MATLAB')
         if obj.makeVideo
             % Close video file
             v.close();
         end
     end
+    
 end

@miSim/teardown.m

@@ -6,25 +6,52 @@ function obj = teardown(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Close plots
+    % % Close plots
-    close(obj.hf);
+    % close(obj.hf);
-    close(obj.fPerf);
+    % close(obj.fPerf);
-    close(obj.f);
+    % 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;
+    out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
+    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;
     obj.constraintAdjacencyMatrix = NaN;
+    obj.constraintAdjacencyHist = [];
     obj.partitioning = NaN;
     obj.performance = 0;
     obj.barrierGain = NaN;
     obj.barrierExponent = NaN;
+    obj.useDoubleIntegrator = false;
+    obj.dampingCoeff = 2.0;
+    obj.useFixedTopology = false;
     obj.artifactName = "";
 
 end

@miSim/updatePlots.m

@@ -61,13 +61,15 @@ function [obj] = updatePlots(obj)
     end
 
     % Update h function plots
-    for ii = 1:size(obj.caPlot, 1)
+    nCA  = size(obj.caPlot, 1);
-        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
+    nObs = size(obj.obsPlot, 1);
+    for ii = 1:nCA
+        obj.caPlot(ii).YData(obj.timestepIndex) = obj.barriers(ii, obj.timestepIndex);
     end
-    for ii = 1:size(obj.obsPlot, 1)
+    for ii = 1:nObs
-        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
+        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + ii, obj.timestepIndex);
     end
     for ii = 1:size(obj.domPlot, 1)
-        obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
+        obj.domPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + nObs + ii, obj.timestepIndex);
     end
 end

@miSim/validate.m

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

@miSim/writeInits.m

@@ -5,29 +5,71 @@ function writeInits(obj)
     arguments (Output)
     end
 
+    % User-supplied obstacles only: initialize() appends a floor obstacle at
+    % the end when minAlt > 0, so exclude it here to avoid double-counting on
+    % reconstruction (initializeFromInits re-adds the floor via minAlt).
+    numInputObs = size(obj.obstacles, 1) - (obj.minAlt > 0);
+    userObstacles = obj.obstacles(1:numInputObs);
+
     % Collect agent parameters
     collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
-    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    if isprop(obj.agents{1}.sensorModel, "alphaDist")
-    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+        % sigmoidSensor parameters
-    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+        alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
-    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+        betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+        alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+        betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents);
+
+        % others to zero
+        lossExponent = zeros(size(obj.agents));
+        P_TX = zeros(size(obj.agents));
+        BW = zeros(size(obj.agents));
+        f_c = zeros(size(obj.agents));
+        G_RX_dBi = zeros(size(obj.agents));
+        beamwidthExponent = zeros(size(obj.agents));
+        
+    elseif isprop(obj.agents{1}.sensorModel, "P_TX")
+        % rfSensor parameters
+        lossExponent = cellfun(@(x) x.sensorModel.lossExponent, obj.agents);
+        P_TX = cellfun(@(x) x.sensorModel.P_TX, obj.agents);
+        BW = cellfun(@(x) x.sensorModel.BW, obj.agents);
+        f_c = cellfun(@(x) x.sensorModel.f_c, obj.agents);
+        G_RX_dBi = cellfun(@(x) x.sensorModel.G_RX_dBi, obj.agents);
+        beamwidthExponent = cellfun(@(x) x.sensorModel.beamwidthExponent, obj.agents);
+
+        % others to zero
+        alphaDist = zeros(size(obj.agents));
+        betaDist = zeros(size(obj.agents));
+        alphaTilt = zeros(size(obj.agents));
+        betaTilt = zeros(size(obj.agents));
+    end
+    % joint parameters
+    tilt = cellfun(@(x) x.sensorModel.tilt, obj.agents);
+    azimuth = cellfun(@(x) x.sensorModel.azimuth, obj.agents);
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
-
+    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, userObstacles, 'UniformOutput', false));
+    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, userObstacles, 'UniformOutput', false));
 
     % Combine with simulation parameters
-    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
+    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter + 1, "minAlt", obj.minAlt, ...
                     "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), ...
+                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ...
-                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, "alphaDist", alphaDist, ...
+                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
-                    "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
+                    "tilt", tilt, "azimuth", azimuth, ... % joint sensor parameters
+                    "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... % sigmoid sensor parameters
+                    "lossExponent", lossExponent, "P_TX", P_TX, "BW", BW, "f_c", f_c, "G_RX_dBi", G_RX_dBi, "beamwidthExponent", beamwidthExponent, ... % RF sensor parameters
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
-                    "pos", pos);
+                    "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
+                    "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ...
+                    "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
+                    "objectiveIntegral", sum(obj.domain.objective.values(:)));
 
     % Save all parameters to output file
     initsFile = strcat(obj.artifactName, "_miSimInits");
     initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile);
     save(initsFile, "-struct", "inits");
-end
+end

@rfSensor/RSS.m

@@ -0,0 +1,24 @@
+function value = RSS(obj, d, dx, dy, dz)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        d  (:, 1) double;
+        dx (:, 1) double;
+        dy (:, 1) double;
+        dz (:, 1) double;
+    end
+    arguments (Output)
+        value (:, 1) double
+    end
+    % Boresight unit vector: [st*sa, st*ca, -ct]
+    % Target direction unit vector: [dx, dy, dz] / d
+    % cos_theta = dot product of the two, computed without per-point trig.
+    st = sind(obj.tilt);
+    ct = cosd(obj.tilt);
+    sa = sind(obj.azimuth);
+    ca = cosd(obj.azimuth);
+    cos_theta = (st .* (dx .* sa + dy .* ca) - ct .* dz) ./ max(d, eps);
+    cos_theta = max(-1, min(1, cos_theta));
+    theta = acosd(cos_theta);
+    gain  = 10 .* obj.beamwidthExponent .* log10((1 + cosd(theta)) ./ 2);
+    value = obj.P_TX_dBm + gain + obj.G_RX_dBi - obj.pathLoss(d);
+end

@rfSensor/clearRssCache.m

@@ -0,0 +1,11 @@
+function obj = clearRssCache(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+    end
+
+    obj.rssCache = double.empty(0, 1);
+
+end

@rfSensor/computePointToPoints.m

@@ -0,0 +1,6 @@
+function [d, dx, dy, dz] = computePointToPoints(~, agentPos, targetPos)
+    dx = targetPos(:,1) - agentPos(1);
+    dy = targetPos(:,2) - agentPos(2);
+    dz = targetPos(:,3) - agentPos(3);
+    d  = sqrt(dx.^2 + dy.^2 + dz.^2);
+end

@rfSensor/halfAngle.m

@@ -0,0 +1,23 @@
+function value = halfAngle(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+    end
+    arguments (Output)
+        value (1, 1) double;
+    end
+    % Sweep angular offset from boresight by evaluating transmitterGain at
+    % (obj.tilt + dtheta, obj.azimuth). The cosine difference identity guarantees
+    % the resulting angular offset from boresight equals dtheta exactly,
+    % independent of the actual pointing direction.
+    dtheta = (0:0.1:179.9)';
+    gain = obj.transmitterGain(obj.tilt + dtheta, obj.azimuth * ones(size(dtheta)));
+    target = gain(1) - 3;
+    idx = find(gain <= target, 1);
+    if isempty(idx) || idx == 1
+        value = dtheta(end);
+        return;
+    end
+    % Linear interpolation between bracketing samples
+    value = dtheta(idx-1) + (target - gain(idx-1)) * ...
+            (dtheta(idx) - dtheta(idx-1)) / (gain(idx) - gain(idx-1));
+end

@rfSensor/initialize.m

@@ -0,0 +1,32 @@
+function obj = initialize(obj, txPower, bandwidth, centerFreq, rxGain_dBi, beamwidthExponent, tilt, azimuth, lossExponent)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")}
+        txPower (1, 1) double;
+        bandwidth (1, 1) double;
+        centerFreq (1, 1) double;
+        rxGain_dBi (1, 1) double;
+        beamwidthExponent (1, 1) double;
+        tilt (1, 1) double = 0;
+        azimuth (1, 1) double = 0;
+        lossExponent (1, 1) double = NaN;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "rfSensor")}
+    end
+
+    %% Provided values
+    obj.P_TX = txPower; % Transmit power (W)
+    obj.BW = bandwidth; % Bandwidth (Hz)
+    obj.f_c = centerFreq; % Center frequency (Hz)
+    obj.G_RX_dBi = rxGain_dBi; % Receiving Antenna Gain (dBi)
+    obj.beamwidthExponent = beamwidthExponent; % Defines how focused the antenna beam is
+    obj.lossExponent = lossExponent;
+
+    % Define initial antenna pointing
+    obj.tilt = tilt;
+    obj.azimuth = azimuth;
+
+    %% Computed values
+    obj.P_TX_dBm = 10*log10(obj.P_TX/1e-3); % Transmit power in dBm
+    obj.N = obj.k_B * obj.T_0 * obj.BW; % Thermal noise
+end

@rfSensor/pathLoss.m

@@ -0,0 +1,13 @@
+function L_FSPL_dB = pathLoss(obj, d)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        d (:, 1) double; % distance from TX to RX
+    end
+    arguments (Output)
+        L_FSPL_dB (:, 1) double
+    end
+
+    % Free Space Path Loss (dB); d clamped away from zero (log undefined at d=0)
+    L_FSPL_dB = obj.lossExponent * 10 * log10(max(d, eps)) + 20 * log10(obj.f_c) + 20 * log10((4*pi)/obj.c);
+
+end

@rfSensor/plot.m

@@ -0,0 +1,125 @@
+function f = plot(obj, altitude, otherSensorsPos, otherSensors)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        altitude (1, 1) double;
+        otherSensorsPos (:, 3) double = NaN(0, 3);
+        otherSensors (:, 1) cell = cell(0, 1);
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+    end
+
+    % Clear local caches so this visualization always uses its own grid
+    obj.rssCache = [];
+    for ii = 1:numel(otherSensors)
+        otherSensors{ii}.rssCache = [];
+    end
+
+    % bias other sensors altitudes appropriately
+    otherSensorsPos = otherSensorsPos + [0, 0, altitude];
+
+    % Create grid on which to evalute SINR, SNR
+    agentPos = [0, 0, altitude];
+    d = 10;
+    if ~isempty(otherSensorsPos)
+        d = max(otherSensorsPos(:, 3) * 0.55);
+        d = max(d, max(vecnorm(otherSensorsPos(:, 1:2), 2, 2)) * 1.25);
+    end
+    c = 0.1;
+    d = ceil(d / c) * c;
+    distances = -d:c:d;
+    [targetPosX, targetPosY] = meshgrid(distances, distances);
+
+    % Compute SINR, SNR
+    [SINR, ~] = obj.sensorPerformance(agentPos, [targetPosX(:), targetPosY(:), zeros(size(targetPosX(:)))], otherSensorsPos, otherSensors);
+    SINR = reshape(SINR, size(targetPosX));
+
+    % normalize in linear scale
+    % SINR = 10.^(SINR/10); SINR = SINR ./ max(SINR(:)); SINR = 10 * log10(SINR);
+
+    % Collect sensor positions and boresight parameters for overlay
+    sensorTilts   = [obj.tilt;    cellfun(@(s) s.tilt,    otherSensors)];
+    sensorAzimuths = [obj.azimuth; cellfun(@(s) s.azimuth, otherSensors)];
+    tailScale = 0.5 * d;
+
+    f = figure;
+    surf(targetPosX, targetPosY, zeros(size(targetPosX)), SINR, "EdgeColor", "none");
+    axis(f.Children(1), "image");
+    colormap(f.Children(1), "hot");
+    title("Ground User SINR and -3 dB antenna gain regions");
+    subtitle(sprintf("%d interfering source(s)", size(otherSensorsPos, 1)));
+    c = colorbar;
+    ylabel(c, "SINR (dB)");
+    xlabel("X (m)");
+    ylabel("Y (m)");
+    hold(f.Children(2), "on");
+    scatter3(0, 0, altitude, 100, 'ko', "LineWidth", 2);
+    scatter3(otherSensorsPos(:, 1), otherSensorsPos(:, 2), otherSensorsPos(:, 3), 100, "bx", "LineWidth", 2);
+    qSelf = quiver3(0, 0, altitude, ...
+        tailScale * sind(obj.tilt) * sind(obj.azimuth), ...
+        tailScale * sind(obj.tilt) * cosd(obj.azimuth), ...
+        -tailScale * cosd(obj.tilt), ...
+        0, 'k', 'LineWidth', 1.5);
+    qSelf.MaxHeadSize = 0.75;
+    if ~isempty(otherSensors)
+        qOthers = quiver3(otherSensorsPos(:,1), otherSensorsPos(:,2), otherSensorsPos(:,3), ...
+            tailScale .* sind(sensorTilts(2:end)) .* sind(sensorAzimuths(2:end)), ...
+            tailScale .* sind(sensorTilts(2:end)) .* cosd(sensorAzimuths(2:end)), ...
+            -tailScale .* cosd(sensorTilts(2:end)), ...
+            0, 'b', 'LineWidth', 1.5);
+        qOthers.MaxHeadSize = 0.75;
+    end
+    % Draw half-angle cones co-boresighted with each quiver arrow
+    N = 48;
+    phi = linspace(0, 2*pi, N);
+    [PHI, S] = meshgrid(phi, [0; 1]);   % row 1 = apex (s=0), row 2 = base (s=1)
+    allSensors = [{obj}; otherSensors];
+    allPos     = [[0, 0, altitude]; otherSensorsPos];
+    for ii = 1:numel(allSensors)
+        ha  = allSensors{ii}.halfAngle();
+        tlt = sensorTilts(ii);
+        az  = sensorAzimuths(ii);
+        pos = allPos(ii, :);
+        % Cone length: enough that the axis tip is guaranteed below z=0
+        coneLength = 1.1 * pos(3) / max(cosd(tlt), 0.1);
+        % Nadir cone mesh: apex at origin, base at z = -coneLength
+        cX = S .* coneLength .* tand(ha) .* cos(PHI);
+        cY = S .* coneLength .* tand(ha) .* sin(PHI);
+        cZ = -S .* coneLength;
+        % Rotate nadir → boresight (same convention as quiver arrows)
+        Ry = [cosd(tlt), 0, -sind(tlt); 0, 1, 0; sind(tlt), 0, cosd(tlt)];
+        Rz = [sind(az), -cosd(az), 0; cosd(az), sind(az), 0; 0, 0, 1];
+        R  = Rz * Ry;
+        pts = R * [cX(:)'; cY(:)'; cZ(:)'];
+        cX  = reshape(pts(1,:), size(cX)) + pos(1);
+        cY  = reshape(pts(2,:), size(cY)) + pos(2);
+        cZ  = reshape(pts(3,:), size(cZ)) + pos(3);
+        if ii == 1
+            fc = [0, 0, 0];
+        else
+            fc = [0, 0, 1];
+        end
+        surf(cX, cY, cZ, "FaceColor", fc, "FaceAlpha", 0.15, "EdgeColor", "none");
+
+        % Conic section: intersect each cone generator with z=0
+        b_vec = R * [0; 0; -1];
+        u_vec = R * [1; 0; 0];
+        v_vec = R * [0; 1; 0];
+        phi_sec = linspace(0, 2*pi, 720)';
+        dirs = cosd(ha) .* b_vec' + sind(ha) .* (cos(phi_sec) .* u_vec' + sin(phi_sec) .* v_vec');
+        t_sec = -pos(3) ./ dirs(:, 3);
+        t_sec(t_sec <= 0) = NaN;
+        sx = pos(1) + t_sec .* dirs(:, 1);
+        sy = pos(2) + t_sec .* dirs(:, 2);
+        plot3(sx, sy, zeros(size(sx)), "Color", fc, "LineWidth", 2);
+    end
+    clim(f.Children(2), [min(SINR(:)), max(SINR(:))]);
+    xlim(f.Children(2), [-d, d]);
+    ylim(f.Children(2), [-d, d]);
+    hold(f.Children(2), "off");
+    zlim([0, Inf]);
+    
+
+
+
+end

@rfSensor/plotParameters.m

@@ -0,0 +1,52 @@
+function f = plotParameters(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+    end
+
+    % Agent altitude layers and angle sample points
+    alt_values = 10.^[1, 2, 3, 4];
+    t_values = 0:2.5:87.5;   % 0=nadir (center), <90=near horizon (edge)
+    a_values = 0:2.5:360;
+
+    [T, A] = meshgrid(t_values, a_values);   % Naz x Nel
+    Ar = deg2rad(A);
+
+    f = figure;
+    hold("on");
+
+    for ii = 1:numel(alt_values)
+        alt = alt_values(ii);
+
+        % For agent at altitude alt, ground target at tilt T has slant distance:
+        D = alt ./ cosd(T);
+
+        % Compute RSS for each (d, t, a) triple
+        rss = obj.RSS(D(:), T(:), A(:));
+        Fslice = reshape(rss, size(D));
+
+        % Disc geometry: t=0 (nadir) -> center, t~90 (horizon) -> edge
+        r = log10(alt) .* T ./ 90;
+        X = r .* cos(Ar);
+        Y = r .* sin(Ar);
+        Z = log10(alt) * ones(size(X));
+
+        hs = surf(X, Y, Z, Fslice);
+        hs.EdgeColor = 'none';
+        hs.FaceColor = 'interp';
+        hs.FaceAlpha = 0.25;
+    end
+
+    colormap(turbo);
+    c = colorbar; c.Label.String = "Received Signal Strength (dB)";
+    daspect([1 1 0.2]);
+    xlabel('X (log_{10} units)'); ylabel('Y (log_{10} units)'); zlabel('log_{10} Altitude (m)');
+    set(gca, 'ZDir', 'reverse');
+    view(3);
+    axis("vis3d");
+    grid("on");
+    scatter3(0, 0, 0, 'rx');
+    hold("off");
+end

@rfSensor/plotPerformance.m

@@ -0,0 +1,91 @@
+function f = plotPerformance(obj, altitude, otherSensorsPos, otherSensors)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        altitude (1, 1) double;
+        otherSensorsPos (:, 3) double = NaN(0, 3);
+        otherSensors (:, 1) cell = cell(0, 1);
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+    end
+
+    % Clear local caches so this visualization always uses its own grid
+    obj.rssCache = [];
+    for ii = 1:numel(otherSensors)
+        otherSensors{ii}.rssCache = [];
+    end
+
+    % bias other sensors altitudes appropriately
+    otherSensorsPos = otherSensorsPos + [0, 0, altitude];
+
+    % Create grid on which to evalute SINR, SNR
+    agentPos = [0, 0, altitude];
+    d = 10;
+    if ~isempty(otherSensorsPos)
+        d = max(d, max(vecnorm(otherSensorsPos(:, 1:2), 2, 2)) * 1.25);
+    end
+    c = 0.1;
+    d = ceil(d / c) * c;
+    distances = -d:c:d;
+    [targetPosX, targetPosY] = meshgrid(distances, distances);
+
+    % Compute SINR, SNR
+    [SINR, SNR] = obj.sensorPerformance(agentPos, [targetPosX(:), targetPosY(:), zeros(size(targetPosX(:)))], otherSensorsPos, otherSensors);
+    SINR = reshape(SINR, size(targetPosX));
+    SNR = reshape(SNR, size(targetPosX));
+
+    % normalize in linear scale
+    SINR = 10.^(SINR/10); SINR = SINR ./ max(SINR(:)); SINR = 10 * log10(SINR);
+    SNR  = 10.^(SNR/10);  SNR  = SNR  ./ max(SNR(:));  SNR  = 10 * log10(SNR);
+
+    % Collect sensor positions and boresight parameters for overlay
+    sensorXY      = [0, 0; otherSensorsPos(:, 1:2)];
+    sensorTilts   = [obj.tilt;    cellfun(@(s) s.tilt,    otherSensors)];
+    sensorAzimuths = [obj.azimuth; cellfun(@(s) s.azimuth, otherSensors)];
+    tailScale = 0.5 * d;
+
+    f = figure;
+    tiledlayout(1, 2, TileSpacing="compact", Padding="compact");
+
+    nexttile;
+    imagesc(distances, distances, SNR);
+    axis("image"); set(gca, 'YDir', 'normal');
+    colorbar; xlabel("X (m)"); ylabel("Y (m)");
+    title("Linearly Normalized SNR (dB)");
+    subtitle("No interfering sources");
+    addSensorOverlay(gca, sensorXY(1, 1:2), sensorTilts(1, 1), sensorAzimuths(1, 1), tailScale);
+
+    nexttile;
+    imagesc(distances, distances, SINR);
+    axis("image"); set(gca, 'YDir', 'normal');
+    colorbar; xlabel("X (m)"); ylabel("Y (m)");
+    title("Linearly Normalized SINR (dB)");
+    subtitle(sprintf("%d interfering source(s)", size(otherSensorsPos, 1)));
+    addSensorOverlay(gca, sensorXY, sensorTilts, sensorAzimuths, tailScale);
+end
+
+function addSensorOverlay(ax, sensorXY, tilts, azimuths, tailScale)
+    % Draw a marker + boresight arrow for each sensor.
+    % Tail direction follows azimuth convention (0=+Y, 90=+X, clockwise).
+    % Tail length = tailScale * sind(tilt), so nadir (0°) has no tail and
+    % horizon (90°) has the full tailScale length.
+    hold(ax, 'on');
+    for ii = 1:size(sensorXY, 1)
+        x = sensorXY(ii, 1);
+        y = sensorXY(ii, 2);
+        if ii == 1
+            c = [0, 0, 0];
+            mk = 'o';
+        else
+            c = [0.9, 0.2, 0.2];
+            mk = 'x';
+        end
+        scatter(ax, x, y, 80, c, mk, LineWidth=2);
+        if tilts(ii) > 0
+            u = tailScale * sind(tilts(ii)) * sind(azimuths(ii));
+            v = tailScale * sind(tilts(ii)) * cosd(azimuths(ii));
+            quiver(ax, x, y, u, v, 0, Color=c, LineWidth=2, MaxHeadSize=1.0);
+        end
+    end
+    hold(ax, 'off');
+end

@rfSensor/rfSensor.m

@@ -0,0 +1,40 @@
+classdef rfSensor
+    properties (SetAccess = private, GetAccess = public)
+        % Physical parameters
+        c = 3e8; % Speed of light (m/s)
+        k_B = 1.38e-23 % Boltzmann constant (W/Hz/K) for thermal noise model
+        T_0 = 300; % Ambient temperature (Kelvin) for thermal noise model
+        lossExponent = NaN; % Path loss exponent (2 for free space, up to 6 for the lossiest environments)
+        % Sensor parameters
+        P_TX = NaN; % Transmit power (Watts)
+        BW = NaN; % Bandwidth (Hz)
+        f_c = NaN; % Center frequency (Hz)
+        G_RX_dBi = NaN; % Receiver antenna gain
+        beamwidthExponent = NaN; % Antenna beamwidth exponent for cosine radiation pattern, larger exponent -> narrower beam
+        % Values computed at initialization
+        P_TX_dBm = NaN; % Transmit power (dBm)
+        N = NaN; % Thermal noise
+        % Cached state (per timestep)
+    end
+    properties (Access = public)
+        tilt = NaN;     % Antenna boresight tilt (deg): 0=nadir, 90=horizon
+        azimuth = NaN;  % Antenna boresight azimuth (deg): 0=+y, 90=+x, 180=-y, 270=-x
+        rssCache (:,1) double = double.empty(0,1); % linear-scale RSS to last ground targets grid
+    end
+
+    methods (Access = public)
+        [obj] = initialize(obj, txPower, bandwidth, centerFreq, rxGain, beamwidthExponent, tilt, azimuth); % initialize sensor, define parameters
+        [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targetPos, otherSensorsPos, otherSensors); % determine sensor performance for a given single sensor and target geometry
+        [d, dx, dy, dz] = computePointToPoints(obj, agentPos, targetPos);
+        [value] = halfAngle(obj); % tilt angle (deg) at which sensor performance is halved
+        [f] = plotParameters(obj); % debug, plot sensor response as a function of distance and tilt angle
+        [f] = plotPerformance(obj, altitude, otherSensorsPos, otherSensors); % debug, plot SNR or SINR ground heatmap for a given geometry
+        [f] = plot(obj, altitude, otherSensorsPos, otherSensors);
+        obj = clearRssCache(obj);
+    end
+    methods (Access = private)
+        x = RSS(obj, d, dx, dy, dz); % Received signal strength (function of distance and tilt angle)
+        G_TX_dB = transmitterGain(obj, t, a); % Antenna gain for a given TX/RX pair 
+        L_FSPL_dB = pathLoss(obj, d); % Free space path loss for a given TX/RX pair
+    end
+end

@rfSensor/sensorPerformance.m

@@ -0,0 +1,34 @@
+function [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targetPos, otherSensorsPos, otherSensors)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        agentPos (1, 3) double;
+        targetPos (:, 3) double;
+        otherSensorsPos (:, 3) double = [];
+        otherSensors (:, 1) cell = {};
+    end
+    arguments (Output)
+        SINR (:, 1) double;
+        SNR (:, 1) double;
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        otherSensors (:, 1) cell;
+    end
+    assert(size(otherSensorsPos, 1) == size(otherSensors, 1), "Mismatch in number of other sensor positions (%d) and number of other sensors (%d) provided", size(otherSensorsPos, 1), size(otherSensors, 1));
+
+    if isempty(obj.rssCache)
+        [d, dx, dy, dz] = obj.computePointToPoints(agentPos, targetPos);
+        obj.rssCache = 1e-3 .* 10 .^ (0.1 .* obj.RSS(d, dx, dy, dz));  % dBm → W
+    end
+    S = obj.rssCache;
+
+    I = zeros(size(S));
+    for ii = 1:size(otherSensors, 1)
+        if isempty(otherSensors{ii}.rssCache)
+            [d_o, dx_o, dy_o, dz_o] = otherSensors{ii}.computePointToPoints(otherSensorsPos(ii, 1:3), targetPos);
+            otherSensors{ii}.rssCache = 1e-3 .* 10 .^ (0.1 .* otherSensors{ii}.RSS(d_o, dx_o, dy_o, dz_o));  % dBm → W
+        end
+        I = I + otherSensors{ii}.rssCache;
+    end
+
+    SINR = 10*log10(S ./ (I + obj.N));
+    SNR  = 10*log10(S ./ obj.N);
+end

@rfSensor/transmitterGain.m

@@ -0,0 +1,23 @@
+function value = transmitterGain(obj, t, a)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rfSensor")};
+        t (:, 1) double; % LOS tilt angle
+        a (:, 1) double; % LOS azimuth angle
+    end
+    arguments (Output)
+        value (:, 1) double
+    end
+    if ~isequal(size(t), size(a))
+        error("t and a must be the same size");
+    end
+
+    % Angular offset from boresight via spherical law of cosines
+    % Convention: t=0° nadir, t=90° horizon; a=0° +y, a=90° +x
+    cos_theta = sind(obj.tilt) .* sind(t) .* cosd(a - obj.azimuth) + ...
+                cosd(obj.tilt) .* cosd(t);
+    cos_theta = max(-1, min(1, cos_theta));  % clamp for numerical safety
+    theta = acosd(cos_theta);
+
+    % Cardioid family: peak at boresight (theta=0), null opposite (theta=180°)
+    value = 10 .* obj.beamwidthExponent .* log10((1 + cosd(theta)) ./ 2);
+end

@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")};
@@ -36,9 +38,14 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     obj.values = obj.values ./ max(obj.values, [], "all");
 
     % store ground position
-    idx = obj.values == 1;
+    idx = obj.values == 1;    
-    obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    if any(isnan(objectiveMu))
-    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+        obj.groundPos = [obj.X(idx), obj.Y(idx)];
+        obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
+    else
+        obj.groundPos = objectiveMu;
+    end
+    obj.objectiveSigma = objectiveSigma;
 
-    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective");
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -11,16 +11,16 @@ 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
 
     % Set random distribution parameters
-    sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
+    sig = reshape([2 + rand * 2, 1; 1, 2 + rand * 2], [1 2 2]);
 
     % Set up random bivariate normal distribution function
     objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
-    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange, 1e-6, mu(1:2), sig);
 end

@sensingObjective/plot.m

@@ -11,19 +11,26 @@ function f = plot(obj, ind, f)
     % Create axes if they don't already exist
     f = firstPlotSetup(f);
 
+    normalized = obj.values ./ sum(obj.values, "all");
+    cRange = [min(normalized, [], "all"), max(normalized, [], "all")];
+
     % Plot gradient on the "floor" of the domain
     if isnan(ind)
-        hold(f.CurrentAxes, "on");
+        ax = f.CurrentAxes;
-        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        hold(ax, "on");
+        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        hold(f.CurrentAxes, "off");
+        clim(ax, cRange);
+        hold(ax, "off");
     else
-        hold(f.Children(1).Children(ind(1)), "on");
+        ax = f.Children(1).Children(ind(1));
-        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        hold(ax, "on");
+        o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
         o.HitTest = "off";
         o.PickableParts = "none";
-        hold(f.Children(1).Children(ind(1)), "off");
+        clim(ax, cRange);
+        hold(ax, "off");
     end
 
     % Add to other perspectives

@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 = [];

@sigmoidSensor/halfAngle.m

@@ -0,0 +1,9 @@
+function value = halfAngle(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+    end
+    arguments (Output)
+        value (1, 1) double;
+    end
+    value = obj.alphaTilt;
+end

@sigmoidSensor/initialize.m

@@ -1,17 +1,24 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
+        tilt (1, 1) double = 0;
+        azimuth (1, 1) double = 0;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
     end
     
+    % Sensor performance parameters
     obj.alphaDist = alphaDist;
     obj.betaDist = betaDist;
     obj.alphaTilt = alphaTilt;
     obj.betaTilt = betaTilt;
+
+    % Sensor pointing parameters
+    obj.tilt = tilt;
+    obj.azimuth = azimuth;
 end

@sigmoidSensor/sensorPerformance.m

@@ -8,16 +8,20 @@ function value = sensorPerformance(obj, agentPos, targetPos)
         value (:, 1) double;
     end
 
-    % compute direct distance and distance projected onto the ground
+    % Unit vectors from agent to each target
-    d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
+    diffs = targetPos - agentPos;
-    x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
+    d = vecnorm(diffs, 2, 2);
+    dirs = diffs ./ d;
 
-    % compute tilt angle
+    % Boresight unit vector: tilt=0 → nadir [0,0,-1]; azimuth 0=+Y, 90=+X clockwise
-    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
+    boresight = [sind(obj.tilt)*sind(obj.azimuth), sind(obj.tilt)*cosd(obj.azimuth), -cosd(obj.tilt)];
+
+    % Angular offset from boresight to each target direction
+    angularOffset = acosd(dirs * boresight');
 
     % Membership functions
     mu_d = obj.distanceMembership(d);
-    mu_t = obj.tiltMembership(tiltAngle);
+    mu_t = obj.tiltMembership(angularOffset);
 
     value = mu_d .* mu_t; % assume pan membership is always 1
 end

@sigmoidSensor/sigmoidSensor.m

@@ -6,14 +6,20 @@ classdef sigmoidSensor
         alphaTilt = NaN; % degrees
         betaTilt = NaN;
     end
+    properties (Access = public)
+        % pointing states
+        tilt = 0;
+        azimuth = 0;
+    end
 
     methods (Access = public)
-        [obj]               = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt);
+        [obj]   = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt, tilt, azimuth); % initialize sensor, define parameters
-        [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
+        [value] = sensorPerformance(obj, agentPos, targetPos); % determine sensor performance for a given single sensor and target geometry
-        [f] = plotParameters(obj);
+        [value] = halfAngle(obj); % tilt angle (deg) at which sensor performance is halved
+        [f]     = plotParameters(obj); % debug, plot sensor response as a function of distance and tilt angle
     end
     methods (Access = private)
-        x = distanceMembership(obj, d);
+        x = distanceMembership(obj, d); % used in computing distance factor of sensor performance
-        x = tiltMembership(obj, t);
+        x = tiltMembership(obj, t); % used in computing tilt factor of sensor performance
     end
 end

aerpaw/client/uav_runner.py

@@ -164,7 +164,7 @@ class UAVRunner(BasicRunner):
 
         # Retry connection up to 10 times (~30 seconds total)
         reader, writer = None, None
-        for attempt in range(10):
+        for attempt in range(100):
             try:
                 reader, writer = await asyncio.wait_for(
                     asyncio.open_connection(self.server_ip, self.server_port),

aerpaw/config/client1.yaml

@@ -12,8 +12,8 @@ tdm:
 
 # ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
 origin:
-  lat: 35.72550610629396
+  lat: 35.72595214250436
-  lon: -78.70019657805574
+  lon: -78.69917609299937
   alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
 # Environment-specific settings
 environments:
@@ -28,11 +28,11 @@ environments:
       port: 5000
 
   testbed:
-    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects to us (UDP)
     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.X.1, generally)
     controller:
-      ip: "192.168.122.1"
+      ip: "192.168.112.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
+  lat: 35.72595214250436
-  lon: -78.70019657805574
+  lon: -78.69917609299937
   alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
 # Environment-specific settings
 environments:
@@ -28,11 +28,11 @@ environments:
       port: 5000
 
   testbed:
-    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects to us (UDP)
     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.X.1, generally)
     controller:
-      ip: "192.168.122.1"
+      ip: "192.168.112.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
+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, 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"
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_columns.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_lock.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_two_around_wall.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 100, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 80.0", "0.25, 0.25", "8.0, 8.0", "0.1, 0.1", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "66.6, 66.6", "55, 35, 35, 55", 0.15, "15.0, 15.0, 50.0, 40.0, 15.0, 50.0", 1, "0.0, 35.0, 0.0", "50, 40.0, 60", 1, 2.0, 1

aerpaw/config/scenario_walls.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1

aerpaw/controller.coderprj

@@ -133,6 +133,46 @@
                   <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 id="28" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="29" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="30" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="31" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="32" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="33" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
+                <Types id="34" 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>
@@ -374,719 +414,773 @@
                 </Artifacts>
                 <Artifacts id="40" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="41" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/cone.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="42" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="43" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="44" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="45" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="46" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="47" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="48" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="49" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="50" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="51" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="52" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="53" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="54" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="55" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="56" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="57" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="58" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="59" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/dot.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="60" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="61" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/driver.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="62" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="63" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/exp.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="64" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="65" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/eye.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="66" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="67" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="68" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="69" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="70" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="71" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="72" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="73" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/find.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="74" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="75" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="76" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="77" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="78" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="79" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="80" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="81" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="82" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="83" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="84" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="85" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="86" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="87" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="88" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="89" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="90" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="91" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="92" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="93" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="94" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="95" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="96" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="97" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="98" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="99" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="100" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="101" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/norm.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="102" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="103" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/partition.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="104" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="105" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="106" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="107" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="108" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="109" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="110" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="111" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="112" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="113" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="114" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="115" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="116" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="117" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="118" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="119" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="120" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="121" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="122" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="123" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="124" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="125" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/run.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="126" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="127" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="128" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="129" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="130" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="131" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="132" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="133" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="134" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="135" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sort.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="136" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="137" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="138" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="139" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="140" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="141" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="142" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="143" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/string1.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="144" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="145" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/sum.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="146" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/triu.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="147" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/triu.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="148" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="149" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/unique.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="150" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="151" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="152" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="153" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="154" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="155" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="156" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="157" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/matlab/R2025a/extern/include/tmwtypes.h</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="158" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
-                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp</Path>
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp</Path>
                   </File>
                   <Type>GENERATED_SOURCE</Type>
                 </Artifacts>
                 <Artifacts id="159" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="160" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="161" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="162" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="163" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="164" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="165" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="166" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/matlab/R2025a/extern/include/tmwtypes.h</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="167" type="coderapp.internal.build.mfz.CodegenArtifact">
+                  <File type="coderapp.internal.util.mfz.FileSpec">
+                    <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp</Path>
+                  </File>
+                  <Type>GENERATED_SOURCE</Type>
+                </Artifacts>
+                <Artifacts id="168" type="coderapp.internal.build.mfz.CodegenArtifact">
                   <File type="coderapp.internal.util.mfz.FileSpec">
                     <Path>/home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h</Path>
                   </File>
@@ -1094,7 +1188,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-03-03T19:58:03</Timestamp>
+                <Timestamp>2026-04-07T22:43:01</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

aerpaw/controller.m

@@ -7,34 +7,47 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-MAX_TARGETS = MAX_CLIENTS;
+% Three waypoints per UAV: one axis-aligned move per dimension (taxicab flyout/flyback)
+MAX_TARGETS = MAX_CLIENTS * 3;
+
+% Taxicab flyout/flyback only supports exactly 2 UAVs
+if numClients ~= int32(2)
+    error('Taxicab flyout/flyback requires exactly 2 UAVs');
+end
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
 numWaypoints = int32(0);
 totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
 
-% Load initial UAV positions from scenario CSV
+% Experiment start positions from scenario CSV (N x 3)
+scenarioPositions = zeros(MAX_CLIENTS, 3);
+
+% Load experiment start positions from scenario CSV
 if coder.target('MATLAB')
     disp('Loading initial positions from scenario.csv (simulation)...');
     tmpSim = miSim;
     sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
     flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
-    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
+    posMatrix = reshape(flatPos, 3, [])';   % N×3
     totalLoaded = int32(size(posMatrix, 1));
+    scenarioPositions(1:totalLoaded, :) = posMatrix;
+    % MATLAB path: send directly to scenario positions in one waypoint
     targets(1:totalLoaded, :) = posMatrix;
     numWaypoints = int32(1);
     disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
 else
     coder.cinclude('controller_impl.h');
     filename = ['config/scenario.csv', char(0)];
+    % Load into targets temporarily; copy to scenarioPositions below
     totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
                 coder.ref(targets), int32(MAX_TARGETS));
-    numWaypoints = totalLoaded / int32(numClients);
+    scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    numWaypoints = int32(3);
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 45;
+NUM_SCENARIO_PARAMS = 55;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
@@ -68,6 +81,46 @@ for i = 1:numClients
     end
 end
 
+% Query takeoff-pad GPS positions before sending any waypoints.
+% These are also the flyback targets so each UAV lands where it took off.
+initialPositions = zeros(MAX_CLIENTS, 3);
+if ~coder.target('MATLAB')
+    coder.ceval('sendRequestPositions', int32(numClients));
+    coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS));
+else
+    % Simulation: treat scenario positions as the takeoff locations
+    initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
+end
+
+% ---- Build taxicab flyout waypoints ------------------------------------------
+% Determine which UAV has the higher final altitude; it moves Z first so it
+% clears vertical separation before the lower UAV converges on the same X/Y.
+%   Higher UAV order: Z → Y → X
+%   Lower  UAV order: X → Y → Z
+if ~coder.target('MATLAB')
+    if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
+        higherIdx = int32(1);
+        lowerIdx  = int32(2);
+    else
+        higherIdx = int32(2);
+        lowerIdx  = int32(1);
+    end
+
+    hBase = double(higherIdx - 1) * double(numWaypoints);
+    lBase = double(lowerIdx  - 1) * double(numWaypoints);
+
+    % Higher UAV: Z first
+    targets(hBase + 1, :) = [initialPositions(higherIdx,1), initialPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 2, :) = [initialPositions(higherIdx,1), scenarioPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 3, :) =  scenarioPositions(higherIdx, :);
+
+    % Lower UAV: X first
+    targets(lBase + 1, :) = [scenarioPositions(lowerIdx,1), initialPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
+    targets(lBase + 2, :) = [scenarioPositions(lowerIdx,1), scenarioPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
+    targets(lBase + 3, :) =  scenarioPositions(lowerIdx, :);
+end
+% ------------------------------------------------------------------------------
+
 % Waypoint loop: send each waypoint to all clients, wait for all to arrive
 for w = 1:numWaypoints
     % Send TARGET for waypoint w to each client
@@ -78,7 +131,7 @@ for w = 1:numWaypoints
         target = targets(targetIdx, :);
 
         if coder.target('MATLAB')
-            disp([datestr(now, 'HH:MM:SS'), ' Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
+            disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
                   num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
         else
             coder.ceval('sendTarget', int32(i), coder.ref(target));
@@ -103,8 +156,13 @@ for w = 1:numWaypoints
 end
 
 % ---- Phase 2: miSim guidance loop ----------------------------------------
-% Guidance parameters (adjust here and recompile as needed)
+% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2))
-MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+% so the controller and the agent step-decay schedule stay in sync.
+if coder.target('MATLAB')
+    MAX_GUIDANCE_STEPS = int32(sc.maxIter);
+else
+    MAX_GUIDANCE_STEPS = int32(scenarioParams(2));
+end
 
 % Enter guidance mode on all clients
 if ~coder.target('MATLAB')
@@ -117,14 +175,18 @@ if ~coder.target('MATLAB')
     coder.ceval('sendRequestPositions', int32(numClients));
     coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
 else
-    % Simulation: seed positions from CSV waypoints so agents don't start at origin
+    % Simulation: seed positions from scenario positions so agents don't start at origin
-    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
 end
 guidance_step(positions(1:numClients, :), true, ...
               scenarioParams, obstacleMin, obstacleMax, numObstacles);
 
 % Main guidance loop (event-triggered)
 for step = 1:MAX_GUIDANCE_STEPS
+    if ~coder.target('MATLAB')
+        coder.ceval('setGuidanceStep', int32(step), int32(MAX_GUIDANCE_STEPS));
+    end
+
     % Run one guidance step: feed current GPS positions in, get targets out
     nextPositions = guidance_step(positions(1:numClients, :), false, ...
                                   scenarioParams, obstacleMin, obstacleMax, numObstacles);
@@ -135,7 +197,7 @@ for step = 1:MAX_GUIDANCE_STEPS
         if ~coder.target('MATLAB')
             coder.ceval('sendTarget', int32(i), coder.ref(target));
         else
-            disp([datestr(now, 'HH:MM:SS'), ' [guidance] target UAV ', num2str(i), ': ', num2str(target)]);
+            disp(['[step ', num2str(step), '] target UAV ', num2str(i), ': ', num2str(target)]);
         end
     end
 
@@ -164,6 +226,51 @@ if ~coder.target('MATLAB')
     % last guidance navigation and is back in sequential (ACK/READY) mode.
     coder.ceval('waitForAllMessageType', int32(numClients), ...
                 int32(MESSAGE_TYPE.ACK));
+    % Reset step counter so post-guidance logging carries no step prefix.
+    coder.ceval('setGuidanceStep', int32(0), int32(MAX_GUIDANCE_STEPS));
+end
+% --------------------------------------------------------------------------
+
+% ---- Taxicab flyback: return each UAV to its takeoff-pad position ---------
+% The UAV that ended guidance at the higher altitude moves Z last (X → Y → Z)
+% so it stays high while the lower UAV descends first, maintaining separation
+% as both converge back on their respective home X/Y positions.
+NUM_RETURN_WP = int32(3);
+returnTargets = zeros(MAX_TARGETS, 3);
+
+if ~coder.target('MATLAB')
+    if positions(1, 3) >= positions(2, 3)
+        higherRetIdx = int32(1);
+        lowerRetIdx  = int32(2);
+    else
+        higherRetIdx = int32(2);
+        lowerRetIdx  = int32(1);
+    end
+
+    hRetBase = double(higherRetIdx - 1) * double(NUM_RETURN_WP);
+    lRetBase = double(lowerRetIdx  - 1) * double(NUM_RETURN_WP);
+
+    % Higher post-guidance UAV: X → Y → Z (descend last)
+    returnTargets(hRetBase + 1, :) = [initialPositions(higherRetIdx,1), positions(higherRetIdx,2),        positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 2, :) = [initialPositions(higherRetIdx,1), initialPositions(higherRetIdx,2), positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 3, :) =  initialPositions(higherRetIdx, :);
+
+    % Lower post-guidance UAV: Z → Y → X (descend first)
+    returnTargets(lRetBase + 1, :) = [positions(lowerRetIdx,1),        positions(lowerRetIdx,2),        initialPositions(lowerRetIdx,3)];
+    returnTargets(lRetBase + 2, :) = [positions(lowerRetIdx,1),        initialPositions(lowerRetIdx,2), initialPositions(lowerRetIdx,3)];
+    returnTargets(lRetBase + 3, :) =  initialPositions(lowerRetIdx, :);
+
+    for w = 1:NUM_RETURN_WP
+        for i = 1:numClients
+            retIdx = double(i - 1) * double(NUM_RETURN_WP) + w;
+            retTarget = returnTargets(retIdx, :);
+            coder.ceval('sendTarget', int32(i), coder.ref(retTarget));
+        end
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
+    end
+else
+    disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.');
 end
 % --------------------------------------------------------------------------
 

aerpaw/guidance_step.m

@@ -29,6 +29,9 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                                               39-40  objectivePos
 %                                               41-44  objectiveVar (2x2, col-major)
 %                                               45     sensorPerformanceMinimum
+%                                               46     useDoubleIntegrator
+%                                               47     dampingCoeff
+%                                               48     useFixedTopology
 %   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
 %   obstacleMax       (MAX_OBSTACLES × 3) double
 %   numObstacles      (1,1) int32                 actual obstacle count
@@ -91,26 +94,34 @@ if isInit
         BETA_TILT_VEC        = scenarioParams(29:32);
         DOMAIN_MIN                  = scenarioParams(33:35);
         DOMAIN_MAX                  = scenarioParams(36:38);
-        OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
+        NUM_OBJ_COMPONENTS          = int32(scenarioParams(39));
-        OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
+        OBJECTIVE_POS_FLAT          = scenarioParams(40:43); % [x1,y1,x2,y2]; zero-padded if N=1
-        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+        OBJECTIVE_VAR_FLAT          = scenarioParams(44:51); % [v11,v12,v21,v22 per component]
+        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(52);
+        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(53));
+        DAMPING_COEFF               = scenarioParams(54);
+        USE_FIXED_TOPOLOGY          = logical(scenarioParams(55));
 
         % --- Build domain geometry ---
         dom = rectangularPrism;
         dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
 
-        % --- Build sensing objective (inline Gaussian; codegen-compatible) ---
+        % --- Build sensing objective: sum of N bivariate Gaussians (codegen-compatible) ---
         dom.objective = sensingObjective;
         xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]);
         yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]);
         [gridX, gridY] = meshgrid(xGrid, yGrid);
-        dx = gridX - OBJECTIVE_GROUND_POS(1);
+        objValues = zeros(size(gridX));
-        dy = gridY - OBJECTIVE_GROUND_POS(2);
+        for kk = 1:NUM_OBJ_COMPONENTS
-        % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv())
+            pos_k = OBJECTIVE_POS_FLAT((kk-1)*2+1 : (kk-1)*2+2);
-        ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2);
+            var_k = reshape(OBJECTIVE_VAR_FLAT((kk-1)*4+1 : (kk-1)*4+4), 2, 2);
-        ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2);
+            dx = gridX - pos_k(1);
-        ov_det = ov_a * ov_d - ov_b * ov_c;
+            dy = gridY - pos_k(2);
-        objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+            ov_a = var_k(1,1); ov_b = var_k(1,2);
+            ov_c = var_k(2,1); ov_d = var_k(2,2);
+            ov_det = ov_a * ov_d - ov_b * ov_c;
+            objValues = objValues + exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+        end
         dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
             DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
 
@@ -146,7 +157,8 @@ if isInit
         % --- Initialise simulation (plots and video disabled) ---
         sim = miSim;
         sim = sim.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
 
     % On the init call return current positions unchanged
@@ -176,7 +188,9 @@ else
     sim.timestepIndex = sim.timestepIndex + 1;
 
     % 3. Update communications topology (Lesser Neighbour Assignment)
-    sim = sim.lesserNeighbor();
+    if ~sim.useFixedTopology
+        sim = sim.lesserNeighbor();
+    end
 
     % 4. Compute Voronoi partitioning
     sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
@@ -184,7 +198,8 @@ else
     % 5. Unconstrained gradient-ascent step for each agent
     for ii = 1:size(sim.agents, 1)
         sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
-                                             sim.timestepIndex, ii, sim.agents);
+                                             sim.timestepIndex, ii, ...
+                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

aerpaw/impl/controller_impl.cpp

@@ -16,6 +16,44 @@
 
 static int serverSocket = -1;
 static std::vector<int> clientSockets;
+static int guidanceStep = 0;
+static int guidanceTotalSteps = 0;
+static struct timespec lastStepTime = {0, 0};
+
+// During guidance returns "(%d/%d) "; outside guidance returns "HH:MM:SS ".
+static std::string logPrefix() {
+    if (guidanceStep > 0) {
+        char buf[32];
+        snprintf(buf, sizeof(buf), "(%d/%d) ", guidanceStep, guidanceTotalSteps);
+        return std::string(buf);
+    }
+    time_t now = time(nullptr);
+    struct tm* lt = localtime(&now);
+    char ts[16];
+    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
+    return std::string(ts) + " ";
+}
+
+void setGuidanceStep(int step, int totalSteps) {
+    struct timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+
+    // From step 2 onward, elapsed = setGuidanceStep(N-1) → setGuidanceStep(N),
+    // which spans the full prior iteration: guidance computation + target send
+    // + flight + position request/receive.
+    if (step > 1 && lastStepTime.tv_sec != 0) {
+        double elapsed = (now.tv_sec - lastStepTime.tv_sec)
+                       + (now.tv_nsec - lastStepTime.tv_nsec) * 1e-9;
+        guidanceStep = step;
+        guidanceTotalSteps = totalSteps;
+        std::cout << logPrefix() << "Iteration duration: " << elapsed << " s\n";
+    } else {
+        guidanceStep = step;
+        guidanceTotalSteps = totalSteps;
+    }
+
+    lastStepTime = now;
+}
 
 void initSockets() {}
 void cleanupSockets() {}
@@ -184,9 +222,13 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
 //   28-31: betaTilt[1:4]
 //   32-34: domainMin  (east, north, up)
 //   35-37: domainMax  (east, north, up)
-//   38-39: objectivePos (east, north)
+//   38   : numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
+//   39-42: objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   44   : sensorPerformanceMinimum
+//   43-50: objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
+//   51   : sensorPerformanceMinimum  (CSV column 18)
+//   52   : useDoubleIntegrator       (CSV column 23)
+//   53   : dampingCoeff              (CSV column 24)
+//   54   : useFixedTopology          (CSV column 25)
 // Returns 1 on success, 0 on failure.
 int loadScenario(const char* filename, double* params) {
     char line[4096];
@@ -198,8 +240,8 @@ int loadScenario(const char* filename, double* params) {
 
     char* fields[32];
     int nf = splitCSVRow(copy, fields, 32);
-    if (nf < 19) {
+    if (nf < 26) {
-        fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf);
+        fprintf(stderr, "loadScenario: expected >=26 columns, got %d\n", nf);
         return 0;
     }
 
@@ -264,34 +306,78 @@ int loadScenario(const char* filename, double* params) {
         }
     }
 
-    // objectivePos: column 16
+    // objectivePos: column 16 — 2 values per component (up to 2 components).
+    // Infer numObjectiveComponents from the number of values parsed.
     {
         char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
         char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf", &params[38], &params[39]) != 2) {
+        double posVals[4] = {0, 0, 0, 0};
-            fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
+        int posCount = 0;
+        char* tok = strtok(t, ",");
+        while (tok && posCount < 4) {
+            posVals[posCount++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        // Check for a 5th token — would mean > 2 components
+        if (tok) {
+            fprintf(stderr, "loadScenario: at most 2 objective Gaussian components supported (objectivePos has >4 values)\n");
             return 0;
         }
+        if (posCount == 0 || posCount % 2 != 0) {
+            fprintf(stderr, "loadScenario: objectivePos must have 2 or 4 values, got %d\n", posCount);
+            return 0;
+        }
+        int nObj = posCount / 2;
+        params[38] = (double)nObj;
+        for (int k = 0; k < 4; k++) params[39 + k] = posVals[k];  // zero-padded for nObj=1
     }
 
-    // objectiveVar: column 17, format "v11, v12, v21, v22"
+    // objectiveVar: column 17 — 4 values per component (v11,v12,v21,v22).
     {
-        char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char tmp[512]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
         char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf , %lf , %lf", &params[40], &params[41], &params[42], &params[43]) != 4) {
+        int nObj = (int)params[38];
-            fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
+        double varVals[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+        int varCount = 0;
+        char* tok = strtok(t, ",");
+        while (tok && varCount < 8) {
+            varVals[varCount++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (varCount != nObj * 4) {
+            fprintf(stderr, "loadScenario: objectiveVar has %d values but expected %d (4 per component)\n",
+                    varCount, nObj * 4);
             return 0;
         }
+        for (int k = 0; k < 8; k++) params[43 + k] = varVals[k];  // zero-padded for nObj=1
     }
 
     // sensorPerformanceMinimum: column 18
     {
         char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[44] = atof(trimField(tmp));
+        params[51] = atof(trimField(tmp));
     }
 
-    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
+    // useDoubleIntegrator: column 23
-           params[32], params[33], params[34], params[35], params[36], params[37]);
+    {
+        char tmp[64]; strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[52] = atof(trimField(tmp));
+    }
+
+    // dampingCoeff: column 24
+    {
+        char tmp[64]; strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[53] = atof(trimField(tmp));
+    }
+
+    // useFixedTopology: column 25
+    {
+        char tmp[64]; strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[54] = atof(trimField(tmp));
+    }
+
+    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g], %d objective component(s)\n",
+           params[32], params[33], params[34], params[35], params[36], params[37], (int)params[38]);
     return 1;
 }
 
@@ -430,18 +516,22 @@ static const char* messageTypeName(uint8_t msgType) {
     }
 }
 
-// Send a single-byte message type to a client
+// Send a single-byte message type to a client (no logging)
-int sendMessageType(int clientId, int msgType) {
+static int sendMessageTypeRaw(int clientId, int msgType) {
     if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-
     uint8_t msg = (uint8_t)msgType;
     ssize_t sent = send(clientSockets[clientId - 1], &msg, 1, 0);
     if (sent != 1) {
         std::cerr << "Send failed for client " << clientId << "\n";
         return 0;
     }
+    return 1;
+}
 
-    std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
+// Send a single-byte message type to a client
+int sendMessageType(int clientId, int msgType) {
+    if (!sendMessageTypeRaw(clientId, msgType)) return 0;
+    std::cout << logPrefix() << "Sent " << messageTypeName((uint8_t)msgType) << " to client " << clientId << "\n";
     return 1;
 }
 
@@ -460,13 +550,7 @@ int sendTarget(int clientId, const double* coords) {
         return 0;
     }
 
-    // Timestamp
+    std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
-    time_t now = time(nullptr);
-    struct tm* lt = localtime(&now);
-    char ts[16];
-    strftime(ts, sizeof(ts), "%H:%M:%S", lt);
-
-    std::cout << ts << " Sent TARGET to client " << clientId << ": "
               << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     return 1;
 }
@@ -514,31 +598,36 @@ int waitForAllMessageType(int numClients, int expectedType) {
                     return 0;
                 }
 
-                std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
-
                 if (msgType == expected) {
                     completed[i] = true;
                     completedCount++;
+                } else {
+                    std::cerr << logPrefix() << "Unexpected " << messageTypeName(msgType)
+                              << " from client " << (i + 1)
+                              << " (expected " << messageTypeName(expected) << ")\n";
                 }
             }
         }
     }
 
+    std::cout << logPrefix() << "Received " << messageTypeName(expected) << " from all clients\n";
     return 1;
 }
 
 // Broadcast GUIDANCE_TOGGLE to all clients
 void sendGuidanceToggle(int numClients) {
     for (int i = 1; i <= numClients; i++) {
-        sendMessageType(i, 6);  // GUIDANCE_TOGGLE = 6
+        sendMessageTypeRaw(i, 6);  // GUIDANCE_TOGGLE = 6
     }
+    std::cout << logPrefix() << "Sent GUIDANCE_TOGGLE to clients\n";
 }
 
 // Send REQUEST_POSITION to all clients
 int sendRequestPositions(int numClients) {
     for (int i = 1; i <= numClients; i++) {
-        if (!sendMessageType(i, 7)) return 0;  // REQUEST_POSITION = 7
+        if (!sendMessageTypeRaw(i, 7)) return 0;  // REQUEST_POSITION = 7
     }
+    std::cout << logPrefix() << "Sent REQUEST_POSITION to clients\n";
     return 1;
 }
 
@@ -573,7 +662,7 @@ int recvPositions(int numClients, double* positions, int maxClients) {
         positions[i + 1 * maxClients] = coords[1];  // north (y)
         positions[i + 2 * maxClients] = coords[2];  // up    (z)
 
-        std::cout << "Position from client " << (i + 1) << ": "
+        std::cout << logPrefix() << "Position from client " << (i + 1) << ": "
                   << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
     }
     return 1;

aerpaw/impl/controller_impl.h

@@ -27,10 +27,14 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   28-31 betaTilt[1:4]
 //   32-34 domainMin
 //   35-37 domainMax
-//   38-39 objectivePos
+//   38    numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   40-43 objectiveVar (2x2 col-major)
+//   39-42 objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   44    sensorPerformanceMinimum
+//   43-50 objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
-#define NUM_SCENARIO_PARAMS 45
+//   51    sensorPerformanceMinimum
+//   52    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
+//   53    dampingCoeff
+//   54    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
+#define NUM_SCENARIO_PARAMS 55
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8
@@ -59,6 +63,7 @@ int sendTarget(int clientId, const double* coords);
 int waitForAllMessageType(int numClients, int expectedType);
 
 // Guidance loop operations
+void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration
 void sendGuidanceToggle(int numClients);
 int  sendRequestPositions(int numClients);
 int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3

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
+    // Derive numClients from initialPositions in scenario.csv
-    int numClients = 2;  // for now
+    double targets[MAX_CLIENTS_PER_PARAM * 3];
-
+    int numClients = loadInitialPositions("config/scenario.csv",
-    std::cout << "Initializing TCP server...\n";
+                                          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/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,16 @@
 #!/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"
+echo "REMEMBER! Manually copy startVehicle_controller.sh to ~/Profiles/ProfileScripts/Vehicle/startVehicle.sh on the fixed node"

aerpaw/results/controllerAnalysis.m

@@ -0,0 +1,28 @@
+function controller = controllerAnalysis(resultsPath)
+    arguments (Input)
+        resultsPath (1, 1) string;
+    end
+    arguments (Output)
+        controller table;
+    end
+    
+    % Measure intervals between issuing commands from the controller 
+    % (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
+    r = dir(resultsPath);
+    controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
+    controllerPath = dir(controllerPath);
+    controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
+    controller = readControllerLogs(controllerPath);
+    rpIdx = startsWith(controller.message, "Iteration duration: ");
+    s = split(controller.message(rpIdx), "Iteration duration: ");
+    s = split(s(:, 2), ' s');
+    s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
+    s.Format = "mm:ss.SSS";
+    fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
+    fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
+    fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
+    fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
+    if seconds(min(s)) < 4 
+        warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
+    end
+end

aerpaw/results/plotGpsLogs.m

@@ -1,7 +1,8 @@
-function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
+function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
     arguments (Input)
         logDirs (1, 1) string;
         seaToGroundLevel (1, 1) double = 110; % measured approximately from USGS national map viewer for the AERPAW test field
+        plotWholeFlight (1, 1) logical = false;
     end
     arguments (Output)
         f (1, 1) matlab.ui.Figure;
@@ -28,6 +29,7 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
     logDirs = dir(logDirs);
     logDirs = logDirs(3:end);
     logDirs = logDirs([logDirs.isdir] == 1);
+    logDirs = logDirs(~startsWith({logDirs.name}, "controller_"));
 
     G = cell(size(logDirs));
     for ii = 1:size(logDirs, 1)
@@ -48,8 +50,10 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
         stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last");
     
         % % Plot whole flight, including setup/cleanup
-        % startIdx = 1;
+        if plotWholeFlight
-        % stopIdx = length(verticalSpeed);
+            startIdx = 1;
+            stopIdx = length(verticalSpeed);
+        end
 
         % Convert LLA trajectory data to ENU for external analysis
         % NaN out entries outside the algorithm flight range so they don't plot
@@ -57,6 +61,27 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
         enu(startIdx:stopIdx, :) = lla2enu([G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx)], lla0, "flat");
         enu = array2table(enu, 'VariableNames', ["East", "North", "Up"]);
         G{ii} = [G{ii}, enu];
+
+        % Do crude comparison of pairwise distances between this UAV and
+        % all previous UAVs
+        for jj = 1:(ii - 1)
+            Ai = G{ii}(:, [1, end-2:end]);
+            Aj = G{jj}(:, [1, end-2:end]);
+
+            % Trim data to match sizes
+            idx = min([size(Ai, 1), size(Aj, 1)]);
+            Ai = Ai(1:idx, :); Aj = Aj(1:idx, :);
+
+            pos_i = [Ai.East, Ai.North, Ai.Up];
+            pos_j = [Aj.East, Aj.North, Aj.Up];
+            d = vecnorm(pos_i - pos_j, 2, 2);
+            d = d(~isnan(d));
+
+            fprintf("Minimum distance between agents %d and %d is %2.3f\n", ii, jj, min(d));
+            if min(d) < 6
+                warning("Minimum distance between agents %d and %d of %2.3f is questionable for AERPAW", ii, jj, min(d));
+            end
+        end
     
         % Plot recorded trajectory over specified range of indices
         geoplot3(gf, G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx) + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);

aerpaw/results/plotRadioLogs.m

@@ -1,9 +1,12 @@
-function [f, R] = plotRadioLogs(resultsPath)
+function [f, fDist, R] = plotRadioLogs(resultsPath, G, tLim)
     arguments (Input)
         resultsPath (1, 1) string;
+        G cell = {};
+        tLim (1, 2) datetime = [datetime(-Inf, 'ConvertFrom', 'datenum'), datetime(Inf, 'ConvertFrom', 'datenum')];
     end
     arguments (Output)
         f (1, 1) matlab.ui.Figure;
+        fDist (1, 1) matlab.ui.Figure;
         R cell;
     end
 
@@ -40,11 +43,44 @@ function [f, R] = plotRadioLogs(resultsPath)
 
     metricNames = ["SNR", "Power", "Quality", "PathLoss", "NoiseFloor", "FreqOffset"];
     yLabels     = ["SNR (dB)", "Power (dB)", "Quality", "Path Loss (dB)", "Noise Floor (dB)", "Freq Offset (MHz)"];
+    nMetrics    = numel(metricNames);
 
+    % --- Time-based figure ---
     f = figure;
-    tl = tiledlayout(numel(metricNames), 1, 'TileSpacing', 'compact', 'Padding', 'compact');
+    tl = tiledlayout(nMetrics + 1, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
 
-    for mi = 1:numel(metricNames)
+    % Distance vs time tile (first)
+    ax = nexttile(tl);
+    hold(ax, 'on'); grid(ax, 'on');
+    legendEntries = string.empty;
+    ci = 1;
+    if ~isempty(G)
+        for rxIdx = 1:nUAV
+            tbl = R{rxIdx};
+            txIDs = unique(tbl.TxUAVID);
+            for ti = 1:numel(txIDs)
+                txID = txIDs(ti);
+                rows = tbl(tbl.TxUAVID == txID, :);
+                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
+                if isempty(rows), continue; end
+                [~, ia] = unique(rows.Timestamp);
+                [radioPt, dist] = pairDist(rows(ia, :), G);
+                if isempty(dist) || all(isnan(dist)), continue; end
+                valid = ~isnan(dist);
+                si = mod(ci - 1, numel(styles)) + 1;
+                plot(ax, datetime(radioPt(valid), 'ConvertFrom', 'posixtime'), dist(valid), ...
+                    styles(si), 'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
+                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
+                ci = ci + 1;
+            end
+        end
+    end
+    ylabel(ax, 'Distance (m)');
+    xlabel(ax, 'Time');
+    legend(ax, legendEntries, 'Location', 'best');
+    hold(ax, 'off');
+
+    for mi = 1:nMetrics
         ax = nexttile(tl);
         hold(ax, 'on');
         grid(ax, 'on');
@@ -57,23 +93,32 @@ function [f, R] = plotRadioLogs(resultsPath)
             for ti = 1:numel(txIDs)
                 txID = txIDs(ti);
                 rows = tbl(tbl.TxUAVID == txID, :);
+                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
                 vals = rows.(metricNames(mi));
+                valid = ~isnan(vals);
+                rows = rows(valid, :);
+                vals = vals(valid);
 
-                % Skip if all NaN for this metric
+                if isempty(rows)
-                if all(isnan(vals))
                     continue;
                 end
 
                 si = mod(ci - 1, numel(styles)) + 1;
                 plot(ax, rows.Timestamp, vals, styles(si), ...
-                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 1);
+                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
                 legendEntries(end+1) = sprintf("TX %d → RX %d", txID, tbl.RxUAVID(1)); %#ok<AGROW>
+
+                % Median per 1/3-second time bin
+                [t_med, v_med] = timeBinMedian(posixtime(rows.Timestamp), vals, 1/3);
+                plot(ax, datetime(t_med, 'ConvertFrom', 'posixtime'), v_med, '-', ...
+                    'Color', 'r', 'LineWidth', 2);
+                legendEntries(end+1) = sprintf("TX %d → RX %d (median)", txID, tbl.RxUAVID(1)); %#ok<AGROW>
                 ci = ci + 1;
             end
         end
 
         ylabel(ax, yLabels(mi));
-        if mi == numel(metricNames)
+        if mi == nMetrics
             xlabel(ax, 'Time');
         end
         legend(ax, legendEntries, 'Location', 'best');
@@ -81,4 +126,134 @@ function [f, R] = plotRadioLogs(resultsPath)
     end
 
     title(tl, 'Radio Channel Metrics');
-end
+
+    % --- Distance-based figure ---
+    fDist = figure;
+
+    if isempty(G)
+        return;
+    end
+
+    tl2 = tiledlayout(nMetrics + 1, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
+
+    % Distance vs time tile (first)
+    ax = nexttile(tl2);
+    hold(ax, 'on'); grid(ax, 'on');
+    legendEntries = string.empty;
+    ci = 1;
+    for rxIdx = 1:nUAV
+        tbl = R{rxIdx};
+        txIDs = unique(tbl.TxUAVID);
+        for ti = 1:numel(txIDs)
+            txID = txIDs(ti);
+            rows = tbl(tbl.TxUAVID == txID, :);
+            rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
+            if isempty(rows), continue; end
+            [~, ia] = unique(rows.Timestamp);
+            [radioPt, dist] = pairDist(rows(ia, :), G);
+            if isempty(dist) || all(isnan(dist)), continue; end
+            valid = ~isnan(dist);
+            si = mod(ci - 1, numel(styles)) + 1;
+            plot(ax, datetime(radioPt(valid), 'ConvertFrom', 'posixtime'), dist(valid), ...
+                styles(si), 'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 0.5);
+            legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
+            ci = ci + 1;
+        end
+    end
+    ylabel(ax, 'Distance (m)');
+    xlabel(ax, 'Time');
+    legend(ax, legendEntries, 'Location', 'best');
+    hold(ax, 'off');
+
+    for mi = 1:nMetrics
+        ax = nexttile(tl2);
+        hold(ax, 'on');
+        grid(ax, 'on');
+
+        legendEntries = string.empty;
+        ci = 1;
+        for rxIdx = 1:nUAV
+            tbl = R{rxIdx};
+            txIDs = unique(tbl.TxUAVID);
+            for ti = 1:numel(txIDs)
+                txID = txIDs(ti);
+                rows = tbl(tbl.TxUAVID == txID, :);
+
+                if isempty(rows)
+                    continue;
+                end
+
+                rows = rows(rows.Timestamp >= tLim(1) & rows.Timestamp <= tLim(2), :);
+                if isempty(rows)
+                    continue;
+                end
+
+                vals = rows.(metricNames(mi));
+                valid = ~isnan(vals);
+                rows = rows(valid, :);
+                vals = vals(valid);
+
+                if isempty(rows)
+                    continue;
+                end
+
+                [radioPt, dist] = pairDist(rows, G);
+                if isempty(dist) || all(isnan(dist)), continue; end
+
+                % Drop points where GPS interpolation returned NaN
+                validDist = ~isnan(dist);
+                rowTs = radioPt(validDist);
+                dist  = dist(validDist);
+                vals  = vals(validDist);
+
+                si = mod(ci - 1, numel(styles)) + 1;
+                scatter(ax, dist, vals, 9, colors(ci, :), strrep(styles(si), "-", ""), 'filled');
+                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, rows.RxUAVID(1)); %#ok<AGROW>
+
+                % Median per 1/3-second time bin, plotted against median distance
+                [~, dv_med] = timeBinMedian(rowTs, [dist, vals], 1/3);
+                [d_med, si_sort] = sort(dv_med(:, 1));
+                v_med = dv_med(si_sort, 2);
+                plot(ax, d_med, v_med, '-', 'Color', 'r', 'LineWidth', 2);
+                legendEntries(end+1) = sprintf("TX %d → RX %d (median)", txID, rows.RxUAVID(1)); %#ok<AGROW>
+                ci = ci + 1;
+            end
+        end
+
+        ylabel(ax, yLabels(mi));
+        if mi == nMetrics
+            xlabel(ax, 'Distance (m)');
+        end
+        legend(ax, legendEntries, 'Location', 'best');
+        hold(ax, 'off');
+    end
+
+    title(tl2, 'Radio Channel Metrics vs Distance');
+end
+
+
+function [radioPt, dist] = pairDist(rows, G)
+    % Interpolate GPS-based inter-UAV distance at each row's timestamp.
+    radioPt = []; dist = [];
+    txGpsIdx = double(rows.TxUAVID(1)) + 1;
+    rxGpsIdx = double(rows.RxUAVID(1)) + 1;
+    if txGpsIdx > numel(G) || rxGpsIdx > numel(G), return; end
+    Gtx = G{txGpsIdx};
+    Grx = G{rxGpsIdx};
+    if ~ismember('East', Gtx.Properties.VariableNames) || ...
+       ~ismember('East', Grx.Properties.VariableNames), return; end
+    txTs = Gtx.Timestamp; txTs.TimeZone = '';
+    rxTs = Grx.Timestamp; rxTs.TimeZone = '';
+    txPt = posixtime(txTs);
+    rxPt = posixtime(rxTs);
+    radioPt = posixtime(rows.Timestamp);
+    validTx = ~isnan(Gtx.East);
+    validRx = ~isnan(Grx.East);
+    txE = interp1(txPt(validTx), Gtx.East(validTx),  radioPt, 'linear', NaN);
+    txN = interp1(txPt(validTx), Gtx.North(validTx), radioPt, 'linear', NaN);
+    txU = interp1(txPt(validTx), Gtx.Up(validTx),    radioPt, 'linear', NaN);
+    rxE = interp1(rxPt(validRx), Grx.East(validRx),  radioPt, 'linear', NaN);
+    rxN = interp1(rxPt(validRx), Grx.North(validRx), radioPt, 'linear', NaN);
+    rxU = interp1(rxPt(validRx), Grx.Up(validRx),    radioPt, 'linear', NaN);
+    dist = vecnorm([txE - rxE, txN - rxN, txU - rxU], 2, 2);
+end

aerpaw/results/readControllerLogs.m

@@ -0,0 +1,32 @@
+function T2 = readControllerLogs(filepath)
+    arguments (Input)
+        filepath (1, 1) string;
+    end
+    arguments (Output)
+        T2 table;
+    end
+    assert(isfile(filepath), "File not found at %s", filepath);
+
+    T = readtable(filepath, 'VariableNamingRule', 'preserve');
+    s = split(T.(T.Properties.VariableNames{1}), ']');
+    s2 = strip(s(startsWith(s(:, 2), " ("), 1), 'left', '[');
+    d = datetime(s2, "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS")';
+    it = s(startsWith(s(:, 2), " ("), 2);
+    it = str2double(strip(strip(it, 'left'), 'left', '('));
+    T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", T.Var7));
+    T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = [];
+    msg = T.(T.Properties.VariableNames{2});
+    msg = msg(startsWith(s(:, 2), " ("), :);
+    s3 = split(msg, ') ');
+    s3 = s3(:, 2);
+    msg = append(s3, T.Var3(startsWith(s(:, 2), " (")));
+    T2 = table(it, d', msg, 'VariableNames', ["iteration", "timestamp", "message"]);
+    % T.Var1 = datetime(strip(strip(append(T.Var1, " ", T.Var2), 'left', '['), 'right', ']'), "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS");
+    % T.Var2 = [];
+    % T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", string(T.Var7), " ", T.Var8, " ", T.Var9));
+    % T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = []; T.Var8 = []; T.Var9 = [];
+    % T.Properties.VariableNames{1} = 'timestamp';
+    % T.Properties.VariableNames{2} = 'message';
+
+    % T(ismissing(T.message), :) = [];
+end

aerpaw/results/readRadioLogs.m

@@ -2,7 +2,6 @@ function R = readRadioLogs(logPath)
     arguments (Input)
         logPath (1, 1) string {isfolder(logPath)};
     end
-
     arguments (Output)
         R (:, 8) table;
     end
@@ -31,14 +30,26 @@ function R = readRadioLogs(logPath)
         end
 
         fid = fopen(filepath, 'r');
-        % Skip 3 lines: 2 junk (tail errors) + 1 header (tx_uav_id,value)
+        % Skip header lines: some files have 2 tail-error lines + 1 column
-        for k = 1:3
+        % header ("tx_uav_id,value"), others start with data immediately.
-            fgetl(fid);
+        % Read until a line that looks like a data record, then rewind to it.
+        dataPattern = '^\[\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}\.\d+\] [-\d]';
+        linePos = ftell(fid);
+        while true
+            line = fgetl(fid);
+            if ~ischar(line)
+                break;  % EOF
+            end
+            if ~isempty(regexp(line, dataPattern, 'once'))
+                fseek(fid, linePos, 'bof');  % rewind to start of this line
+                break;
+            end
+            linePos = ftell(fid);
         end
         data = textscan(fid, '[%26c] %d,%f');
         fclose(fid);
 
-        ts = datetime(data{1}, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
+        ts = datetime(cellstr(data{1}), 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
         txId = int32(data{2});
         val = data{3};
 
@@ -59,6 +70,40 @@ function R = readRadioLogs(logPath)
 
     R = sortrows(R, "Timestamp");
 
+    % Reconstruct per-measurement timestamps within GNURadio processing batches.
+    % The flowgraph accumulates one full PN sequence (4095 chips at samp_rate/sps)
+    % per measurement, but outputs the whole batch simultaneously with a single
+    % wall-clock timestamp. We reassign timestamps by counting backward from the
+    % batch processing time at the known PN period interval.
+    pn_period = 4095 / (2e6 / 16);  % 32.76 ms per PN correlation period
+
+    for txId = unique(R.TxUAVID)'
+        rows = find(R.TxUAVID == txId);
+        if numel(rows) < 2, continue; end
+
+        dt = seconds(diff(R.Timestamp(rows)));
+        break_pos = [1; find(dt > 0.5) + 1];
+        end_pos   = [break_pos(2:end) - 1; numel(rows)];
+
+        for b = 1:numel(break_pos)
+            idx      = rows(break_pos(b) : end_pos(b));
+            batch_ts = posixtime(R.Timestamp(idx));
+            t_ref    = max(batch_ts);
+
+            % Multiple metric files share the same processing timestamp for
+            % each PN period, so group by unique original timestamp rather
+            % than treating every row as a separate PN period.
+            [~, ~, group_id] = unique(batch_ts);
+            n_groups = max(group_id);
+            new_ts   = t_ref - (n_groups - 1 : -1 : 0)' * pn_period;
+
+            for g = 1:n_groups
+                R.Timestamp(idx(group_id == g)) = ...
+                    datetime(new_ts(g), 'ConvertFrom', 'posixtime');
+            end
+        end
+    end
+
     % Remove rows during defined guard period between TDM shifts
     R(R.TxUAVID == -1, :) = [];
 

aerpaw/results/resultsAnalysis.m

@@ -1,12 +1,16 @@
 %% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "t2"); % 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
+
+% Check timeline in controller logs
+controller = controllerAnalysis(resultsPath);
 
 % Plot GPS logged data and scenario information (domain, objective, obstacles)
 seaToGroundLevel = 110; % measured approximately from USGS national map viewer
-[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel);
+plotWholeFlight = true; % do not attempt to automatically trim initial and final positioning and landing from flight plot (buggy)
+[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel, true);
 
-% Plot radio statistics
+% Plot radio statistics (time-based and distance-based)
-[fRadio, R] = plotRadioLogs(resultsPath);
+[fRadio, fRadioDist, R] = plotRadioLogs(resultsPath, G, controller.timestamp([1, end]));
 
 %% Run simulation
 % Run miSim using same AERPAW scenario definition CSV
@@ -21,7 +25,7 @@ makeVideo = true;
 % Define scenario according to CSV specification
 domain = rectangularPrism;
 domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
-domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
+domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [1, 2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
 
 agents = cell(size(params.initialPositions, 2) / 3, 1);
 for ii = 1:size(agents, 1)
@@ -33,7 +37,7 @@ for ii = 1:size(agents, 1)
     collisionGeometry = spherical;
     collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
 
-    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), plotCommsGeometry);
+    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, 5.0, sprintf("Agent %d", ii), plotCommsGeometry);
 end
 
 % Create obstacles
@@ -60,9 +64,12 @@ copyobj(sim.f.Children, comparison);
 
 % Plot trajectories on top
 for ii = 1:size(G, 1)
+    gpsTimes = G{ii}.Timestamp;
+    gpsTimes.TimeZone = '';
+    inRange = gpsTimes >= controller.timestamp(1) & gpsTimes <= controller.timestamp(end);
     for jj = 1:size(sim.spatialPlotIndices, 2)
         hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "on");
-        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East, G{ii}.North, G{ii}.Up + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
+        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East(inRange), G{ii}.North(inRange), G{ii}.Up(inRange) + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
         hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "off");
     end
 end

aerpaw/results/timeBinMedian.m

@@ -0,0 +1,29 @@
+function [t_med, v_med] = timeBinMedian(t, v, binWidth)
+    % Compute median of each column of v within fixed-width time bins.
+    %
+    %   t        - (N,1) posixtime values
+    %   v        - (N,K) data matrix; one column per quantity
+    %   binWidth - scalar bin width in seconds
+    %
+    %   t_med    - (B,1) median time of each non-empty bin
+    %   v_med    - (B,K) median of each column per non-empty bin
+
+    edges = (floor(min(t) / binWidth) * binWidth) : binWidth : ...
+            (floor(max(t) / binWidth) * binWidth + binWidth);
+    bins  = discretize(t, edges);
+    nBins = numel(edges) - 1;
+    K     = size(v, 2);
+
+    t_all = NaN(nBins, 1);
+    v_all = NaN(nBins, K);
+    for bi = 1:nBins
+        mask = bins == bi;
+        if ~any(mask), continue; end
+        t_all(bi)   = median(t(mask));
+        v_all(bi,:) = median(v(mask,:), 1);
+    end
+
+    ok    = ~isnan(t_all);
+    t_med = t_all(ok);
+    v_med = v_all(ok, :);
+end

aerpaw/run_uav.sh

@@ -32,8 +32,8 @@ else
     exit 1
 fi
 
-# Client config file (optional 2nd argument)
+# Client config file: 2nd argument > AERPAW_CLIENT_CONFIG env var > default
-CONFIG_FILE="${2:-config/client.yaml}"
+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

@@ -1,43 +1,100 @@
 #!/bin/bash
-#RX
+
+# 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"
 
-screen -S rxGRC -dm \
+if [ "$NUM_UAVS" -eq 2 ]; then
-       bash -c "stdbuf -oL -eL ./startchannelsounderRXGRC.sh \
+    # Direct 1-to-1 mode: UAV 0 = TX only, UAV 1 = RX only
-       2>&1 | ts $TS_FORMAT \
+    echo "[Radio] 2-UAV direct mode: UAV_ID=$UAV_ID"
-       | tee $RESULTS_DIR/$LOG_PREFIX\_radio_channelsounderrxgrc_log.txt"
 
-screen -S power -dm \
+    if [ "$UAV_ID" -eq 0 ]; then
-       bash -c "stdbuf -oL -eL tail -F /root/Power\
+        # TX only (--num-uavs 1 disables TDM muting)
-        2>&1 | ts $TS_FORMAT \
+        screen -S txGRC -dm \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
+               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 quality -dm \
+        screen -S power -dm \
-       bash -c "stdbuf -oL -eL tail -F /root/Quality\
+               bash -c "stdbuf -oL -eL tail -F /root/Power\
-        2>&1 | ts $TS_FORMAT \
+                2>&1 | ts $TS_FORMAT \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
+               | tee $RESULTS_DIR/$LOG_PREFIX\_power_log.txt"
 
-screen -S snr -dm \
+        screen -S quality -dm \
-       bash -c "stdbuf -oL -eL tail -F /root/SNR\
+               bash -c "stdbuf -oL -eL tail -F /root/Quality\
-        2>&1 | ts $TS_FORMAT \
+                2>&1 | ts $TS_FORMAT \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
+               | tee $RESULTS_DIR/$LOG_PREFIX\_quality_log.txt"
 
-screen -S noisefloor -dm \
+        screen -S snr -dm \
-      bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
+               bash -c "stdbuf -oL -eL tail -F /root/SNR\
-        2>&1 | ts $TS_FORMAT \
+                2>&1 | ts $TS_FORMAT \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_noisefloor_log.txt"
+               | tee $RESULTS_DIR/$LOG_PREFIX\_snr_log.txt"
 
-screen -S freqoffset -dm \
+        screen -S noisefloor -dm \
-       bash -c "stdbuf -oL -eL tail -F /root/FreqOffset\
+              bash -c "stdbuf -oL -eL tail -F /root/NoiseFloor\
-        2>&1 | ts $TS_FORMAT \
+                2>&1 | ts $TS_FORMAT \
-       | tee $RESULTS_DIR/$LOG_PREFIX\_freqoffset_log.txt"
+               | 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"
 
-#TX
+    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
 
-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"
 cd -

aerpaw/scripts/startVehicle.sh

@@ -23,7 +23,7 @@ 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 /root/miSim/aerpaw/config/client1.yaml \
+       bash -c "stdbuf -oL -eL ./run_uav.sh testbed \
        | ts $TS_FORMAT \
        | tee $RESULTS_DIR/$LOG_PREFIX\_vehicle_log.txt"     
 

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

@@ -40,12 +40,9 @@ 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

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

geometries/@cone/cone.m

@@ -6,9 +6,11 @@ classdef cone
         label = "";
 
         % Spatial
-        center = NaN;
+        center  = NaN;
-        radius = NaN;
+        radius  = NaN;
-        height = NaN;
+        height  = NaN;
+        tilt    = 0;   % degrees, 0=nadir 90=horizon
+        azimuth = 0;   % degrees, 0=+Y 90=+X clockwise
 
         % Plotting
         surface;

geometries/@cone/initialize.m

@@ -1,19 +1,23 @@
-function obj = initialize(obj, center, radius, height, tag, label)
+function obj = initialize(obj, center, radius, height, tag, label, tilt, azimuth)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, "cone")};
+        obj     (1, 1) {mustBeA(obj, "cone")};
-        center (1, 3) double;
+        center  (1, 3) double;
-        radius (1, 1) double;
+        radius  (1, 1) double;
-        height (1, 1) double;
+        height  (1, 1) double;
-        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        tag     (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
-        label (1, 1) string = "";
+        label   (1, 1) string      = "";
+        tilt    (1, 1) double      = 0;
+        azimuth (1, 1) double      = 0;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "cone")};
     end
 
-    obj.center = center;
+    obj.center  = center;
-    obj.radius = radius;
+    obj.radius  = radius;
-    obj.height = height;
+    obj.height  = height;
-    obj.tag = tag;
+    obj.tag     = tag;
-    obj.label = label;
+    obj.label   = label;
+    obj.tilt    = tilt;
+    obj.azimuth = azimuth;
 end

geometries/@cone/plot.m

@@ -20,7 +20,18 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
     
     % Scale to match height
     Z = Z * maxAlt;
-    
+
+    % Rotate mesh around apex to match boresight tilt and azimuth.
+    % Apex sits at [0, 0, maxAlt] before center translation.
+    % Convention: tilt 0=nadir, 90=horizon; azimuth 0=+Y, 90=+X, clockwise.
+    Ry = [cosd(obj.tilt), 0, -sind(obj.tilt); 0, 1, 0; sind(obj.tilt), 0, cosd(obj.tilt)];
+    Rz = [sind(obj.azimuth), -cosd(obj.azimuth), 0; cosd(obj.azimuth), sind(obj.azimuth), 0; 0, 0, 1];
+    R  = Rz * Ry;
+    pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
+    X = reshape(pts(1, :), size(X));
+    Y = reshape(pts(2, :), size(Y));
+    Z = reshape(pts(3, :) + maxAlt, size(Z));
+
     % Move to center location
     X = X + obj.center(1);
     Y = Y + obj.center(2);

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/FI0gxbH-PhwjE_riDQGHPyYMHks/BsVG5tuGBaruV5jvy4e3yG1jm4Qd.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/BsVG5tuGBaruV5jvy4e3yG1jm4Qp.xml

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

resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/p7b0T5lVDU-D8JiROZNSX56qOhcp.xml

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

resources/project/Hp7WImb16Wc_c7gK28oUydAFVts/EhSD3dO3mU2KP9IqbTVz7g60j5Ap.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/3zY8iV4IlWXtpru_5KSo-mgBjoEd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/3zY8iV4IlWXtpru_5KSo-mgBjoEp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/7wWFVcqPEpBA2edOtCoXYRUzhTAd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/7wWFVcqPEpBA2edOtCoXYRUzhTAp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/9BCm86xMEE43ruM6njCMPywT3mgp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/DKU4t7TpyzULRGxIyA4AFql5gKQp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/VtHVtnVxsd3b5Abj2jXLEoe6skMp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/W5VZZvYGRyYmGtZeTv2swuCWOZ8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/WZmh1oISZ4y-rj3ybGXq16hboAYp.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8d.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Waxi82DIfDNaoB9qVMSu-XeU4h8p.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnod.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/Y0nVGErZr2RP2C5DDHiHUMtLRnop.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/bflJ_m_JEPly6uLay0BQMV_1uAUd.xml

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

resources/project/MoO8eFA7MNcwhsnIRzm_IDZfmpg/bflJ_m_JEPly6uLay0BQMV_1uAUp.xml

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