obstacles in but ignored

.gitignore

@@ -45,3 +45,24 @@ sandbox/*
 # Videos
 *.mp4
 *.avi
+
+# Figures
+*.fig
+
+# Python Virtual Environment
+aerpaw/venv/
+aerpaw/venv/*
+
+# Pycache
+__pycache__
+
+# aerpaw stuff
+aerpaw/build/
+aerpaw/build/*
+aerpaw/client/__pycache__/
+aerpaw/client/__pycache__/*
+aerpaw/codegen/
+aerpaw/codegen/*
+
+# results
+*.csv

.gitmodules

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

@agent/agent.m

@@ -1,22 +1,14 @@
 classdef agent
-    properties (SetAccess = private, GetAccess = public)
+    properties (SetAccess = public, GetAccess = public)
         % Identifiers
-        index = NaN;
         label = "";
 
-        % Sensor
-        sensorModel;
-        sensingLength = 0.05; % length parameter used by sensing function
-
-        % Guidance
-        guidanceModel;
-
         % State
         lastPos = NaN(1, 3); % position from previous timestep
         pos = NaN(1, 3); % current position
-        vel = NaN(1, 3); % current velocity
-        pan = NaN; % pan angle
-        tilt = NaN; % tilt angle
+
+        % Sensor
+        sensorModel;
 
         % Collision
         collisionGeometry;
@@ -25,15 +17,37 @@ classdef agent
         fovGeometry;
 
         % Communication
-        comRange = NaN;
+        commsGeometry;
+        lesserNeighbors = zeros(1, 0);
+
+        % Performance
+        performance = 0;
 
         % Plotting
         scatterPoints;
+        plotCommsGeometry = true;
+    end
+
+    properties (SetAccess = private, GetAccess = public)
+        initialStepSize = NaN;
+        stepDecayRate = NaN;
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, sensingObjective, domain, partitioning);
+        function obj = agent()
+            arguments (Input)
+            end
+            arguments (Output)
+                obj (1, 1) agent
+            end
+            obj.collisionGeometry = spherical;
+            obj.sensorModel = sigmoidSensor;
+            obj.fovGeometry = cone;
+            obj.commsGeometry = spherical;
+        end
+        [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
+        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [partitioning] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,33 +1,36 @@
-function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
-        vel (1, 3) double;
-        pan (1, 1) double;
-        tilt (1, 1) double;
         collisionGeometry (1, 1) {mustBeGeometry};
-        sensorModel (1, 1) {mustBeSensor}
-        guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
-        comRange (1, 1) double = NaN;
-        index (1, 1) double = NaN;
+        sensorModel (1, 1) {mustBeSensor};
+        comRange (1, 1) double;
+        maxIter (1, 1) double;
+        initialStepSize (1, 1) double = 0.2;
         label (1, 1) string = "";
+        plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
 
     obj.pos = pos;
-    obj.vel = vel;
-    obj.pan = pan;
-    obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
-    obj.guidanceModel = guidanceModel;
-    obj.comRange = comRange;
-    obj.index = index;
     obj.label = label;
+    obj.plotCommsGeometry = plotCommsGeometry;
+    obj.initialStepSize = initialStepSize;
+    obj.stepDecayRate = obj.initialStepSize / maxIter;
+
+    % Initialize performance vector
+    if coder.target('MATLAB')
+        obj.performance = [0, NaN(1, maxIter), 0];
+    end
+
+    % Add spherical geometry based on com range
+    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tan(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@agent/partition.m

@@ -0,0 +1,39 @@
+function [partitioning] = partition(obj, agents, objective)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "agent")};
+        agents (:, 1) {mustBeA(agents, "cell")};
+        objective (1, 1) {mustBeA(objective, "sensingObjective")};
+    end
+    arguments (Output)
+        partitioning (:, :) double;
+    end
+
+    nAgents = size(agents, 1);
+    gridM   = size(objective.X, 1);
+    gridN   = size(objective.X, 2);
+    nPoints = gridM * gridN;
+
+    % Assess sensing performance of each agent at each sample point.
+    % agentPerf is (nPoints x nAgents+1): the extra column is the
+    % 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, ...
+            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
+        agentPerf(:, aa) = p(:);
+    end
+    agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;
+
+    % Find which agent has highest performance at each point.
+    % If the threshold column wins (idx == nAgents+1) the point is unassigned (0).
+    [~, idx] = max(agentPerf, [], 2);
+
+    assignedAgent = zeros(nPoints, 1);
+    for pp = 1:nPoints
+        if idx(pp) <= nAgents
+            assignedAgent(pp) = idx(pp);
+        end
+    end
+
+    partitioning = reshape(assignedAgent, gridM, gridN);
+end

@agent/plot.m

@@ -1,12 +1,12 @@
 function [obj, f] = plot(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        obj (1, 1) {mustBeA(obj, "agent")};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -14,11 +14,11 @@ function [obj, f] = plot(obj, ind, f)
 
     % Plot points representing the agent position
     hold(f.Children(1).Children(end), "on");
-    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
+    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), "filled", "ko", "SizeData", 25);
     hold(f.Children(1).Children(end), "off");
 
     % Check if this is a tiled layout figure
-    if strcmp(f.Children(1).Type, 'tiledlayout')
+    if strcmp(f.Children(1).Type, "tiledlayout")
         % Add to other perspectives
         o = [o; copyobj(o(1), f.Children(1).Children(2))];
         o = [o; copyobj(o(1), f.Children(1).Children(3))];
@@ -30,6 +30,12 @@ function [obj, f] = plot(obj, ind, f)
     % Plot collision geometry
     [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
 
+    % Plot communications geometry
+    if obj.plotCommsGeometry
+        [obj.commsGeometry, f] = obj.commsGeometry.plotWireframe(ind, f);
+    end
+
     % Plot FOV geometry
-    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
+    maxAlt = f.Children(1).Children(end).ZLim(2); % to avoid scaling the FOV geometry as the sim runs, let's just make it really big and hide the excess under the floor of the domain. Check the domain altitude to figure out how big it needs to be to achieve this deception.
+    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f, maxAlt);
 end

@agent/run.m

@@ -1,28 +1,101 @@
-function obj = run(obj, sensingObjective, domain, partitioning)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
-        sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
+        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")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
 
-    % Do sensing
-    [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
+    % Collect objective function values across partition
+    partitionMask = partitioning == index;
+    if ~any(partitionMask(:))
+        % This agent has no partition, maintain current state
+        return;
+    end
+    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-    % Determine next planned position
-    nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
+    % Compute sensor performance on partition
+    maskedX = domain.objective.X(partitionMask);
+    maskedY = domain.objective.Y(partitionMask);
+
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+    deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
+    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+    for ii = 1:7
+        % Apply delta to position
+        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+        
+        % Compute performance values on partition
+        if ii < 6
+            % Compute sensing performance
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+            % Objective performance does not change for 0, +/- X, +/- Y steps.
+            % Those values are computed once before the loop and are only
+            % recomputed when +/- Z steps are applied
+        else
+            % Redo partitioning for Z stepping only
+            partitioning = obj.partition(agents, domain.objective);
+
+            % Recompute partiton-derived performance values for objective
+            partitionMask = partitioning == index;
+            objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
+
+            % Recompute partiton-derived performance values for sensing
+            maskedX = domain.objective.X(partitionMask);
+            maskedY = domain.objective.Y(partitionMask);
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+        end
+
+        % Rearrange data into image arrays
+        F = NaN(size(partitionMask));
+        F(partitionMask) = objectiveValues;
+        S = NaN(size(partitionMask));
+        S(partitionMask) = sensorValues;
+
+        % Compute agent performance
+        C = S .* F;
+        C_delta(ii) = sum(C(~isnan(C)));
+    end
+
+    % Store agent performance at current time and place
+    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)];
+
+    % Compute scaling factor
+    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+    gradNorm = norm(gradC);
+
+    % Compute unconstrained next position.
+    % Guard against near-zero gradient: when sensor performance is saturated
+    % or near-zero across the whole partition, rateFactor -> Inf and pNext
+    % explodes. Stay put instead.
+    if gradNorm < 1e-100
+        pNext = obj.pos;
+    else
+        pNext = obj.pos + (targetRate / gradNorm) * gradC;
+    end
 
     % Move to next position
-    % (dynamics not modeled at this time)
     obj.lastPos = obj.pos;
-    obj.pos = nextPos;
-
-    % Calculate movement
-    d = obj.pos - obj.collisionGeometry.center;
+    obj.pos = pNext;
 
     % Reinitialize collision geometry in the new position
-    obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
+    d = obj.pos - obj.collisionGeometry.center;
+    if isa(obj.collisionGeometry, "rectangularPrism")
+        obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
+    elseif isa(obj.collisionGeometry, "spherical")
+        obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
+    else
+        error("?");
+    end
 end

@agent/updatePlots.m

@@ -1,10 +1,17 @@
 function updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
     arguments (Output)
     end
 
+    % Find change in agent position since last timestep
+    deltaPos = obj.pos - obj.lastPos;
+    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+        % Agent did not move, so nothing has to move on the plots
+        return;
+    end
+
     % Scatterplot point positions
     for ii = 1:size(obj.scatterPoints, 1)
         obj.scatterPoints(ii).XData = obj.pos(1);
@@ -12,9 +19,6 @@ function updatePlots(obj)
         obj.scatterPoints(ii).ZData = obj.pos(3);
     end
 
-    % Find change in agent position since last timestep
-    deltaPos = obj.pos - obj.lastPos;
-
     % Collision geometry edges
     for jj = 1:size(obj.collisionGeometry.lines, 2)
         % Update plotting
@@ -25,9 +29,21 @@ function updatePlots(obj)
         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
+
     % Update FOV geometry surfaces
     for jj = 1:size(obj.fovGeometry.surface, 2)
         % Update each plot
+        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
         obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
         obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
         obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);

@miSim/constrainMotion.m

@@ -0,0 +1,184 @@
+function [obj] = constrainMotion(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    nAgents = size(obj.agents, 1);
+
+    if nAgents < 2
+        nAAPairs = 0;
+    else
+        nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
+    end
+
+    % Compute velocity matrix from unconstrained gradient-ascent step
+    v = zeros(nAgents, 3);
+    for ii = 1:nAgents
+        v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
+    end
+    if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all")
+        % Agents are not attempting to move, so there is no motion to be
+        % constrained
+        return;
+    end
+
+    % Initialize QP based on number of agents and obstacles
+    nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
+    nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
+    nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
+    total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
+    kk = 1;
+    A = zeros(total, 3 * nAgents);
+    b = zeros(total, 1);
+
+    % 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(jj, ii) = h(ii, jj);
+
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+            A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
+            % Slack derived from existing params: recovery velocity = max gradient approach velocity.
+            % Correction splits between 2 agents, so |A| = 2*r_sum
+            r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius;
+            v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
+            slack = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent);
+            if norm(A(kk, :)) < 1e-9
+                % Agents are coincident: A-row is zero, so b < 0 would make
+                % 0 ≤ b unsatisfiable. Fall back to b = 0 (no correction possible).
+                b(kk) = 0;
+            else
+                b(kk) = obj.barrierGain * max(slack, h(ii, jj))^obj.barrierExponent;
+            end
+            kk = kk + 1;
+        end
+    end
+
+    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);
+
+            hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - 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;
+            h_floor_i = -(2 * r_i * v_max_i / obj.barrierGain)^(1 / obj.barrierExponent);
+            b(kk) = obj.barrierGain * max(h_floor_i, hObs(ii, jj))^obj.barrierExponent;
+
+            kk = kk + 1;
+        end
+    end
+
+    % 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;
+        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;
+        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;
+        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;
+        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;
+        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;
+        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')
+        % Save off h function values (logging only — not needed in compiled mode)
+        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
+
+    % Add communication network constraints
+    hComms = NaN(nAgents, nAgents);
+    hComms(logical(eye(nAgents))) = 0;
+    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;
+
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+                A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
+                b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent;
+
+                kk = kk + 1;
+            end
+        end
+    end
+
+    % Solve QP program generated earlier
+    vhat = reshape(v', 3 * nAgents, 1);
+    H = 2 * eye(3 * nAgents);
+    f = -2 * vhat;
+
+    % Update solution based on constraints
+    if coder.target('MATLAB')
+        assert(size(A,2) == size(H,1))
+        assert(size(A,1) == size(b,1))
+        assert(size(H,1) == length(f))
+    end
+    opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
+    x0 = zeros(size(H, 1), 1);
+    [vNew, ~, exitflag, m] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
+    if coder.target('MATLAB')
+        assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message));
+    end
+    vNew = reshape(vNew, 3, nAgents)';
+
+    if exitflag <= 0
+        if coder.target('MATLAB')
+            warning("QP failed, continuing with unconstrained solution...")
+        end
+        vNew = v;
+    end
+
+    % Update the "next position" that was previously set by unconstrained
+    % GA using the constrained solution produced here
+    for ii = 1:size(vNew, 1)
+        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
+    end
+
+    % Here we run this at the simulation level, but in reality there is no
+    % parent level, so this would be run independently on each agent.
+    % Running at the simulation level is just meant to simplify the
+    % simulation
+
+end

@miSim/initialize.m

@@ -1,49 +1,118 @@
-function obj = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
-        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
         agents (:, 1) cell;
+        barrierGain (1, 1) double = 100;
+        barrierExponent (1, 1) double = 3;
+        minAlt (1, 1) double = 1;
         timestep (:, 1) double = 0.05;
-        partitoningFreq (:, 1) double = 0.25
         maxIter (:, 1) double = 1000;
         obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
+        makePlots(1, 1) logical = true;
+        makeVideo (1, 1) logical = true;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    % enable/disable plotting and video writer
+    obj.makePlots = makePlots;
+    if ~obj.makePlots
+        if makeVideo
+            if coder.target('MATLAB')
+                warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
+            end
+            makeVideo = false;
+        end
+    end
+    obj.makeVideo = makeVideo;
+
+    % Generate artifact(s) name
+    if coder.target('MATLAB')
+        obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
+    else
+        obj.artifactName = ""; % Generate no artifacts from simulation in codegen
     end
 
     % Define simulation time parameters
     obj.timestep = timestep;
+    obj.timestepIndex = 0;
     obj.maxIter = maxIter - 1;
 
     % Define domain
     obj.domain = domain;
-    obj.partitioningFreq = partitoningFreq;
 
-    % Add geometries representing obstacles within the domain
-    obj.obstacles = obstacles;
+    % Add geometries representing obstacles within the domain, pre-allocating
+    % one extra slot for the minimum altitude floor obstacle if needed
+    numInputObs = size(obstacles, 1);
+    if minAlt > 0
+        obj.obstacles = repmat({rectangularPrism}, numInputObs + 1, 1);
+    else
+        obj.obstacles = repmat({rectangularPrism}, numInputObs, 1);
+    end
+    for kk = 1:numInputObs
+        obj.obstacles{kk} = obstacles{kk};
+    end
 
-    % Define objective
-    obj.objective = objective;
+    % Add an additional obstacle spanning the domain's footprint to
+    % represent the minimum allowable altitude
+    if minAlt > 0
+        minAltObstacle = rectangularPrism;
+        minAltObstacle = minAltObstacle.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        obj.obstacles{numInputObs + 1} = minAltObstacle;
+    end
 
     % Define agents
     obj.agents = agents;
+    obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
-    % Compute adjacency matrix
+    % Set labels for agents and collision geometries in cases where they
+    % were not provieded at the time of their initialization
+    for ii = 1:size(obj.agents, 1)
+        % Agent
+        if isempty(char(obj.agents{ii}.label))
+            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
+        end
+
+        % Collision geometry
+        if isempty(char(obj.agents{ii}.collisionGeometry.label))
+            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+        end
+    end
+
+    % Set CBF parameters
+    obj.barrierGain = barrierGain;
+    obj.barrierExponent = barrierExponent;
+    obj.minAlt = minAlt;
+
+    % Compute adjacency matrix and lesser neighbors
     obj = obj.updateAdjacency();
+    obj = obj.lesserNeighbor();
 
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
-    obj.partitioningTimes = obj.times(obj.partitioningFreq:obj.partitioningFreq:size(obj.times, 1));
 
-    % Prepare performance data store (at t = 0, all have 0 performance)
-    obj.fPerf = figure;
-    obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
+    if coder.target('MATLAB')
+        % 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 = obj.partition();
+    obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
-    % Set up plots showing initialized state
-    obj = obj.plot();
+    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);
+
+        % Set up plots showing initialized state
+        obj = obj.plot();
+
+        % Run validations
+        obj.validate();
+    end
 end

@miSim/initializeFromCsv.m

@@ -0,0 +1,129 @@
+function obj = initializeFromCsv(obj, csvPath)
+% INITIALIZEFROMCSV  Initialize miSim from an AERPAW scenario CSV file.
+%
+% Reads all guidance parameters, domain geometry, initial UAV positions,
+% and obstacle definitions from the CSV, then builds and initialises the
+% simulation. Ends by calling the standard obj.initialize(...) method.
+%
+% This is the MATLAB-path counterpart to the compiled path that unpacks a
+% flat scenarioParams array in guidance_step.m.  It is only ever called
+% from within a coder.target('MATLAB') guard and is never compiled.
+%
+% Usage (inside guidance_step.m on MATLAB path):
+%   sim = sim.initializeFromCsv('aerpaw/config/scenario.csv');
+%
+% Expected CSV columns (see scenario.csv):
+%   timestep, maxIter, minAlt, discretizationStep, protectedRange,
+%   initialStepSize, barrierGain, barrierExponent,
+%   collisionRadius (per-UAV), comRange (per-UAV),
+%   alphaDist (per-UAV), betaDist (per-UAV),
+%   alphaTilt (per-UAV), betaTilt (per-UAV),
+%   domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"),
+%   objectiveVar ("v11,v12,v21,v22"), sensorPerformanceMinimum,
+%   initialPositions (flat "x1,y1,z1, x2,y2,z2,..."),
+%   numObstacles, obstacleMin (flat), obstacleMax (flat)
+
+arguments (Input)
+    obj     (1, 1) {mustBeA(obj, 'miSim')};
+    csvPath (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+% ---- Parse CSV via readScenarioCsv ---------------------------------------
+scenario = obj.readScenarioCsv(csvPath);
+
+TIMESTEP            = scenario.timestep;
+MAX_ITER            = scenario.maxIter;
+MIN_ALT             = scenario.minAlt;
+DISCRETIZATION_STEP = scenario.discretizationStep;
+PROTECTED_RANGE     = scenario.protectedRange;
+INITIAL_STEP_SIZE   = scenario.initialStepSize;
+BARRIER_GAIN        = scenario.barrierGain;
+BARRIER_EXPONENT    = scenario.barrierExponent;
+% Per-UAV parameters (vectors with one element per UAV)
+COLLISION_RADIUS_VEC = scenario.collisionRadius;   % 1×N
+COMMS_RANGE_VEC      = scenario.comRange;           % 1×N
+ALPHA_DIST_VEC       = scenario.alphaDist;          % 1×N
+BETA_DIST_VEC        = scenario.betaDist;           % 1×N
+ALPHA_TILT_VEC       = scenario.alphaTilt;          % 1×N
+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
+SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
+
+% Initial UAV positions: flat vector reshaped to N×3
+flatPos = scenario.initialPositions;          % 1×(3*N)
+assert(mod(numel(flatPos), 3) == 0, ...
+    "initialPositions must have a multiple of 3 values; got %d", numel(flatPos));
+positions  = reshape(flatPos, 3, [])';        % N×3
+numAgents  = size(positions, 1);
+
+% Validate per-UAV parameter lengths match numAgents
+assert(numel(COLLISION_RADIUS_VEC) == numAgents, ...
+    "collisionRadius has %d values but expected %d (one per UAV)", numel(COLLISION_RADIUS_VEC), numAgents);
+assert(numel(COMMS_RANGE_VEC) == numAgents, ...
+    "comRange has %d values but expected %d (one per UAV)", numel(COMMS_RANGE_VEC), numAgents);
+assert(numel(ALPHA_DIST_VEC) == numAgents, ...
+    "alphaDist has %d values but expected %d (one per UAV)", numel(ALPHA_DIST_VEC), numAgents);
+assert(numel(BETA_DIST_VEC) == numAgents, ...
+    "betaDist has %d values but expected %d (one per UAV)", numel(BETA_DIST_VEC), numAgents);
+assert(numel(ALPHA_TILT_VEC) == numAgents, ...
+    "alphaTilt has %d values but expected %d (one per UAV)", numel(ALPHA_TILT_VEC), numAgents);
+assert(numel(BETA_TILT_VEC) == numAgents, ...
+    "betaTilt has %d values but expected %d (one per UAV)", numel(BETA_TILT_VEC), numAgents);
+
+numObstacles = scenario.numObstacles;
+
+% ---- Build domain --------------------------------------------------------
+dom = rectangularPrism;
+dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
+
+% ---- Build sensing objective (MATLAB path: objectiveFunctionWrapper) -----
+dom.objective = sensingObjective;
+objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR);
+dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
+
+% ---- Initialise agents from scenario positions ---------------------------
+% Each agent gets its own sensor model and collision/comms radii from the
+% per-UAV parameter vectors.
+agentList = cell(numAgents, 1);
+for ii = 1:numAgents
+    pos  = positions(ii, :);
+
+    % Per-UAV sensor model
+    sensor = sigmoidSensor;
+    sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
+                               ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
+
+    geom = spherical;
+    geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
+                           sprintf("UAV %d Collision", ii));
+    ag = agent;
+    ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
+                       INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+    agentList{ii} = ag;
+end
+
+% ---- Build obstacles from CSV --------------------------------------------
+obstacleList = cell(numObstacles, 1);
+if numObstacles > 0
+    obsMin = reshape(scenario.obstacleMin, 3, numObstacles)';  % N×3
+    obsMax = reshape(scenario.obstacleMax, 3, numObstacles)';
+    for ii = 1:numObstacles
+        obs = rectangularPrism;
+        obs = obs.initialize([obsMin(ii, :); obsMax(ii, :)], ...
+                             REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+        obstacleList{ii} = obs;
+    end
+end
+
+% ---- Initialise simulation (plots and video disabled) --------------------
+obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
+                     MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
+
+end

@miSim/lesserNeighbor.m

@@ -0,0 +1,92 @@
+function obj = lesserNeighbor(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    % initialize solution with self-connections only
+    constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
+
+    nAgents = size(obj.agents, 1);
+    for ii = 1:nAgents
+        % Find lesser neighbors of each agent
+        % Lesser neighbors of ii are jj < ii in range of ii
+        lnBuf = zeros(1, nAgents);
+        lnCount = 0;
+        for jj = 1:(ii - 1)
+            if obj.adjacency(ii, jj)
+                lnCount = lnCount + 1;
+                lnBuf(lnCount) = jj;
+            end
+        end
+        obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
+
+        % Early exit for isolated agents
+        if lnCount == 0
+            continue
+        end
+
+        % Focus on subgraph defined by lesser neighbors
+        subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
+
+        % Find connected components; store only the max global index per
+        % component (the only value needed below) to avoid a cell array
+        visited = false(1, lnCount);
+        maxInComponent = zeros(1, lnCount);
+        numComponents = 0;
+        for jj = 1:lnCount
+            if ~visited(jj)
+                reachable = bfs(subgraphAdjacency, jj);
+                visited(reachable) = true;
+                numComponents = numComponents + 1;
+                maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable));
+            end
+        end
+
+        % Connect to the greatest index in each connected component in the
+        % lesser neighborhood of this agent
+        for jj = 1:numComponents
+            maxIdx = maxInComponent(jj);
+            constraintAdjacencyMatrix(ii, maxIdx) = true;
+            constraintAdjacencyMatrix(maxIdx, ii) = true;
+        end
+    end
+    obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
+end
+
+function cComp = bfs(subgraphAdjacency, startIdx)
+    n = size(subgraphAdjacency, 1);
+    visited = false(1, n);
+
+    % Pre-allocated queue and component buffer with head/tail pointers
+    % to avoid element deletion and dynamic array growth
+    queue    = zeros(1, n);
+    cCompBuf = zeros(1, n);
+    qHead = 1;
+    qTail = 2;
+    queue(1)    = startIdx;
+    cCompBuf(1) = startIdx;
+    cSize = 1;
+    visited(startIdx) = true;
+
+    while qHead < qTail
+        current = queue(qHead);
+        qHead = qHead + 1;
+
+        % Find all neighbors of current node in the subgraph
+        neighbors = find(subgraphAdjacency(current, :));
+        for kk = 1:numel(neighbors)
+            neighbor = neighbors(kk);
+            if ~visited(neighbor)
+                visited(neighbor) = true;
+                cCompBuf(cSize + 1) = neighbor;
+                cSize = cSize + 1;
+                queue(qTail) = neighbor;
+                qTail = qTail + 1;
+            end
+        end
+    end
+    cComp = sort(cCompBuf(1:cSize));
+end

@miSim/miSim.m

@@ -2,53 +2,85 @@ classdef miSim
     % multiagent interconnection simulation
 
     % Simulation parameters
-    properties (SetAccess = private, GetAccess = public)
+    properties (SetAccess = public, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
-        partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
+        timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
-        domain = rectangularPrism;
-        objective = sensingObjective;
-        obstacles = cell(0, 1); % geometries that define obstacles within the domain
-        agents = cell(0, 1); % agents that move within the domain
-        adjacency = NaN; % Adjacency matrix representing communications network graph
-        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
-        partitioning = NaN;
-        performance = NaN; % current cumulative sensor performance
+        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
+        constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections
+        partitioning = zeros(0, 0);
+        perf; % sensor performance timeseries array
+        performance = 0; % simulation performance timeseries vector
+        barrierGain = NaN; % CBF gain parameter
+        barrierExponent = NaN; % CBF exponent parameter
+        minAlt = 0; % minimum allowable altitude (m)
+        artifactName = "";
+        fPerf; % performance plot figure
     end
 
     properties (Access = private)
         % Sim
         t = NaN; % current sim time
-        perf; % sensor performance timeseries array
         times;
         partitioningTimes;
 
         % Plot objects
-        f = firstPlotSetup(); % main plotting tiled layout figure
+        makePlots = true; % enable/disable simulation plotting (performance implications)
+        makeVideo = true; % enable/disable VideoWriter (performance implications)
+        f; % main plotting tiled layout figure
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
         partitionPlot; % objects for partition plot
-
-        fPerf; % performance plot figure
         performancePlot; % objects for sensor performance plot
 
+        posHist; % data for trail plot
+        trailPlot; % objects for agent trail plot
+
         % Indicies for various plot types in the main tiled layout figure
         spatialPlotIndices = [6, 4, 3, 2];
         objectivePlotIndices = [6, 4];
         networkGraphIndex = 5;
         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
+        domPlot; % objects for domain h function plot
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
+        function obj = miSim()
+            arguments (Output)
+                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] = initializeFromCsv(obj, csvPath);
         [obj] = run(obj);
+        [obj] = lesserNeighbor(obj);
+        [obj] = constrainMotion(obj);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
         [obj] = plot(obj);
         [obj] = plotConnections(obj);
         [obj] = plotPartitions(obj);
         [obj] = plotGraph(obj);
-        [obj] = updatePlots(obj, updatePartitions);
+        [obj] = plotTrails(obj);
+        [obj] = plotH(obj);
+        [obj] = updatePlots(obj);
+        [obj] = teardown(obj);
+        writeInits(obj);
+        validate(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/partition.m

@@ -1,36 +0,0 @@
-function obj = partition(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    % Assess sensing performance of each agent at each sample point
-    % in the domain
-    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
-    agentPerformances = cat(3, agentPerformances{:});
-    
-    % Get highest performance value at each point
-    [~, idx] = max(agentPerformances, [], 3);
-
-    % Collect agent indices in the same way as performance
-    agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
-    agentInds = cat(3, agentInds{:});
-
-    % Get highest performing agent's index
-    [m,n,~] = size(agentInds);
-    [jj,kk] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-
-    % Get individual agent sensor performance
-    nowIdx = [0; obj.partitioningTimes] == obj.t;
-    for ii = 1:size(obj.agents, 1)
-        obj.perf(ii, nowIdx) = sum(agentPerformances(sub2ind(size(agentInds), jj, kk, ii)), 'all');
-    end
-
-    % Current total performance
-    obj.perf(end, nowIdx) = sum(obj.perf(1:(end - 1), nowIdx));
-end

@miSim/plot.m

@@ -1,9 +1,14 @@
 function obj = plot(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    
+    % fast exit when plotting is disabled
+    if ~obj.makePlots
+        return;
     end
 
     % Plot domain
@@ -17,7 +22,7 @@ function obj = plot(obj)
     % Plot objective gradient
     obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
 
-    % Plot agents and their collision geometries
+    % Plot agents and their collision/communications geometries
     for ii = 1:size(obj.agents, 1)
         [obj.agents{ii}, obj.f] = obj.agents{ii}.plot(obj.spatialPlotIndices, obj.f);
     end
@@ -31,6 +36,9 @@ function obj = plot(obj)
     % Plot domain partitioning
     obj = obj.plotPartitions();
 
+    % Plot agent trails
+    obj = obj.plotTrails();
+
     % Enforce plot limits
     for ii = 1:size(obj.spatialPlotIndices, 2)
         xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
@@ -40,4 +48,10 @@ function obj = plot(obj)
 
     % Plot performance
     obj = obj.plotPerformance();
+
+    % Plot h functions
+    obj = obj.plotH();
+
+    % Switch back to primary figure
+    figure(obj.f);
 end

@miSim/plotConnections.m

@@ -1,17 +1,17 @@
 function obj = plotConnections(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Iterate over lower triangle off-diagonal region of the 
     % adjacency matrix to plot communications links between agents
     X = []; Y = []; Z = [];
-    for ii = 2:size(obj.adjacency, 1)
+    for ii = 2:size(obj.constraintAdjacencyMatrix, 1)
         for jj = 1:(ii - 1)
-            if obj.adjacency(ii, jj)
+            if obj.constraintAdjacencyMatrix(ii, jj)
                 X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
                 Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
                 Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];
@@ -23,19 +23,17 @@ function obj = plotConnections(obj)
     % Plot the connections
     if isnan(obj.spatialPlotIndices)
         hold(obj.f.CurrentAxes, "on");
-        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        o = plot3(obj.f.CurrentAxes, X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
         hold(obj.f.CurrentAxes, "off");
     else
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
     end
 
     % Copy to other plots
-    if size(obj.spatialPlotIndices, 2) > 1
-        for ii = 2:size(obj.spatialPlotIndices, 2)
-            o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
-        end 
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
     end
 
     obj.connectionsPlot = o;

@miSim/plotGraph.m

@@ -1,23 +1,23 @@
 function obj = plotGraph(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Form graph from adjacency matrix
-    G = graph(obj.adjacency, 'omitselfloops');
+    G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
 
     % Plot graph object
     if isnan(obj.networkGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
-        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(obj.f.CurrentAxes, 'off');
+        hold(obj.f.CurrentAxes, "on");
+        o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
+        hold(obj.f.CurrentAxes, "off");
     else
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
-        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "on");
+        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
         if size(obj.networkGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];

@miSim/plotH.m

@@ -0,0 +1,61 @@
+function obj = plotH(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    obj.hf = figure;
+    tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
+
+    nexttile(obj.hf.Children(1));
+    axes(obj.hf.Children(1).Children(1));
+    grid(obj.hf.Children(1).Children(1), "on");
+    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), :)');
+    legendStrings = [];
+    for ii = 2:size(obj.agents, 1)
+        for jj = 1:(ii - 1)
+            legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
+        end
+    end
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
+    hold(obj.hf.Children(1).Children(2), "off");
+
+    nexttile(obj.hf.Children(1));
+    axes(obj.hf.Children(1).Children(1));
+    grid(obj.hf.Children(1).Children(1), "on");
+    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)), :)');
+    legendStrings = [];
+    for ii = 1:size(obj.obstacles, 1)
+        for jj = 1:size(obj.agents, 1)
+            legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
+        end
+    end
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
+    hold(obj.hf.Children(1).Children(2), "off");
+
+    nexttile(obj.hf.Children(1));
+    axes(obj.hf.Children(1).Children(1));
+    grid(obj.hf.Children(1).Children(1), "on");
+    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)');
+    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
+    hold(obj.hf.Children(1).Children(2), "off");
+
+    nexttile(obj.hf.Children(1));
+    axes(obj.hf.Children(1).Children(1));
+    grid(obj.hf.Children(1).Children(1), "on");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
+    title(obj.hf.Children(1).Children(1), "Communications");
+    % skipped this for now because it is very complicated
+
+end

@miSim/plotPartitions.m

@@ -1,19 +1,19 @@
 function obj = plotPartitions(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     if isnan(obj.partitionGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
+        hold(obj.f.CurrentAxes, "on");
         o = imagesc(obj.f.CurrentAxes, obj.partitioning);
-        hold(obj.f.CurrentAxes, 'off');
+        hold(obj.f.CurrentAxes, "off");
     else
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "on");
         o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "off");
         if size(obj.partitionGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];

@miSim/plotPerformance.m

@@ -1,28 +1,45 @@
 function obj = plotPerformance(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
+    % fast exit when plotting is disabled
+    if ~obj.makePlots
+        return;
+    end
+    
+    obj.fPerf = figure;
+
     axes(obj.fPerf);
     title(obj.fPerf.Children(1), "Sensor Performance");
-    xlabel(obj.fPerf.Children(1), 'Time (s)');
-    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
-    grid(obj.fPerf.Children(1), 'on');
+    xlabel(obj.fPerf.Children(1), "Time (s)");
+    ylabel(obj.fPerf.Children(1), "Sensor Performance");
+    grid(obj.fPerf.Children(1), "on");
 
     % Plot current cumulative performance
-    hold(obj.fPerf.Children(1), 'on');
+    hold(obj.fPerf.Children(1), "on");
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    hold(obj.fPerf.Children(1), 'off');
+    warning("off", "MATLAB:gui:array:InvalidArrayShape"); % suppress this warning to avoid polluting output
+    o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
+    o.YData = [0, NaN(1, obj.maxIter - 1)];
+    hold(obj.fPerf.Children(1), "off");
 
     % Plot current agent performance
     for ii = 1:(size(obj.perf, 1) - 1)
-        hold(obj.fPerf.Children(1), 'on');
+        hold(obj.fPerf.Children(1), "on");
         o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
-        hold(obj.fPerf.Children(1), 'off');
+        o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
+        o(end).YData = [0, NaN(1, obj.maxIter - 1)];
+        hold(obj.fPerf.Children(1), "off");
     end
 
+    % Add legend
+    agentStrings = string(cellfun(@(x) x.label, obj.agents, "UniformOutput", false));
+    agentStrings = ["Total"; agentStrings];
+    legend(obj.fPerf.Children(1), agentStrings, "Location", "northwest");
+
     obj.performancePlot = o;
 end

@miSim/plotTrails.m

@@ -0,0 +1,30 @@
+function obj = plotTrails(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")}
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")}
+    end
+    
+    % fast exit when plotting is disabled
+    if ~obj.makePlots
+        return;
+    end
+
+    % Plot full range of position history on each spatial plot axes
+    o = [];
+    for ii = 1:(size(obj.posHist, 1))
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
+        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), "Color", "k", "LineWidth", 1)];
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
+    end
+
+    % Copy to other plots
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    end 
+
+    % Add legend?
+
+    obj.trailPlot = o;
+end

@miSim/run.m

@@ -1,43 +1,72 @@
 function [obj] = run(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Start video writer
-    v = obj.setupVideoWriter();
-    v.open();
+    if coder.target('MATLAB')
+        % Start video writer
+        if obj.makeVideo
+            v = obj.setupVideoWriter();
+            v.open();
+        end
+    end
 
     for ii = 1:size(obj.times, 1)
         % Display current sim time
         obj.t = obj.times(ii);
-        fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
+        obj.timestepIndex = ii;
+        if coder.target('MATLAB')
+            fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
-        % Check if it's time for new partitions
-        updatePartitions = false;
-        if ismember(obj.t, obj.partitioningTimes)
-            updatePartitions = true;
-            obj = obj.partition();
+            % Validate current simulation configuration
+            obj.validate();
         end
 
-        % Iterate over agents to simulate their motion
+        % Update partitioning before moving (this one is strictly for
+        % plotting purposes, the real partitioning is done by the agents)
+        obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+
+        % Determine desired communications links
+        obj = obj.lesserNeighbor();
+
+        % Moving
+        % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
         end
 
-        % Update adjacency matrix
-        obj = obj.updateAdjacency();
+        % Adjust motion determined by unconstrained gradient ascent using
+        % CBF constraints solved by QP
+        obj = constrainMotion(obj);
 
-        % Update plots
-        obj = obj.updatePlots(updatePartitions);
+        if coder.target('MATLAB')
+            % Update agent position history array
+            obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
 
-        % Write frame in to video
-        I = getframe(obj.f);
-        v.writeVideo(I);
+            % Update total performance
+            obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
+
+            % Update adjacency matrix
+            obj = obj.updateAdjacency();
+
+            % Update plots
+            obj = obj.updatePlots();
+
+            % Write frame in to video
+            if obj.makeVideo
+                I = getframe(obj.f);
+                v.writeVideo(I);
+            end
+        end
     end
 
-    % Close video file
-    v.close();
+    if coder.target('MATLAB')
+        if obj.makeVideo
+            % Close video file
+            v.close();
+        end
+    end
 end

@miSim/setupVideoWriter.m

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

@miSim/teardown.m

@@ -0,0 +1,30 @@
+function obj = teardown(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    % Close plots
+    close(obj.hf);
+    close(obj.fPerf);
+    close(obj.f);
+
+    % 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.partitioning = NaN;
+    obj.performance = 0;
+    obj.barrierGain = NaN;
+    obj.barrierExponent = NaN;
+    obj.artifactName = "";
+
+end

@miSim/updateAdjacency.m

@@ -1,32 +1,24 @@
 function obj = updateAdjacency(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Initialize assuming only self-connections
-    A = logical(eye(size(obj.agents, 1)));
+    A = true(size(obj.agents, 1));
 
     % Check lower triangle off-diagonal connections
     for ii = 2:size(A, 1)
         for jj = 1:(ii - 1)
-            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange])
-                % Make sure that obstacles don't obstruct the line
-                % of sight, breaking the connection
-                for kk = 1:size(obj.obstacles, 1)
-                    if ~obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
-                        A(ii, jj) = true;
-                    end
-                end
-                % need extra handling for cases with no obstacles
-                if isempty(obj.obstacles)
-                    A(ii, jj) = true;
-                end
+            % Check that agents are not out of range
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
+                A(ii, jj) = false; % comm range violation
+                continue;
             end
         end
     end
 
-    obj.adjacency = A | A';
+    obj.adjacency = A & A';
 end

@miSim/updatePlots.m

@@ -1,19 +1,23 @@
-function [obj] = updatePlots(obj, updatePartitions)
+function [obj] = updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-        updatePartitions (1, 1) logical = false;
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Update agent positions, collision geometries
+    % Fast exit when plotting is disabled
+    if ~obj.makePlots
+        return;
+    end
+
+    % Update agent positions, collision/communication geometries
     for ii = 1:size(obj.agents, 1)
         obj.agents{ii}.updatePlots();
     end
 
-    % The remaining updates might be possible to do in a clever way
-    % that moves existing lines instead of clearing and 
+    % The remaining updates might should all be possible to do in a clever 
+    % way that moves existing lines instead of clearing and 
     % re-plotting, which is much better for performance boost
 
     % Update agent connections plot
@@ -25,10 +29,8 @@ function [obj] = updatePlots(obj, updatePartitions)
     obj = obj.plotGraph();
 
     % Update partitioning plot
-    if updatePartitions
-        delete(obj.partitionPlot);
-        obj = obj.plotPartitions();
-    end
+    delete(obj.partitionPlot);
+    obj = obj.plotPartitions();
 
     % reset plot limits to fit domain
     for ii = 1:size(obj.spatialPlotIndices, 2)
@@ -36,17 +38,36 @@ function [obj] = updatePlots(obj, updatePartitions)
         ylim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
         zlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
     end
+
+    % Update agent trails
+    for jj = 1:size(obj.spatialPlotIndices, 2)
+        for ii = 1:size(obj.agents, 1)
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+        end
+    end
+
     drawnow;
     
     % Update performance plot
-    if updatePartitions
-        nowIdx = [0; obj.partitioningTimes] == obj.t;
-        % set(obj.performancePlot(1), 'YData', obj.perf(end, 1:find(nowIdx)));
-        obj.performancePlot(1).YData(nowIdx) = obj.perf(end, nowIdx);
-        for ii = 2:size(obj.performancePlot, 1)
-            obj.performancePlot(ii).YData(nowIdx) = obj.perf(ii, nowIdx);
-        end
-        drawnow;
+    % Re-normalize performance plot
+    normalizingFactor = 1/max(obj.performance);
+    obj.performancePlot(1).YData(1:(length(obj.performance) + 1)) = [obj.performance, 0] * normalizingFactor;
+    obj.performancePlot(1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
+    for ii = 1:(size(obj.agents, 1))
+        obj.performancePlot(ii + 1).YData(1:(length(obj.performance) + 1)) = [obj.agents{ii}.performance(1:length(obj.performance)), 0] * normalizingFactor;
+        obj.performancePlot(ii + 1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
     end
 
+    % Update h function plots
+    for ii = 1:size(obj.caPlot, 1)
+        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
+    end
+    for ii = 1:size(obj.obsPlot, 1)
+        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), 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);
+    end
 end

@miSim/validate.m

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

@miSim/writeInits.m

@@ -0,0 +1,33 @@
+function writeInits(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+    end
+
+    % Collect agent parameters
+    collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
+    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    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));
+
+
+    % Combine with simulation parameters
+    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
+                    "discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ...
+                    "sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ...
+                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
+                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, "alphaDist", alphaDist, ...
+                    "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
+                    "pos", pos);
+
+    % Save all parameters to output file
+    initsFile = strcat(obj.artifactName, "_miSimInits");
+    initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile);
+    save(initsFile, "-struct", "inits");
+end

@sensingObjective/initialize.m

@@ -1,16 +1,20 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
-        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
+        objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
     end
     arguments (Output)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
     end
 
-    obj.groundAlt = domain.minCorner(3);
+    obj.discretizationStep = discretizationStep;
+
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+    
     obj.protectedRange = protectedRange;
 
     % Extract footprint limits
@@ -19,19 +23,22 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     yMin = min(domain.footprint(:, 2));
     yMax = max(domain.footprint(:, 2));
 
-    xGrid = unique([xMin:discretizationStep:xMax, xMax]);
-    yGrid = unique([yMin:discretizationStep:yMax, yMax]);
+    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
     
     % Store grid points for plotting later
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
 
     % Evaluate function over grid points
-    obj.objectiveFunction = objectiveFunction;
-    obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
+    obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X));
+    
+    % Normalize
+    obj.values = obj.values ./ max(obj.values, [], "all");
 
     % store ground position
-    idx = obj.values == max(obj.values, [], "all");
+    idx = obj.values == 1;
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
 
     assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -1,12 +1,12 @@
 function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+        obj (1, 1) {mustBeA(obj, "sensingObjective")};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+        obj (1, 1) {mustBeA(obj, "sensingObjective")};
     end
 
     % Set random objective position
@@ -14,13 +14,12 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     while domain.distance(mu) < protectedRange
         mu = domain.random();
     end
-    mu = mu(1:2);
 
     % Set random distribution parameters
     sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
 
     % Set up random bivariate normal distribution function
-    objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig);
+    objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
     obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);

@sensingObjective/initializeWithValues.m

@@ -0,0 +1,42 @@
+function obj = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+    arguments (Input)
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
+        values (:,:) double;
+        domain (1, 1) {mustBeGeometry};
+        discretizationStep (1, 1) double = 1;
+        protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
+    end
+    arguments (Output)
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
+    end
+
+    obj.discretizationStep = discretizationStep;
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+    obj.protectedRange = protectedRange;
+
+    % Extract footprint limits
+    xMin = min(domain.footprint(:, 1));
+    xMax = max(domain.footprint(:, 1));
+    yMin = min(domain.footprint(:, 2));
+    yMax = max(domain.footprint(:, 2));
+
+    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
+
+    % Store grid points
+    [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
+
+    % Use pre-computed values (caller must evaluate on same grid)
+    obj.values = reshape(values, size(obj.X));
+
+    % Normalize
+    obj.values = obj.values ./ max(obj.values, [], "all");
+
+    % Store ground position (peak of objective)
+    idx = obj.values == 1;
+    obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    obj.groundPos = obj.groundPos(1, 1:2);
+
+    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+end

@sensingObjective/plot.m

@@ -1,11 +1,11 @@
 function f = plot(obj, ind, f)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
         ind (1, :) double = NaN;
-        f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1,1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1,1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -14,15 +14,15 @@ function f = plot(obj, ind, f)
     % Plot gradient on the "floor" of the domain
     if isnan(ind)
         hold(f.CurrentAxes, "on");
-        o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
-        o.HitTest = 'off';
-        o.PickableParts = 'none';
+        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        o.HitTest = "off";
+        o.PickableParts = "none";
         hold(f.CurrentAxes, "off");
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
-        o.HitTest = 'off';
-        o.PickableParts = 'none';
+        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        o.HitTest = "off";
+        o.PickableParts = "none";
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

@sensingObjective/sensingObjective.m

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

@sigmoidSensor/distanceMembership.m

@@ -1,6 +1,6 @@
 function x = distanceMembership(obj, d)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
         d (:, 1) double;
     end
     arguments (Output)

@sigmoidSensor/initialize.m

@@ -1,21 +1,17 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
-        alphaPan (1, 1) double;
-        betaPan (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
     end
     
     obj.alphaDist = alphaDist;
     obj.betaDist = betaDist;
-    obj.alphaPan = alphaPan;
-    obj.betaPan = betaPan;
     obj.alphaTilt = alphaTilt;
     obj.betaTilt = betaTilt;
 end

@sigmoidSensor/plotParameters.m

@@ -1,9 +1,9 @@
 function f = plotParameters(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
     end
     arguments (Output)
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Distance and tilt sample points
@@ -12,7 +12,7 @@ function f = plotParameters(obj)
 
     % Sample membership functions
     d_x = obj.distanceMembership(d);
-    t_x = obj.tiltMembership(deg2rad(t));
+    t_x = obj.tiltMembership(t);
 
     % Plot resultant sigmoid curves
     f = figure;
@@ -24,9 +24,9 @@ function f = plotParameters(obj)
     title("Distance Membership Sigmoid");
     xlabel("Distance (m)");
     ylabel("Membership");
-    hold('on');
-    plot(d, d_x, 'LineWidth', 2);
-    hold('off');
+    hold("on");
+    plot(d, d_x, "LineWidth", 2);
+    hold("off");
     ylim([0, 1]);
 
     % Tilt
@@ -35,8 +35,8 @@ function f = plotParameters(obj)
     title("Tilt Membership Sigmoid");
     xlabel("Tilt (deg)");
     ylabel("Membership");
-    hold('on');
-    plot(t, t_x, 'LineWidth', 2);
-    hold('off');
+    hold("on");
+    plot(t, t_x, "LineWidth", 2);
+    hold("off");
     ylim([0, 1]);
 end

@sigmoidSensor/sensorPerformance.m

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

@sigmoidSensor/sigmoidSensor.m

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

@sigmoidSensor/tiltMembership.m

@@ -1,7 +1,7 @@
 function x = tiltMembership(obj, t)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-        t (:, 1) double;
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        t (:, 1) double; % degrees
     end
     arguments (Output)
         x (:, 1) double;

aerpaw/MESSAGE_TYPE.m

@@ -0,0 +1,12 @@
+classdef MESSAGE_TYPE < uint8
+    enumeration
+        TARGET  (1)  % Server->Client: target coordinates follow (3 doubles)
+        ACK     (2)  % Client->Server: command received
+        READY   (3)  % Both: ready for next command / mission complete
+        RTL              (4)  % Server->Client: return to launch
+        LAND             (5)  % Server->Client: land now
+        GUIDANCE_TOGGLE  (6)  % Server->Client: toggle guidance mode on/off
+        REQUEST_POSITION (7)  % Server->Client: respond with current ENU position
+        POSITION         (8)  % Client->Server: current ENU position (3 doubles)
+    end
+end

aerpaw/client/uav_runner.py

@@ -0,0 +1,298 @@
+#!/usr/bin/env python3
+"""
+UAV Runner for AERPAW - aerpawlib BasicRunner implementation.
+
+Run via:
+    python -m aerpawlib --script client.uav_runner --conn <conn> --vehicle drone
+
+Or use the auto-detecting wrapper:
+    ./run_uav.sh
+
+Binary Protocol:
+    For each waypoint:
+        Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
+        Client sends: ACK (1 byte)
+        Client (after moving): READY (1 byte)
+    After all waypoints:
+        Server sends: RTL (1 byte) → Client: ACK, return home, READY
+        Server sends: LAND (1 byte) → Client: ACK, land, READY
+        Server sends: READY (1 byte) - mission complete, disconnect
+"""
+from enum import IntEnum
+from pathlib import Path
+import asyncio
+import csv
+import datetime
+import os
+import platform
+import struct
+import time
+import yaml
+
+from aerpawlib.runner import BasicRunner, entrypoint
+from aerpawlib.util import Coordinate, VectorNED
+from aerpawlib.vehicle import Drone
+
+
+# Message types - must match MESSAGE_TYPE.m enum
+class MessageType(IntEnum):
+    TARGET           = 1
+    ACK              = 2
+    READY            = 3
+    RTL              = 4
+    LAND             = 5
+    GUIDANCE_TOGGLE  = 6
+    REQUEST_POSITION = 7
+    POSITION         = 8
+
+
+AERPAW_DIR = Path(__file__).parent.parent
+CONFIG_FILE = AERPAW_DIR / "config" / "client.yaml"
+
+
+def load_config():
+    """Load configuration from YAML file."""
+    with open(CONFIG_FILE, 'r') as f:
+        return yaml.safe_load(f)
+
+
+def get_environment():
+    """Get environment from AERPAW_ENV variable. Fails if not set."""
+    env = os.environ.get('AERPAW_ENV')
+    if env is None:
+        raise RuntimeError(
+            "AERPAW_ENV environment variable not set. "
+            "Set to 'local' or 'testbed', or use: ./run_uav.sh [local|testbed]"
+        )
+    if env not in ('local', 'testbed'):
+        raise RuntimeError(
+            f"Invalid AERPAW_ENV '{env}'. Must be 'local' or 'testbed'."
+        )
+    return env
+
+
+async def recv_exactly(reader: asyncio.StreamReader, n: int) -> bytes:
+    """Receive exactly n bytes from async stream."""
+    data = await reader.readexactly(n)
+    return data
+
+
+async def recv_message_type(reader: asyncio.StreamReader) -> MessageType:
+    """Receive a single-byte message type."""
+    data = await recv_exactly(reader, 1)
+    return MessageType(data[0])
+
+
+async def send_message_type(writer: asyncio.StreamWriter, msg_type: MessageType):
+    """Send a single-byte message type."""
+    writer.write(bytes([msg_type]))
+    await writer.drain()
+
+
+def _gps_log_row(vehicle, line_num, writer):
+    """Sample vehicle state and write one CSV row (matches gps_logger.py format)."""
+    pos = vehicle.position
+    lat, lon = pos.lat, pos.lon
+    alt = round(float(str(pos.alt)), 3)
+
+    batt = str(vehicle.battery)
+    volt = float(batt[16:batt.find(",")])
+
+    timestamp = datetime.datetime.now()
+    fix, num_sat = vehicle.gps.fix_type, vehicle.gps.satellites_visible
+
+    if fix < 2:
+        lat, lon, alt = -999, -999, -999
+
+    vel = vehicle.velocity
+    attitude = vehicle.attitude
+    attitude_str = (
+        "(" + ",".join(map(str, [attitude.pitch, attitude.yaw, attitude.roll])) + ")"
+    )
+
+    writer.writerow([line_num, lon, lat, alt, attitude_str, vel, volt, timestamp, fix, num_sat])
+
+
+async def _gps_log_loop(drone):
+    """Background async task that logs GPS data at 1Hz."""
+    host = platform.node()
+    filename = f"/root/Results/GPS_DATA_{host}_{datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S')}.csv"
+    print(f"[UAV] GPS logging to {filename}")
+    line_num = 1
+    try:
+        with open(filename, "w+") as f:
+            writer = csv.writer(f)
+            while True:
+                if drone.connected:
+                    _gps_log_row(drone, line_num, writer)
+                    f.flush()
+                    os.fsync(f)
+                    line_num += 1
+                else:
+                    print("[UAV] [GPS] No vehicle heartbeat")
+                await asyncio.sleep(1.0)
+    except asyncio.CancelledError:
+        print(f"[UAV] GPS logging stopped ({line_num - 1} rows written)")
+    except Exception as e:
+        print(f"[UAV] GPS logging error: {e}")
+
+
+class UAVRunner(BasicRunner):
+    def initialize_args(self, extra_args):
+        """Load configuration from YAML config file."""
+        config = load_config()
+        env = get_environment()
+        print(f"[UAV] Environment: {env}")
+
+        # Load origin
+        origin = config['origin']
+        self.origin = Coordinate(origin['lat'], origin['lon'], origin['alt'])
+        print(f"[UAV] Origin: {origin['lat']}, {origin['lon']}, {origin['alt']}")
+
+        # Load controller address for this environment
+        env_config = config['environments'][env]
+        self.server_ip = env_config['controller']['ip']
+        self.server_port = env_config['controller']['port']
+        print(f"[UAV] Controller: {self.server_ip}:{self.server_port}")
+
+    @entrypoint
+    async def run_mission(self, drone: Drone):
+        """Main mission entry point."""
+        print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
+
+        # Retry connection up to 10 times (~30 seconds total)
+        reader, writer = None, None
+        for attempt in range(10):
+            try:
+                reader, writer = await asyncio.wait_for(
+                    asyncio.open_connection(self.server_ip, self.server_port),
+                    timeout=5,
+                )
+                print(f"[UAV] Connected to controller")
+                break
+            except (ConnectionRefusedError, asyncio.TimeoutError, OSError) as e:
+                print(f"[UAV] Connection attempt {attempt + 1}/10 failed: {e}")
+                if attempt < 9:
+                    await asyncio.sleep(3)
+
+        if reader is None:
+            print("[UAV] Failed to connect to controller after 10 attempts")
+            return
+
+        log_task = None
+        nav_task = None
+        try:
+            # Takeoff to above AERPAW minimum altitude
+            print("[UAV] Taking off...")
+            await drone.takeoff(25)
+            print("[UAV] Takeoff complete, waiting for commands...")
+
+            # Start GPS logging in background
+            log_task = asyncio.create_task(_gps_log_loop(drone))
+
+            # Command loop - handle all messages from controller
+            waypoint_num = 0
+            in_guidance = False
+            while True:
+                msg_type = await recv_message_type(reader)
+                print(f"[UAV] Received: {msg_type.name}")
+
+                if msg_type == MessageType.GUIDANCE_TOGGLE:
+                    in_guidance = not in_guidance
+                    print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}")
+                    if not in_guidance:
+                        # Exiting guidance: wait for current navigation to finish
+                        # before resuming sequential (ACK/READY) mode
+                        if nav_task and not nav_task.done():
+                            print("[UAV] Waiting for current navigation to complete...")
+                            await nav_task
+                            nav_task = None
+                        # Acknowledge that we are ready for sequential commands
+                        await send_message_type(writer, MessageType.ACK)
+                        print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
+
+                elif msg_type == MessageType.REQUEST_POSITION:
+                    # Respond immediately with current ENU position relative to origin
+                    pos = drone.position
+                    enu = pos - self.origin  # VectorNED(north, east, down)
+                    await send_message_type(writer, MessageType.POSITION)
+                    writer.write(struct.pack('<ddd', enu.east, enu.north, -enu.down))
+                    await writer.drain()
+                    print(f"[UAV] Sent POSITION: E={enu.east:.1f} N={enu.north:.1f} U={-enu.down:.1f}")
+
+                elif msg_type == MessageType.TARGET:
+                    # Read 24 bytes of coordinates (3 little-endian doubles)
+                    data = await recv_exactly(reader, 24)
+                    enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
+
+                    # Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
+                    target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
+
+                    if in_guidance:
+                        # Guidance mode (event-triggered): navigate to target,
+                        # then send ACK once arrived so the controller knows
+                        # all UAVs have reached their targets before it
+                        # requests positions and computes the next step.
+                        print(f"[UAV] Guidance TARGET: E={enu_x:.1f} N={enu_y:.1f} U={enu_z:.1f}")
+                        if nav_task and not nav_task.done():
+                            nav_task.cancel()
+                            await asyncio.gather(nav_task, return_exceptions=True)
+                        await drone.goto_coordinates(target)
+                        await send_message_type(writer, MessageType.ACK)
+                        print("[UAV] Sent ACK (arrived at guidance target)")
+                        nav_task = None
+                    else:
+                        # Sequential mode: ACK → navigate → READY
+                        waypoint_num += 1
+                        print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x:.1f}, y={enu_y:.1f}, z={enu_z:.1f}")
+                        print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
+                        await send_message_type(writer, MessageType.ACK)
+                        print("[UAV] Sent ACK")
+                        print(f"[UAV] Moving to waypoint {waypoint_num}...")
+                        await drone.goto_coordinates(target)
+                        print(f"[UAV] Arrived at waypoint {waypoint_num}")
+                        await send_message_type(writer, MessageType.READY)
+                        print("[UAV] Sent READY")
+
+                elif msg_type == MessageType.RTL:
+                    await send_message_type(writer, MessageType.ACK)
+                    print(f"[UAV] Sent ACK")
+                    print("[UAV] Returning to home...")
+                    home = drone.home_coords
+                    safe_alt = 25
+                    rtl_target = Coordinate(home.lat, home.lon, safe_alt)
+                    print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
+                    await drone.goto_coordinates(rtl_target)
+                    print("[UAV] Arrived at home position")
+                    await send_message_type(writer, MessageType.READY)
+                    print(f"[UAV] Sent READY")
+
+                elif msg_type == MessageType.LAND:
+                    await send_message_type(writer, MessageType.ACK)
+                    print(f"[UAV] Sent ACK")
+                    print("[UAV] Landing...")
+                    await drone.land()
+                    print("[UAV] Landed and disarmed")
+                    await send_message_type(writer, MessageType.READY)
+                    print(f"[UAV] Sent READY")
+
+                elif msg_type == MessageType.READY:
+                    print("[UAV] Mission complete")
+                    break
+
+                else:
+                    print(f"[UAV] Unknown command: {msg_type}")
+
+        except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
+            print(f"[UAV] Error: {e}")
+
+        finally:
+            if nav_task is not None and not nav_task.done():
+                nav_task.cancel()
+                await asyncio.gather(nav_task, return_exceptions=True)
+            if log_task is not None:
+                log_task.cancel()
+                await asyncio.gather(log_task, return_exceptions=True)
+            writer.close()
+            await writer.wait_closed()
+            print("[UAV] Connection closed")

aerpaw/compile.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+# Build controller_app from codegen + impl sources
+
+AERPAW_DIR="$(cd "$(dirname "$0")" && pwd)"
+CODEGEN="$AERPAW_DIR/codegen"
+IMPL="$AERPAW_DIR/impl"
+BUILD="$AERPAW_DIR/build"
+
+mkdir -p "$BUILD"
+
+echo "Compiling controller executable..."
+
+# Compile all codegen sources (handles any new generated files)
+g++ -I/home/kdee/matlab/R2025a/extern/include \
+    -I"$CODEGEN" \
+    -I"$IMPL" \
+    "$IMPL/controller_main.cpp" \
+    "$IMPL/controller_impl.cpp" \
+    "$CODEGEN"/*.cpp \
+    -o "$BUILD/controller_app" \
+    -fopenmp \
+    -lpthread
+
+echo "Built: $BUILD/controller_app"

aerpaw/config/client.yaml

@@ -0,0 +1,28 @@
+# AERPAW UAV (Client) Configuration
+
+# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
+origin:
+  lat: 35.72550610629396
+  lon: -78.70019657805574
+  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
+# Environment-specific settings
+environments:
+  local:
+    # MAVLink connection for SITL simulation (UDP)
+    mavlink:
+      ip: "127.0.0.1"
+      port: 14550
+    # Controller server address
+    controller:
+      ip: "127.0.0.1"
+      port: 5000
+
+  testbed:
+    # 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:
+      ip: "192.168.122.1"
+      port: 5000

aerpaw/config/scenario.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
+5, 100, 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"

aerpaw/controller.m

@@ -0,0 +1,236 @@
+function controller(numClients)
+arguments (Input)
+    numClients (1, 1) int32;
+end
+
+coder.extrinsic('disp', 'readScenarioCsv');
+
+% Maximum clients supported (one initial position per UAV)
+MAX_CLIENTS = 4;
+MAX_TARGETS = MAX_CLIENTS;
+
+% 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
+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
+    totalLoaded = int32(size(posMatrix, 1));
+    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)];
+    totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
+                coder.ref(targets), int32(MAX_TARGETS));
+    numWaypoints = totalLoaded / int32(numClients);
+end
+
+% Load guidance scenario from CSV (parameters for guidance_step)
+NUM_SCENARIO_PARAMS = 45;
+MAX_OBSTACLES_CTRL  = int32(8);
+scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
+obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
+obstacleMax    = zeros(MAX_OBSTACLES_CTRL, 3);
+numObstacles   = int32(0);
+if ~coder.target('MATLAB')
+    coder.cinclude('controller_impl.h');
+    scenarioFilename = ['config/scenario.csv', char(0)];
+    coder.ceval('loadScenario', coder.ref(scenarioFilename), coder.ref(scenarioParams));
+    numObstacles = coder.ceval('loadObstacles', coder.ref(scenarioFilename), ...
+                         coder.ref(obstacleMin), coder.ref(obstacleMax), ...
+                         int32(MAX_OBSTACLES_CTRL));
+end
+% On MATLAB path, scenarioParams and obstacle arrays are left as zeros.
+% guidance_step's MATLAB path loads parameters directly from scenario.csv
+% via sim.initializeFromCsv and does not use these arrays.
+
+% Initialize server
+if coder.target('MATLAB')
+    disp('Initializing server (simulation)...');
+else
+    coder.ceval('initServer');
+end
+
+% Accept clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Accepting client ', num2str(i)]);
+    else
+        coder.ceval('acceptClient', int32(i));
+    end
+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
+    for i = 1:numClients
+        % Targets are grouped by client: client i's waypoints are at rows
+        % (i-1)*numWaypoints+1 through i*numWaypoints
+        targetIdx = (i - 1) * numWaypoints + w;
+        target = targets(targetIdx, :);
+
+        if coder.target('MATLAB')
+            disp([datestr(now, 'HH:MM:SS'), ' 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));
+        end
+    end
+
+    % Wait for ACK from all clients
+    if coder.target('MATLAB')
+        disp('Waiting for ACK from all clients...');
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.ACK));
+    end
+
+    % Wait for READY from all clients (all arrived at waypoint w)
+    if coder.target('MATLAB')
+        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.READY));
+    end
+end
+
+% ---- Phase 2: miSim guidance loop ----------------------------------------
+% Guidance parameters (adjust here and recompile as needed)
+MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+
+% Enter guidance mode on all clients
+if ~coder.target('MATLAB')
+    coder.ceval('sendGuidanceToggle', int32(numClients));
+end
+
+% Request initial GPS positions and initialise guidance algorithm
+positions = zeros(MAX_CLIENTS, 3);
+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
+    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+end
+guidance_step(positions(1:numClients, :), true, ...
+              scenarioParams, obstacleMin, obstacleMax, numObstacles);
+
+% Main guidance loop (event-triggered)
+for step = 1:MAX_GUIDANCE_STEPS
+    % Run one guidance step: feed current GPS positions in, get targets out
+    nextPositions = guidance_step(positions(1:numClients, :), false, ...
+                                  scenarioParams, obstacleMin, obstacleMax, numObstacles);
+
+    % Send target to each client
+    for i = 1:numClients
+        target = nextPositions(i, :);
+        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)]);
+        end
+    end
+
+    % Wait for ACK from all clients (each UAV ACKs when it arrives at its target)
+    if ~coder.target('MATLAB')
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.ACK));
+    else
+        disp(['[guidance] step ', num2str(step), ': all UAVs arrived']);
+    end
+
+    % Request current GPS positions from all clients
+    if ~coder.target('MATLAB')
+        coder.ceval('sendRequestPositions', int32(numClients));
+        coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
+    else
+        % Simulation: advance positions to guidance outputs for closed-loop feedback
+        positions(1:numClients, :) = nextPositions(1:numClients, :);
+    end
+end
+
+% Exit guidance mode on all clients (second toggle)
+if ~coder.target('MATLAB')
+    coder.ceval('sendGuidanceToggle', int32(numClients));
+    % Wait for ACK from all clients: confirms each client has finished its
+    % last guidance navigation and is back in sequential (ACK/READY) mode.
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+% --------------------------------------------------------------------------
+
+% Send RTL command to all clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending RTL to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL));
+    end
+end
+
+% Wait for ACK from all clients
+if coder.target('MATLAB')
+    disp('Waiting for ACK from all clients...');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+
+% Wait for READY from all clients (returned to home)
+if coder.target('MATLAB')
+    disp('All UAVs returned to home.');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.READY));
+end
+
+% Send LAND command to all clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending LAND to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND));
+    end
+end
+
+% Wait for ACK from all clients
+if coder.target('MATLAB')
+    disp('Waiting for ACK from all clients...');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+
+% Wait for READY from all clients (landed and disarmed)
+if coder.target('MATLAB')
+    disp('All UAVs landed and disarmed.');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.READY));
+end
+
+% Send READY to all clients to signal mission complete
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending READY to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY));
+    end
+end
+
+% Close server
+if ~coder.target('MATLAB')
+    coder.ceval('closeServer');
+end
+
+disp('Experiment complete.');
+
+end

aerpaw/guidance_step.m

@@ -0,0 +1,200 @@
+function nextPositions = guidance_step(currentPositions, isInit, ...
+                                       scenarioParams, ...
+                                       obstacleMin, obstacleMax, numObstacles)
+% guidance_step  One step of the miSim sensing coverage guidance algorithm.
+%
+% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW.
+% Holds full simulation state in a persistent variable between calls.
+%
+% Usage (from controller.m):
+%   guidance_step(initPositions, true,  scenarioParams, obstacleMin, obstacleMax, numObstacles)
+%   nextPos = guidance_step(gpsPos, false, scenarioParams, obstacleMin, obstacleMax, numObstacles)
+%
+% Inputs:
+%   currentPositions  (MAX_CLIENTS × 3) double  ENU [east north up] metres
+%   isInit            (1,1)             logical  true on first call only
+%   scenarioParams    (1 × NUM_SCENARIO_PARAMS) double
+%                       Flat array of guidance parameters (compiled path).
+%                       On MATLAB path this is ignored; parameters are loaded
+%                       from scenario.csv via initializeFromCsv instead.
+%                       Index mapping (1-based):
+%                         1  timestep            9-12  collisionRadius[1:4]
+%                         2  maxIter            13-16  comRange[1:4]
+%                         3  minAlt             17-20  alphaDist[1:4]
+%                         4  discretizationStep 21-24  betaDist[1:4]
+%                         5  protectedRange     25-28  alphaTilt[1:4]
+%                         6  initialStepSize    29-32  betaTilt[1:4]
+%                         7  barrierGain        33-35  domainMin
+%                         8  barrierExponent    36-38  domainMax
+%                                               39-40  objectivePos
+%                                               41-44  objectiveVar (2x2, col-major)
+%                                               45     sensorPerformanceMinimum
+%   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
+%   obstacleMax       (MAX_OBSTACLES × 3) double
+%   numObstacles      (1,1) int32                 actual obstacle count
+%
+% Output:
+%   nextPositions     (MAX_CLIENTS × 3) double  guidance targets, ENU metres
+%
+% Codegen notes:
+%   - Persistent variable 'sim' holds the miSim object between calls.
+%   - On MATLAB path isInit uses sim.initializeFromCsv (file I/O, not compiled).
+%   - On compiled path isInit uses scenarioParams + obstacle arrays directly.
+%   - Plotting/video are disabled (makePlots=false, makeVideo=false).
+
+coder.extrinsic('disp', 'objectiveFunctionWrapper', 'initializeFromCsv');
+
+MAX_CLIENTS = int32(4);      % must match MAX_CLIENTS in controller.m
+
+% Path to scenario CSV — used on MATLAB path only (not compiled)
+SCENARIO_CSV = 'aerpaw/config/scenario.csv';
+
+persistent sim;
+if isempty(sim)
+    sim = miSim;
+end
+
+% Pre-allocate output with known static size (required for codegen)
+nextPositions = zeros(MAX_CLIENTS, 3);
+
+numAgents = int32(size(currentPositions, 1));
+
+if isInit
+    if coder.target('MATLAB')
+        disp('[guidance_step] Initialising simulation...');
+
+        % MATLAB path: load all parameters and obstacles from scenario CSV.
+        % initializeFromCsv reads the file, builds domain/agents/obstacles,
+        % and calls sim.initialize internally.
+        sim = sim.initializeFromCsv(SCENARIO_CSV);
+
+        disp('[guidance_step] Initialisation complete.');
+    else
+        % ================================================================
+        % Compiled path: unpack scenarioParams array and obstacle arrays.
+        % Per-UAV parameters are stored as MAX_CLIENTS-wide blocks; only
+        % the first numAgents entries of each block are used.
+        % ================================================================
+        TIMESTEP            = scenarioParams(1);
+        MAX_ITER            = int32(scenarioParams(2));
+        MIN_ALT             = scenarioParams(3);
+        DISCRETIZATION_STEP = scenarioParams(4);
+        PROTECTED_RANGE     = scenarioParams(5);
+        INITIAL_STEP_SIZE   = scenarioParams(6);
+        BARRIER_GAIN        = scenarioParams(7);
+        BARRIER_EXPONENT    = scenarioParams(8);
+        COLLISION_RADIUS_VEC = scenarioParams(9:12);   % per-UAV [1:MAX_CLIENTS]
+        COMMS_RANGE_VEC      = scenarioParams(13:16);
+        ALPHA_DIST_VEC       = scenarioParams(17:20);
+        BETA_DIST_VEC        = scenarioParams(21:24);
+        ALPHA_TILT_VEC       = scenarioParams(25:28);
+        BETA_TILT_VEC        = scenarioParams(29:32);
+        DOMAIN_MIN                  = scenarioParams(33:35);
+        DOMAIN_MAX                  = scenarioParams(36:38);
+        OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
+        OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
+        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+
+        % --- Build domain geometry ---
+        dom = rectangularPrism;
+        dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
+
+        % --- Build sensing objective (inline Gaussian; 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);
+        dy = gridY - OBJECTIVE_GROUND_POS(2);
+        % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv())
+        ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2);
+        ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2);
+        ov_det = ov_a * ov_d - ov_b * ov_c;
+        objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+        dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
+            DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
+
+        % --- Initialise agents from GPS positions (per-UAV parameters) ----
+        agentList = cell(numAgents, 1);
+        for ii = 1:numAgents
+            pos  = currentPositions(ii, :);
+
+            % Per-UAV sensor model
+            sensor = sigmoidSensor;
+            sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
+                                       ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
+
+            geom = spherical;
+            geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
+                                   sprintf("UAV %d Collision", ii));
+            ag = agent;
+            ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
+                               INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+            agentList{ii} = ag;
+        end
+
+        % --- Build obstacle list from flat arrays ---
+        coder.varsize('obstacleList', [8, 1], [1, 0]);
+        obstacleList = cell(int32(0), 1);
+        for ii = 1:numObstacles
+            obs = rectangularPrism;
+            obs = obs.initialize([obstacleMin(ii, :); obstacleMax(ii, :)], ...
+                                 REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+            obstacleList{ii} = obs;
+        end
+
+        % --- 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);
+    end
+
+    % On the init call return current positions unchanged
+    for ii = 1:numAgents
+        nextPositions(ii, :) = currentPositions(ii, :);
+    end
+
+else
+    % =====================================================================
+    % One guidance step
+    % =====================================================================
+
+    % 1. Inject actual GPS positions (closed-loop feedback)
+    for ii = 1:size(sim.agents, 1)
+        sim.agents{ii}.lastPos = sim.agents{ii}.pos;
+        sim.agents{ii}.pos     = currentPositions(ii, :);
+
+        % Re-centre collision geometry at new position
+        d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center;
+        sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ...
+            sim.agents{ii}.collisionGeometry.center + d, ...
+            sim.agents{ii}.collisionGeometry.radius, ...
+            REGION_TYPE.COLLISION);
+    end
+
+    % 2. Advance timestep counter
+    sim.timestepIndex = sim.timestepIndex + 1;
+
+    % 3. Update communications topology (Lesser Neighbour Assignment)
+    sim = sim.lesserNeighbor();
+
+    % 4. Compute Voronoi partitioning
+    sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
+
+    % 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);
+    end
+
+    % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
+    sim = sim.constrainMotion();
+
+    % 7. Return constrained next positions
+    for ii = 1:size(sim.agents, 1)
+        nextPositions(ii, :) = sim.agents{ii}.pos;
+    end
+
+end
+
+end

aerpaw/impl/controller_impl.cpp

@@ -0,0 +1,588 @@
+#include "controller_impl.h"
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <cstdio>
+#include <limits>
+#include <sys/socket.h>
+#include <sys/select.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <time.h>
+
+#define SERVER_PORT 5000
+#define SERVER_IP "127.0.0.1"
+
+static int serverSocket = -1;
+static std::vector<int> clientSockets;
+
+void initSockets() {}
+void cleanupSockets() {}
+
+void initServer() {
+    initSockets();
+    serverSocket = socket(AF_INET, SOCK_STREAM, 0);
+    if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; }
+
+    sockaddr_in serverAddr;
+    serverAddr.sin_family = AF_INET;
+    serverAddr.sin_addr.s_addr = INADDR_ANY;
+    serverAddr.sin_port = htons(SERVER_PORT);
+
+    int opt = 1;
+    setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt));
+
+    if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
+        std::cerr << "Bind failed\n"; return;
+    }
+    if(listen(serverSocket, 5) < 0) {
+        std::cerr << "Listen failed\n"; return;
+    }
+
+    std::cout << "Server initialized\n";
+}
+
+void acceptClient(int clientId) {
+    sockaddr_in clientAddr;
+    socklen_t addrLen = sizeof(clientAddr);
+    int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen);
+    if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; }
+    clientSockets.push_back(clientSock);
+    std::cout << "Client " << clientId << " connected\n";
+}
+
+void closeServer() {
+    for(auto sock : clientSockets) {
+        close(sock);
+    }
+    close(serverSocket);
+    cleanupSockets();
+}
+
+// Load target coordinates from file
+// File format: one line per UAV with "x,y,z" coordinates
+// Returns number of targets loaded
+int loadTargets(const char* filename, double* targets, int maxClients) {
+    FILE* file = fopen(filename, "r");
+    if (!file) {
+        std::cerr << "Failed to open config file: " << filename << "\n";
+        return 0;
+    }
+
+    int count = 0;
+    char line[256];
+    bool inTargets = false;
+
+    // Simple YAML parser for targets section
+    // Expects format:
+    //   targets:
+    //     - [x, y, z]
+    //     - [x, y, z]
+    while (fgets(line, sizeof(line), file) && count < maxClients) {
+        // Check if we've entered the targets section
+        if (strstr(line, "targets:") != nullptr) {
+            inTargets = true;
+            continue;
+        }
+
+        // If we hit another top-level key (no leading whitespace), exit targets section
+        if (inTargets && line[0] != ' ' && line[0] != '\t' && line[0] != '\n' && line[0] != '#') {
+            break;
+        }
+
+        // Parse target entries: "  - [x, y, z]"
+        if (inTargets) {
+            double x, y, z;
+            // Try to match the array format
+            if (sscanf(line, " - [%lf, %lf, %lf]", &x, &y, &z) == 3) {
+                // MATLAB uses column-major order, so for a maxClients x 3 matrix:
+                // Column 1 (x): indices 0, 1, 2, ...
+                // Column 2 (y): indices maxClients, maxClients+1, ...
+                // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ...
+                targets[count + 0 * maxClients] = x;
+                targets[count + 1 * maxClients] = y;
+                targets[count + 2 * maxClients] = z;
+                std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n";
+                count++;
+            }
+        }
+    }
+
+    fclose(file);
+    return count;
+}
+
+// ---------------------------------------------------------------------------
+// Scenario CSV loading
+// ---------------------------------------------------------------------------
+
+// Split a CSV data row into fields, respecting quoted commas.
+// Inserts NUL terminators into `line` in place.
+// Returns the number of fields found.
+static int splitCSVRow(char* line, char** fields, int maxFields) {
+    int n = 0;
+    bool inQuote = false;
+    if (n < maxFields) fields[n++] = line;
+    for (char* p = line; *p && *p != '\n' && *p != '\r'; ++p) {
+        if (*p == '"') {
+            inQuote = !inQuote;
+        } else if (*p == ',' && !inQuote && n < maxFields) {
+            *p = '\0';
+            fields[n++] = p + 1;
+        }
+    }
+    // NUL-terminate the last field (strip trailing newline)
+    for (char* p = fields[n - 1]; *p; ++p) {
+        if (*p == '\n' || *p == '\r') { *p = '\0'; break; }
+    }
+    return n;
+}
+
+// Trim leading/trailing ASCII whitespace and remove enclosing double-quotes.
+// Modifies the string in place and returns the start of the trimmed content.
+static char* trimField(char* s) {
+    // Trim leading whitespace
+    while (*s == ' ' || *s == '\t') ++s;
+    // Remove enclosing quotes
+    size_t len = strlen(s);
+    if (len >= 2 && s[0] == '"' && s[len - 1] == '"') {
+        s[len - 1] = '\0';
+        ++s;
+    }
+    // Trim trailing whitespace
+    char* end = s + strlen(s) - 1;
+    while (end > s && (*end == ' ' || *end == '\t')) { *end-- = '\0'; }
+    return s;
+}
+
+// Open scenario.csv, skip the header row, return the data row in `line`.
+// Returns 1 on success, 0 on failure.
+static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
+    FILE* f = fopen(filename, "r");
+    if (!f) {
+        std::cerr << "loadScenario: cannot open " << filename << "\n";
+        return 0;
+    }
+    // Skip header row
+    if (!fgets(line, lineSize, f)) { fclose(f); return 0; }
+    // Read data row
+    int ok = (fgets(line, lineSize, f) != NULL);
+    fclose(f);
+    return ok ? 1 : 0;
+}
+
+// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
+// Index mapping (0-based):
+//   0-7  : timestep, maxIter, minAlt, discretizationStep, protectedRange,
+//           initialStepSize, barrierGain, barrierExponent
+//   8-11 : collisionRadius[1:4]  (per-UAV, MAX_CLIENTS_PER_PARAM slots)
+//   12-15: comRange[1:4]
+//   16-19: alphaDist[1:4]
+//   20-23: betaDist[1:4]
+//   24-27: alphaTilt[1:4]
+//   28-31: betaTilt[1:4]
+//   32-34: domainMin  (east, north, up)
+//   35-37: domainMax  (east, north, up)
+//   38-39: objectivePos (east, north)
+//   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
+//   44   : sensorPerformanceMinimum
+// Returns 1 on success, 0 on failure.
+int loadScenario(const char* filename, double* params) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 19) {
+        fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf);
+        return 0;
+    }
+
+    // Zero-initialise all params so unused per-UAV slots default to 0
+    for (int i = 0; i < NUM_SCENARIO_PARAMS; i++) params[i] = 0.0;
+
+    // Scalar fields (columns 0–7): timestep..barrierExponent
+    for (int i = 0; i < 8; i++) {
+        params[i] = atof(trimField(fields[i]));
+    }
+
+    // Per-UAV fields (columns 8–13): collisionRadius, comRange,
+    // alphaDist, betaDist, alphaTilt, betaTilt.
+    // Each is a quoted comma-separated list with up to MAX_CLIENTS_PER_PARAM values.
+    // Stored in params as consecutive blocks of MAX_CLIENTS_PER_PARAM (4) slots.
+    {
+        const int perUavCols[] = {8, 9, 10, 11, 12, 13};
+        const int perUavOffsets[] = {8, 12, 16, 20, 24, 28};  // 0-based param indices
+        const char* perUavNames[] = {"collisionRadius", "comRange",
+                                     "alphaDist", "betaDist",
+                                     "alphaTilt", "betaTilt"};
+        for (int p = 0; p < 6; p++) {
+            char tmp[256];
+            strncpy(tmp, fields[perUavCols[p]], sizeof(tmp) - 1);
+            tmp[sizeof(tmp) - 1] = '\0';
+            char* t = trimField(tmp);
+            // Parse comma-separated values
+            double vals[4] = {0, 0, 0, 0};
+            int count = 0;
+            char* tok = strtok(t, ",");
+            while (tok && count < MAX_CLIENTS_PER_PARAM) {
+                vals[count++] = atof(tok);
+                tok = strtok(nullptr, ",");
+            }
+            if (count == 0) {
+                fprintf(stderr, "loadScenario: failed to parse %s\n", perUavNames[p]);
+                return 0;
+            }
+            for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) {
+                params[perUavOffsets[p] + k] = vals[k];
+            }
+        }
+    }
+
+    // domainMin: column 14, format "east, north, up"
+    {
+        char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf , %lf", &params[32], &params[33], &params[34]) != 3) {
+            fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t);
+            return 0;
+        }
+    }
+
+    // domainMax: column 15
+    {
+        char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf , %lf", &params[35], &params[36], &params[37]) != 3) {
+            fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t);
+            return 0;
+        }
+    }
+
+    // objectivePos: column 16
+    {
+        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) {
+            fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
+            return 0;
+        }
+    }
+
+    // objectiveVar: column 17, format "v11, v12, v21, v22"
+    {
+        char tmp[256]; 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) {
+            fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
+            return 0;
+        }
+    }
+
+    // sensorPerformanceMinimum: column 18
+    {
+        char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[44] = atof(trimField(tmp));
+    }
+
+    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
+           params[32], params[33], params[34], params[35], params[36], params[37]);
+    return 1;
+}
+
+// Load initial UAV positions from scenario.csv (column 19: initialPositions).
+// targets is a column-major [maxClients x 3] array (same layout as loadTargets):
+//   targets[i + 0*maxClients] = east
+//   targets[i + 1*maxClients] = north
+//   targets[i + 2*maxClients] = up
+// Returns number of positions loaded.
+int loadInitialPositions(const char* filename, double* targets, int maxClients) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 20) {
+        fprintf(stderr, "loadInitialPositions: expected >=20 columns, got %d\n", nf);
+        return 0;
+    }
+
+    // Column 19: initialPositions flat "x1,y1,z1, x2,y2,z2, ..."
+    char tmp[1024]; strncpy(tmp, fields[19], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+    char* t = trimField(tmp);
+
+    double vals[3 * 4];  // up to MAX_CLIENTS triples
+    int parsed = 0;
+    char* tok = strtok(t, ",");
+    while (tok && parsed < 3 * maxClients) {
+        vals[parsed++] = atof(tok);
+        tok = strtok(nullptr, ",");
+    }
+
+    int count = parsed / 3;
+    for (int i = 0; i < count; i++) {
+        targets[i + 0 * maxClients] = vals[i * 3 + 0];  // east
+        targets[i + 1 * maxClients] = vals[i * 3 + 1];  // north
+        targets[i + 2 * maxClients] = vals[i * 3 + 2];  // up
+    }
+
+    printf("Loaded %d initial position(s) from scenario\n", count);
+    return count;
+}
+
+// Load obstacle bounding-box corners from scenario.csv.
+// Columns used: 20 (numObstacles), 21 (obstacleMin flat), 22 (obstacleMax flat).
+// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays:
+//   obstacleMin[obs + 0*maxObstacles] = east_min
+//   obstacleMin[obs + 1*maxObstacles] = north_min
+//   obstacleMin[obs + 2*maxObstacles] = up_min
+// Returns number of obstacles loaded (0 if none or on error).
+int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax,
+                  int maxObstacles) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 23) return 0;
+
+    // Column 20: numObstacles
+    int n = (int)atof(trimField(fields[20]));
+    if (n <= 0) return 0;
+    if (n > maxObstacles) {
+        fprintf(stderr, "loadObstacles: %d obstacles exceeds MAX_OBSTACLES=%d\n", n, maxObstacles);
+        n = maxObstacles;
+    }
+
+    // Column 21: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..."
+    {
+        char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        double vals[3 * 8];  // up to MAX_OBSTACLES triples
+        int parsed = 0;
+        char* tok = strtok(t, ",");
+        while (tok && parsed < 3 * n) {
+            vals[parsed++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (parsed < 3 * n) {
+            fprintf(stderr, "loadObstacles: obstacleMin has fewer values than expected\n");
+            return 0;
+        }
+        for (int obs = 0; obs < n; obs++) {
+            obstacleMin[obs + 0 * maxObstacles] = vals[obs * 3 + 0];  // east
+            obstacleMin[obs + 1 * maxObstacles] = vals[obs * 3 + 1];  // north
+            obstacleMin[obs + 2 * maxObstacles] = vals[obs * 3 + 2];  // up
+        }
+    }
+
+    // Column 22: obstacleMax flat
+    {
+        char tmp[1024]; strncpy(tmp, fields[22], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        double vals[3 * 8];
+        int parsed = 0;
+        char* tok = strtok(t, ",");
+        while (tok && parsed < 3 * n) {
+            vals[parsed++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (parsed < 3 * n) {
+            fprintf(stderr, "loadObstacles: obstacleMax has fewer values than expected\n");
+            return 0;
+        }
+        for (int obs = 0; obs < n; obs++) {
+            obstacleMax[obs + 0 * maxObstacles] = vals[obs * 3 + 0];
+            obstacleMax[obs + 1 * maxObstacles] = vals[obs * 3 + 1];
+            obstacleMax[obs + 2 * maxObstacles] = vals[obs * 3 + 2];
+        }
+    }
+
+    printf("Loaded %d obstacle(s) from scenario\n", n);
+    return n;
+}
+
+// Message type names for logging
+static const char* messageTypeName(uint8_t msgType) {
+    switch (msgType) {
+        case 1: return "TARGET";
+        case 2: return "ACK";
+        case 3: return "READY";
+        case 4: return "RTL";
+        case 5: return "LAND";
+        case 6: return "GUIDANCE_TOGGLE";
+        case 7: return "REQUEST_POSITION";
+        case 8: return "POSITION";
+        default: return "UNKNOWN";
+    }
+}
+
+// Send a single-byte message type to a client
+int sendMessageType(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;
+    }
+
+    std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
+    return 1;
+}
+
+// Send TARGET message with coordinates (1 byte type + 24 bytes coords)
+int sendTarget(int clientId, const double* coords) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
+
+    // Build message: 1 byte type + 3 doubles (little-endian)
+    uint8_t buffer[1 + 3 * sizeof(double)];
+    buffer[0] = 1;  // TARGET = 1
+    memcpy(buffer + 1, coords, 3 * sizeof(double));
+
+    ssize_t sent = send(clientSockets[clientId - 1], buffer, sizeof(buffer), 0);
+    if (sent != sizeof(buffer)) {
+        std::cerr << "Send target failed for client " << clientId << "\n";
+        return 0;
+    }
+
+    // Timestamp
+    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;
+}
+
+// Wait for a specific message type from ALL clients simultaneously using select()
+// Returns 1 if all clients responded with expected message type, 0 on failure
+int waitForAllMessageType(int numClients, int expectedType) {
+    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
+
+    uint8_t expected = (uint8_t)expectedType;
+    std::vector<bool> completed(numClients, false);
+    int completedCount = 0;
+
+    while (completedCount < numClients) {
+        // Build fd_set for select()
+        fd_set readfds;
+        FD_ZERO(&readfds);
+        int maxfd = -1;
+
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i]) {
+                FD_SET(clientSockets[i], &readfds);
+                if (clientSockets[i] > maxfd) maxfd = clientSockets[i];
+            }
+        }
+
+        // Wait for any socket to have data
+        int ready = select(maxfd + 1, &readfds, nullptr, nullptr, nullptr);
+        if (ready <= 0) return 0;
+
+        // Check each socket
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
+                uint8_t msgType;
+                int len = recv(clientSockets[i], &msgType, 1, 0);
+                if (len == 0) {
+                    std::cerr << "waitForAllMessageType: client " << (i + 1)
+                              << " disconnected while waiting for "
+                              << messageTypeName(expected) << "\n";
+                    return 0;
+                }
+                if (len < 0) {
+                    std::cerr << "waitForAllMessageType: recv error for client " << (i + 1)
+                              << " while waiting for " << messageTypeName(expected) << "\n";
+                    return 0;
+                }
+
+                std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
+
+                if (msgType == expected) {
+                    completed[i] = true;
+                    completedCount++;
+                }
+            }
+        }
+    }
+
+    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
+    }
+}
+
+// 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
+    }
+    return 1;
+}
+
+// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients.
+// Stores results in column-major order: positions[client + 0*maxClients] = x (east),
+//                                       positions[client + 1*maxClients] = y (north),
+//                                       positions[client + 2*maxClients] = z (up)
+int recvPositions(int numClients, double* positions, int maxClients) {
+    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
+
+    for (int i = 0; i < numClients; i++) {
+        // Expect: POSITION byte (1) + 3 doubles (24)
+        uint8_t msgType;
+        if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) {
+            std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n";
+            return 0;
+        }
+        if (msgType != 8) {  // POSITION = 8
+            std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType
+                      << " from client " << (i + 1) << "\n";
+            return 0;
+        }
+
+        double coords[3];
+        if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) {
+            std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n";
+            return 0;
+        }
+
+        // Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up
+        positions[i + 0 * maxClients] = coords[0];  // east  (x)
+        positions[i + 1 * maxClients] = coords[1];  // north (y)
+        positions[i + 2 * maxClients] = coords[2];  // up    (z)
+
+        std::cout << "Position from client " << (i + 1) << ": "
+                  << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
+    }
+    return 1;
+}
+
+// Sleep for the given number of milliseconds
+void sleepMs(int ms) {
+    struct timespec ts;
+    ts.tv_sec  = ms / 1000;
+    ts.tv_nsec = (ms % 1000) * 1000000L;
+    nanosleep(&ts, nullptr);
+}

aerpaw/impl/controller_impl.h

@@ -0,0 +1,71 @@
+#ifndef CONTROLLER_IMPL_H
+#define CONTROLLER_IMPL_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Server lifecycle
+void initServer();
+void acceptClient(int clientId);
+void closeServer();
+
+// Configuration
+int loadTargets(const char* filename, double* targets, int maxClients);
+
+// Scenario loading (scenario.csv)
+// Number of elements in the flat params array passed to guidance_step.
+// Indices (0-based):
+//   0-7   scalars (timestep..barrierExponent)
+//   8-11  collisionRadius[1:4]  (per-UAV, MAX_CLIENTS slots)
+//   12-15 comRange[1:4]
+//   16-19 alphaDist[1:4]
+//   20-23 betaDist[1:4]
+//   24-27 alphaTilt[1:4]
+//   28-31 betaTilt[1:4]
+//   32-34 domainMin
+//   35-37 domainMax
+//   38-39 objectivePos
+//   40-43 objectiveVar (2x2 col-major)
+//   44    sensorPerformanceMinimum
+#define NUM_SCENARIO_PARAMS 45
+#define MAX_CLIENTS_PER_PARAM 4
+// Maximum number of obstacles (upper bound for pre-allocated arrays).
+#define MAX_OBSTACLES 8
+
+// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
+// Returns 1 on success, 0 on failure.
+int loadScenario(const char* filename, double* params);
+
+// Load initial UAV positions from scenario.csv (initialPositions column).
+// targets is a column-major [maxClients x 3] array (same layout as loadTargets).
+// Returns number of positions loaded.
+int loadInitialPositions(const char* filename, double* targets, int maxClients);
+
+// Load obstacle bounding-box corners from scenario.csv.
+// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays
+// (same layout convention as loadTargets / recvPositions).
+// Returns the number of obstacles loaded.
+int loadObstacles(const char* filename,
+                  double* obstacleMin,
+                  double* obstacleMax,
+                  int maxObstacles);
+
+// Binary protocol operations
+int sendMessageType(int clientId, int msgType);
+int sendTarget(int clientId, const double* coords);
+int waitForAllMessageType(int numClients, int expectedType);
+
+// Guidance loop operations
+void sendGuidanceToggle(int numClients);
+int  sendRequestPositions(int numClients);
+int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
+void sleepMs(int ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // CONTROLLER_IMPL_H

aerpaw/impl/controller_main.cpp

@@ -0,0 +1,16 @@
+#include <iostream>
+#include "controller.h" 
+#include "controller_impl.h" // TCP implementation header
+
+int main() {
+    // Number of clients to handle
+    int numClients = 2;  // for now
+
+    std::cout << "Initializing TCP server...\n";
+
+    // Call MATLAB-generated server function
+    controller(numClients);
+
+    std::cout << "Server finished.\n";
+    return 0;
+}

aerpaw/impl/tmwtypes.h

@@ -0,0 +1,888 @@
+/*
+ * Copyright 1984-2023 The MathWorks, Inc.
+ */
+
+#if defined(_MSC_VER)
+# pragma once
+#endif
+#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3))
+# pragma once
+#endif
+
+#ifndef tmwtypes_h
+#define tmwtypes_h
+
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+/*
+ * File    : tmwtypes.h
+ * Abstract:
+ *      Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
+ *
+ *      When compiling stand-alone model code, data types can be overridden
+ *      via compiler switches.
+ *
+ *      Define NO_FLOATS to eliminate reference to real_T, etc.
+ */
+
+#ifdef MW_LIBTOOLING
+#include "mwstdint.h"
+#endif
+
+#include <limits.h>
+
+/* __STDC_VERSION__ version check below means "check for a C99 compiler".
+
+   Visual Studio (checked on versions 2015 and 2017) does
+   not define __STDC_VERSION__, however  it has stdbool.h available,
+   thus a separate check for _MSC_VER below.
+ */
+#if defined(__APPLE_CC__) \
+    || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \
+    || (defined(_MSC_VER) && (_MSC_VER >= 1900))
+#ifndef tmwtypes_do_not_include_stdbool
+#include <stdbool.h>
+#endif
+#endif
+
+#define LOGICAL_IS_A_TYPE
+#define SPARSE_GENERALIZATION
+
+#ifdef NO_FLOATS
+# define double double_not_allowed
+# define float  float_not_allowed
+#endif /*NO_FLOATS*/
+
+#ifndef NO_FLOATS
+
+#ifndef __MWERKS__
+# ifdef __STDC__
+#  include <float.h>
+# else
+#  ifndef FLT_MANT_DIG
+#   define FLT_MANT_DIG 24
+#  endif
+#  ifndef DBL_MANT_DIG
+#   define DBL_MANT_DIG 53
+#  endif
+# endif
+#endif
+
+#endif /*NO_FLOATS*/
+
+/*
+ *      The following data types cannot be overridden when building MEX files.
+ */
+#ifdef MATLAB_MEX_FILE
+# undef CHARACTER_T
+# undef INTEGER_T
+# undef BOOLEAN_T
+# undef REAL_T
+# undef TIME_T
+#endif
+
+/*
+ * The uchar_T, ushort_T and ulong_T types are needed for compilers which do 
+ * not allow defines to be specified, at the command line, with spaces in them.
+ */
+
+typedef unsigned char       uchar_T;
+typedef unsigned short      ushort_T;
+typedef unsigned long       ulong_T;
+
+#if (defined(_MSC_VER) && _MSC_VER >= 1500)      \
+    || defined(__x86_64__) || defined(__LP64__)  \
+    || defined(__LCC64__)
+    
+typedef unsigned long long  ulonglong_T;
+#endif
+
+
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+/* When used with Real Time Workshop generated code, this
+ * header file can be used with a variety of compilers.
+ *
+ * The compiler could be for an 8 bit embedded processor that
+ * only had 8 bits per integer and 16 bits per long.
+ * In that example, a 32 bit integer size is not even available.
+ * This header file should be robust to that.   
+ *
+ * For the case of an 8 bit processor, the preprocessor
+ * may be limited to 16 bit math like its target.  That limitation 
+ * would mean that 32 bit comparisons can't be done accurately.  
+ * To increase robustness to this, comparisons are done against
+ * smaller values first.  An inaccurate 32 bit comparison isn't
+ * attempted if the 16 bit comparison has already succeeded.
+ *
+ * Limitations on preprocessor math can also be stricter than
+ * for the target.  There are known cases where a compiler
+ * targeting processors with 64 bit longs can't do accurate
+ * preprocessor comparisons on more than 32 bits.  
+ */
+
+/* Determine the number of bits for int, long, short, and char.
+ * If one fails to be determined, set the number of bits to -1
+ */
+
+#ifndef TMW_BITS_PER_INT
+# if   INT_MAX  == 0x7FL
+#  define TMW_BITS_PER_INT 8
+# elif INT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_INT 16
+# elif INT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_INT 32
+# else
+#  define TMW_BITS_PER_INT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_LONG
+# if   LONG_MAX  == 0x7FL
+#  define TMW_BITS_PER_LONG 8
+# elif LONG_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_LONG 16
+# elif LONG_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_LONG 32
+# else
+#  define TMW_BITS_PER_LONG -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SHRT
+# if   SHRT_MAX  == 0x7FL
+#  define TMW_BITS_PER_SHRT 8
+# elif SHRT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SHRT 16
+# elif SHRT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SHRT 32
+# else
+#  define TMW_BITS_PER_SHRT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SCHAR
+# if   SCHAR_MAX  == 0x7FL
+#  define TMW_BITS_PER_SCHAR 8
+# elif SCHAR_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SCHAR 16
+# elif SCHAR_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SCHAR 32
+# else
+#  define TMW_BITS_PER_SCHAR -1
+# endif
+#endif
+
+#ifndef TMW_CHAR_SIGNED
+# if SCHAR_MAX == CHAR_MAX
+#  define TMW_CHAR_SIGNED 1
+# else
+#  define TMW_CHAR_SIGNED 0
+# endif
+#endif
+
+/* It is common for one or more of the integer types
+ * to be the same size.  For example, on many embedded
+ * processors, both shorts and ints are 16 bits.  On
+ * processors used for workstations, it is quite common
+ * for both int and long to be 32 bits.  
+ *   When there is more than one choice for typdef'ing
+ * a portable type like int16_T or uint32_T, in
+ * concept, it should not matter which choice is made.
+ * However, some style guides and some code checking
+ * tools do identify and complain about seemingly
+ * irrelevant differences.  For example, a code
+ * checking tool may complain about an implicit
+ * conversion from int to short even though both
+ * are 16 bits.  To reduce these types of
+ * complaints, it is best to make int the
+ * preferred choice when more than one is available.
+ */
+
+#ifndef INT8_T
+# if   defined(MW_LIBTOOLING)
+#  define INT8_T  int8_t
+# elif TMW_BITS_PER_INT   == 8
+#  define  INT8_T int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  INT8_T long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  INT8_T signed char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  INT8_T short
+# endif
+#endif
+#ifdef INT8_T
+ typedef INT8_T int8_T;
+#endif
+
+#ifndef UINT8_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT8_T uint8_t
+# elif   TMW_BITS_PER_INT   == 8
+#  define  UINT8_T unsigned int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  UINT8_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  UINT8_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  UINT8_T unsigned short
+# endif
+#endif
+#ifdef UINT8_T
+ typedef UINT8_T uint8_T;
+#endif
+
+
+#ifndef INT16_T
+# if   defined(MW_LIBTOOLING)
+#  define  INT16_T int16_t
+# elif   TMW_BITS_PER_INT   == 16
+#  define  INT16_T int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  INT16_T long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  INT16_T signed char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  INT16_T short
+# endif
+#endif
+#ifdef INT16_T
+ typedef INT16_T int16_T;
+#endif
+
+
+#ifndef UINT16_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT16_T uint16_t
+# elif TMW_BITS_PER_INT   == 16
+#  define  UINT16_T unsigned int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  UINT16_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  UINT16_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  UINT16_T unsigned short
+# endif
+#endif
+#ifdef UINT16_T
+ typedef UINT16_T uint16_T;
+#endif
+
+
+#ifndef INT32_T
+# if   defined(MW_LIBTOOLING)
+#  define  INT32_T int32_t
+# elif TMW_BITS_PER_INT   == 32
+#  define  INT32_T int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  INT32_T long
+# elif TMW_BITS_PER_SCHAR == 32
+#  define  INT32_T signed char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  INT32_T short
+# endif
+#endif
+#ifdef INT32_T
+ typedef INT32_T int32_T;
+#endif
+
+
+#ifndef UINT32_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT32_T uint32_t
+# elif TMW_BITS_PER_INT   == 32
+#  define  UINT32_T unsigned int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  UINT32_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 32
+#   define UINT32_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  UINT32_T unsigned short
+# endif
+#endif
+#ifdef UINT32_T
+ typedef UINT32_T uint32_T;
+#endif
+
+/* The following is used to emulate smaller integer types when only
+ * larger types are available.  For example, compilers for TI C3x/C4x DSPs 
+ * define char and short to be 32 bits, so 8 and 16 bits are not directly
+ * available.  This target is commonly used with RTW rapid prototyping.
+ * Other DSPs define char to be 16 bits, so 8 bits is not directly 
+ * available.
+ */
+#ifndef INT8_T
+# ifdef INT16_T
+#  define INT8_T INT16_T
+   typedef INT8_T int8_T;
+# else
+#  ifdef INT32_T
+#   define INT8_T INT32_T
+    typedef INT8_T int8_T;
+#  endif
+# endif
+#endif
+
+#ifndef UINT8_T
+# ifdef UINT16_T
+#   define UINT8_T UINT16_T
+    typedef UINT8_T uint8_T;
+# else
+#  ifdef UINT32_T
+#   define UINT8_T UINT32_T
+    typedef UINT8_T uint8_T;
+#  endif
+# endif
+#endif
+
+#ifndef INT16_T
+# ifdef INT32_T
+#   define INT16_T INT32_T
+    typedef INT16_T int16_T;
+# endif
+#endif
+
+#ifndef UINT16_T
+# ifdef UINT32_T
+#   define UINT16_T UINT32_T
+    typedef UINT16_T uint16_T;
+# endif
+#endif
+
+
+#ifndef NO_FLOATS
+
+#ifndef REAL32_T
+# ifndef __MWERKS__
+#  if FLT_MANT_DIG >= 23
+#   define REAL32_T float
+#  endif
+# else
+#  define REAL32_T float
+# endif
+#endif
+#ifdef REAL32_T
+ typedef REAL32_T real32_T;
+#endif
+
+
+#ifndef REAL64_T
+# ifndef __MWERKS__
+#  if DBL_MANT_DIG >= 52
+#   define REAL64_T double
+#  endif
+# else
+#  define REAL64_T double
+# endif
+#endif
+#ifdef REAL64_T
+ typedef REAL64_T real64_T;
+#endif
+
+#endif /* NO_FLOATS*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+
+# if   defined(MW_LIBTOOLING)
+#  ifdef INT64_T
+#    undef  INT64_T
+#  endif
+#  define  INT64_T int64_t
+#  ifdef UINT64_T
+#    undef  UINT64_T
+#  endif
+#  define  UINT64_T uint64_t
+# endif
+#if !defined(INT64_T) || !defined(UINT64_T) || !defined(FMT64)
+# if defined(__APPLE__) || defined(__clang__)
+#  ifndef INT64_T
+#   define INT64_T long long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "ll"
+#  endif
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__) 
+#  ifndef INT64_T
+#   define INT64_T long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "l"
+#  endif
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  ifndef INT64_T
+#   define INT64_T __int64
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned __int64
+#  endif
+#  ifndef FMT64
+#   define FMT64 "I64"
+#  endif
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  ifndef INT64_T
+#   define INT64_T long long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "ll"
+#  endif
+# endif
+
+#endif
+
+#if defined(INT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef INT64_T int64_T;
+#endif
+
+#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \
+                    || defined(__x86_64__) \
+                    || defined(__LP64__)
+#  define INT_TYPE_64_IS_SUPPORTED
+#endif
+
+#if defined(UINT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef UINT64_T uint64_T;
+#endif
+
+/*===========================================================================*
+ * Format string modifiers for using size_t variables in printf statements.  *
+ *===========================================================================*/
+
+#ifndef FMT_SIZE_T
+#  if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__)
+#    define FMT_SIZE_T "z"
+#  elif defined (__WATCOMC__)
+#    define FMT_SIZE_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_SIZE_T "I"
+#  else
+#    define FMT_SIZE_T "l"
+#  endif
+#endif
+
+#ifndef FMT_PTRDIFF_T
+#  if defined(__APPLE__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined( __GNUC__ ) || defined(_STDC_C99)
+#    define FMT_PTRDIFF_T "t"
+#  elif defined (__WATCOMC__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_PTRDIFF_T "I"
+#  else
+#    define FMT_PTRDIFF_T "l"
+#  endif
+#endif
+
+/*===========================================================================*
+ * General or logical data types where the word size is not guaranteed.      *
+ *  real_T     - possible settings include real32_T or real64_T              *
+ *  time_T     - possible settings include real32_T or real64_T              *
+ *  boolean_T                                                                *
+ *  char_T                                                                   *
+ *  int_T                                                                    *
+ *  uint_T                                                                   *
+ *  byte_T                                                                   *
+ *===========================================================================*/
+
+#ifndef NO_FLOATS
+
+#ifndef REAL_T
+# ifdef REAL64_T
+#  define REAL_T real64_T
+# else
+#  ifdef REAL32_T
+#   define REAL_T real32_T
+#  endif
+# endif
+#endif
+#ifdef REAL_T
+ typedef REAL_T real_T;
+#endif
+
+#ifndef TIME_T
+# ifdef REAL_T
+#  define TIME_T real_T
+# endif
+#endif
+#ifdef TIME_T
+ typedef TIME_T time_T;
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef BOOLEAN_T
+# if defined(UINT8_T)
+#  define BOOLEAN_T UINT8_T
+# else
+#  define BOOLEAN_T unsigned int
+# endif
+#endif
+typedef BOOLEAN_T boolean_T;
+
+
+#ifndef CHARACTER_T
+# define CHARACTER_T char
+#endif
+typedef CHARACTER_T char_T;
+
+
+#ifndef INTEGER_T
+# define INTEGER_T int
+#endif
+typedef INTEGER_T int_T;
+
+
+#ifndef UINTEGER_T
+# define UINTEGER_T unsigned
+#endif
+typedef UINTEGER_T uint_T;
+
+
+#ifndef BYTE_T
+# define BYTE_T unsigned char
+#endif
+typedef BYTE_T byte_T;
+
+
+/*===========================================================================*
+ * Define Complex Structures                                                 *
+ *===========================================================================*/
+#ifndef NO_FLOATS
+
+#ifndef CREAL32_T
+#  ifdef REAL32_T
+    typedef struct {
+      real32_T re, im;
+    } creal32_T;
+#    define CREAL32_T creal32_T
+#  endif
+#endif
+
+#ifndef CREAL64_T
+#  ifdef REAL64_T
+    typedef struct {
+      real64_T re, im;
+    } creal64_T;
+#    define CREAL64_T creal64_T
+#  endif
+#endif
+
+#ifndef CREAL_T
+#  ifdef REAL_T
+    typedef struct {
+      real_T re, im;
+    } creal_T;
+#    define CREAL_T creal_T
+#  endif
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef CINT8_T
+#  ifdef INT8_T
+    typedef struct {
+      int8_T re, im;
+    } cint8_T;
+#    define CINT8_T cint8_T
+#  endif
+#endif
+
+#ifndef CUINT8_T
+#  ifdef UINT8_T
+    typedef struct {
+      uint8_T re, im;
+    } cuint8_T;
+#    define CUINT8_T cuint8_T
+#  endif
+#endif
+
+#ifndef CINT16_T
+#  ifdef INT16_T
+    typedef struct {
+      int16_T re, im;
+    } cint16_T;
+#    define CINT16_T cint16_T
+#  endif
+#endif
+
+#ifndef CUINT16_T
+#  ifdef UINT16_T
+    typedef struct {
+      uint16_T re, im;
+    } cuint16_T;
+#    define CUINT16_T cuint16_T
+#  endif
+#endif
+
+#ifndef CINT32_T
+#  ifdef INT32_T
+    typedef struct {
+      int32_T re, im;
+    } cint32_T;
+#    define CINT32_T cint32_T
+#  endif
+#endif
+
+#ifndef CUINT32_T
+#  ifdef UINT32_T
+    typedef struct {
+      uint32_T re, im;
+    } cuint32_T;
+#    define CUINT32_T cuint32_T
+#  endif
+#endif
+
+#ifndef CINT64_T
+#  ifdef INT64_T
+    typedef struct {
+      int64_T re, im;
+    } cint64_T;
+#    define CINT64_T cint64_T
+#  endif
+#endif
+
+#ifndef CUINT64_T
+#  ifdef UINT64_T
+    typedef struct {
+      uint64_T re, im;
+    } cuint64_T;
+#    define CUINT64_T cuint64_T
+#  endif
+#endif
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+
+#define  MAX_int8_T      ((int8_T)(127))            /* 127  */
+#define  MIN_int8_T      ((int8_T)(-128))           /* -128 */
+#define  MAX_uint8_T     ((uint8_T)(255))           /* 255  */
+#define  MIN_uint8_T     ((uint8_T)(0))
+                           
+#define  MAX_int16_T     ((int16_T)(32767))         /* 32767 */
+#define  MIN_int16_T     ((int16_T)(-32768))        /* -32768 */
+#define  MAX_uint16_T    ((uint16_T)(65535))        /* 65535 */
+#define  MIN_uint16_T    ((uint16_T)(0))
+                           
+#define  MAX_int32_T     ((int32_T)(2147483647))    /* 2147483647  */
+#define  MIN_int32_T     ((int32_T)(-2147483647-1)) /* -2147483648 */
+#define  MAX_uint32_T    ((uint32_T)(0xFFFFFFFFU))  /* 4294967295  */
+#define  MIN_uint32_T    ((uint32_T)(0))
+
+#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                      || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100) \
+                      || defined(__LCC64__)
+#  ifdef INT64_T
+#    define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#    define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#  endif
+#  ifdef UINT64_T
+#    define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#    define  MIN_uint64_T    ((uint64_T)(0))
+#  endif
+#else
+#  ifdef INT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807L))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807L-1L))
+#    else
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#    endif
+#  endif
+#  ifdef UINT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    else
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    endif
+#  endif
+#endif
+
+#if (defined(_MSC_VER) && !defined(__clang__))
+
+/* Conversion from unsigned __int64 to double is not implemented in Visual Studio
+ * and results in a compile error, thus the value must first be cast to
+ * signed __int64, and then to double.
+ *
+ * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
+ * the macro below provides a workaround for casting a uint64 value to a double
+ * in windows.
+ */
+#  define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
+            (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
+            (double)(__int64)(u) )
+
+/* The following inline function should only be used in the macro double_to_uint64,
+ * as it only handles the specfic range of double between 2^63 and 2^64-1 */
+__forceinline
+uint64_T double_to_uint64_helper(double d) {
+  union double_to_uint64_union_type {
+    double dd;
+    uint64_T i64;
+  } di;
+  di.dd = d;
+  return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11);
+}
+
+/* The largest double value that can be cast to uint64 in windows is the
+ * signed int64 max, which is 2^63-1. The macro below provides
+ * a workaround for casting large double values to uint64 in windows.
+ */
+/* The magic number 18446744073709551616.0 is 2^64 */
+/* The magic number 9223372036854775808.0 is 2^63 */
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            0xffffffffffffffffULL : \
+            ((d) >= 0.0) ? \
+            ((d) >= 9223372036854775808.0) ? \
+            double_to_uint64_helper(d) : \
+            (unsigned __int64)(d) : \
+            0ULL )
+#else
+#  define uint64_to_double(u) ((double)(u))
+#  if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
+/* double_to_uint64 defined only for MSVC and UNIX */
+#  else
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            (unsigned long long) 0xffffffffffffffffULL : \
+            ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 )
+#  endif
+#endif
+
+#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
+
+#ifndef _bool_T
+#define _bool_T
+
+typedef boolean_T bool;
+
+#ifndef false
+#define false (0)
+#endif
+#ifndef true 
+#define true (1)
+#endif
+
+#endif /* _bool_T */
+
+#endif /* !__cplusplus */
+
+/* 
+ * This software assumes that the code is being compiled on a target using a 
+ * 2's complement representation for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable/model)
+ * including the null-termination character.
+ */
+#define TMW_NAME_LENGTH_MAX 2049
+
+/*
+ * Maximum values for indices and dimensions
+ */
+#include <stddef.h>
+
+#ifdef MX_COMPAT_32
+typedef int mwSize;
+typedef int mwIndex;
+typedef int mwSignedIndex;
+#else
+typedef size_t    mwSize;         /* unsigned pointer-width integer */
+typedef size_t    mwIndex;        /* unsigned pointer-width integer */
+typedef ptrdiff_t mwSignedIndex;  /* a signed pointer-width integer */
+#endif
+
+                                  /* for the individual dim */
+/* If updating SLSize or SLIndex, update defintions in sl_types_def.h
+   as well. */
+#ifndef SLSIZE_SLINDEX
+  #define SLSIZE_SLINDEX
+  #ifdef INT_TYPE_64_IS_SUPPORTED
+    typedef int64_T SLIndex;
+    typedef int64_T SLSize;
+  #else
+    typedef int SLIndex;
+    typedef int SLSize;
+  #endif
+#endif
+
+/* for the total size */
+#define SLIndexType size_t
+#define INVALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max())
+#define MAX_VALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max() -1)
+
+
+#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
+/* Currently 2^48 based on hardware limitations */
+# define MWSIZE_MAX    281474976710655UL
+# define MWINDEX_MAX   281474976710655UL
+# define MWSINDEX_MAX  281474976710655L
+# define MWSINDEX_MIN -281474976710655L
+#else
+# define MWSIZE_MAX    2147483647UL
+# define MWINDEX_MAX   2147483647UL
+# define MWSINDEX_MAX  2147483647L
+# define MWSINDEX_MIN -2147483647L
+#endif
+#define MWSIZE_MIN    0UL
+#define MWINDEX_MIN   0UL
+
+/** UTF-16 character type */
+
+#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT)
+typedef char16_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX u
+#elif defined(_MSC_VER)
+typedef wchar_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX L
+#else
+typedef UINT16_T CHAR16_T;
+#endif
+
+#endif  /* __TMWTYPES__ */
+
+#endif /* tmwtypes_h */

aerpaw/loadTargetsFromYaml.m

@@ -0,0 +1,42 @@
+function targets = loadTargetsFromYaml(filename)
+%LOADTARGETSFROMYAML Load target coordinates from YAML config file
+%   targets = loadTargetsFromYaml(filename) reads the targets section
+%   from a YAML config file and returns an Nx3 matrix of [x, y, z] coordinates.
+
+targets = [];
+fid = fopen(filename, 'r');
+if fid == -1
+    error('Could not open file: %s', filename);
+end
+
+inTargets = false;
+while ~feof(fid)
+    line = fgetl(fid);
+    if ~ischar(line)
+        break;
+    end
+
+    % Check if we've entered the targets section
+    if contains(line, 'targets:')
+        inTargets = true;
+        continue;
+    end
+
+    % If we hit another top-level key, exit targets section
+    if inTargets && ~isempty(line) && line(1) ~= ' ' && line(1) ~= '#'
+        break;
+    end
+
+    % Parse target entries: "  - [x, y, z]"
+    if inTargets
+        % Extract numbers from array format
+        match = regexp(line, '\[\s*([-\d.]+)\s*,\s*([-\d.]+)\s*,\s*([-\d.]+)\s*\]', 'tokens');
+        if ~isempty(match)
+            coords = str2double(match{1});
+            targets = [targets; coords]; %#ok<AGROW>
+        end
+    end
+end
+
+fclose(fid);
+end

aerpaw/results/plotGpsCsvs.m

@@ -0,0 +1,68 @@
+% Plot setup
+f = uifigure;
+gf = geoglobe(f);
+hold(gf, "on");
+c = ["g", "b", "m", "c"]; % plotting colors
+
+% paths
+scenarioCsv = fullfile("aerpaw", "config", "scenario.csv");
+
+% configured data
+params = readScenarioCsv(scenarioCsv);
+
+% coordinate system constants
+seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
+
+fID = fopen(fullfile("aerpaw", "config", "client.yaml"), 'r');
+yaml = fscanf(fID, '%s');
+fclose(fID);
+% origin (LLA)
+lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel];
+
+% Paths to logs
+gpsCsvs = dir(fullfile("sandbox", "test10", "*.csv"));
+
+G = cell(size(gpsCsvs));
+for ii = 1:size(gpsCsvs, 1)
+     % Read CSV
+    G{ii} = readGpsCsv(fullfile(gpsCsvs(ii).folder, gpsCsvs(ii).name));
+
+    % Find when algorithm begins/ends (using ENU altitude rate change)
+    enuTraj = lla2enu([G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude], lla0, 'flat');
+
+    verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]);
+
+    % Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight)
+    pctThreshold = 60; % pctThreshold may need adjusting depending on your flight
+    startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'first'); 
+    stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'last');
+
+    % % Plot whole flight, including setup/cleanup
+    startIdx = 1;
+    stopIdx = length(verticalSpeed);
+    
+    % 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);
+end
+
+% Plot domain
+altOffset = 1; % to avoid clipping into the ground when displayed
+domain = [lla0; enu2lla(params.domainMax, lla0, 'flat')];
+geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
+geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
+geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+
+% Plot floor (minimum altitude constraint)
+floorAlt = params.minAlt;
+geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset + floorAlt, 1, 5), 'LineWidth', 3, 'Color', 'r');
+
+% Plot objective
+objectivePos = [params.objectivePos, 0];
+llaObj = enu2lla(objectivePos, lla0, 'flat');
+geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
+
+% finish
+hold(gf, "off");

aerpaw/results/readGpsCsv.m

@@ -0,0 +1,33 @@
+function [G] = readGpsCsv(csvPath)
+arguments (Input)
+    csvPath (1, 1) string {isfile(csvPath)};
+end
+
+arguments (Output)
+    G (:, 10) table;
+end
+
+G = readtable(csvPath, "ReadVariableNames", false);
+
+% first column is just index, meaningless, toss it
+G = G(:, 2:end);
+
+% switch to the correct LLA convention (lat, lon, alt)
+tmp = G(:, 2);
+G(:, 2) = G(:, 1);
+G(:, 1) = tmp;
+
+% Split pitch, yaw, roll data read in as one string per timestep into separate columns
+PYR = cell2mat(cellfun(@(x) str2num(strip(strip(x, "left", "("), "right", ")")), table2cell(G(:, 5)), "UniformOutput", false)); %#ok<ST2NM>
+% Reinsert to original table
+G = [G(:, 1:3), table(PYR(:, 1), VariableNames="Pitch"), table(PYR(:, 2), VariableNames="Yaw"), table(PYR(:, 3), VariableNames="Roll"), G(:, 6:end)];
+
+% Clean up datetime entry
+G = [table(datetime(G{:,8}, "InputFormat","yyyy-MM-dd HH:mm:ss.SSS", "TimeZone","America/New_York")), G(:, [1:7, 9:10])];
+
+% Fix variable names
+G.Properties.VariableNames = ["Timestamp", "Latitude", "Longitude", "Altitude", "Pitch", "Yaw", "Roll", "Voltage", "GPS Status", "Satellites"];
+G.Properties.VariableTypes = ["datetime", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+G.Properties.VariableUnits = ["yyyy-MM-dd HH:mm:ss.SSS (UTC+5)", "deg", "deg", "m", "deg", "deg", "deg", "Volts", "", ""];
+
+end

aerpaw/run_uav.sh

@@ -0,0 +1,54 @@
+#!/bin/bash
+# run_uav.sh - Wrapper for UAV runner
+# Launches UAV client with environment-specific configuration
+#
+# Usage:
+#   ./run_uav.sh local     # Use local/simulation configuration
+#   ./run_uav.sh testbed   # Use AERPAW testbed configuration
+
+set -e
+
+# Change to script directory
+cd "$(dirname "$0")"
+
+# Activate venv if it exists
+if [ -d "venv" ]; then
+    source venv/bin/activate
+fi
+
+# Determine environment from argument (required)
+if [ "$1" = "testbed" ]; then
+    ENV="testbed"
+elif [ "$1" = "local" ]; then
+    ENV="local"
+else
+    echo "Error: Environment not specified."
+    echo "Usage: $0 [local|testbed]"
+    echo ""
+    echo "  local   - Use local/simulation configuration"
+    echo "  testbed - Use AERPAW testbed configuration"
+    exit 1
+fi
+
+echo "[run_uav] Environment: $ENV"
+
+# Export environment for Python to use
+export AERPAW_ENV="$ENV"
+
+# Read MAVLink connection from config.yaml using Python
+CONN=$(python3 -c "
+import yaml
+with open('config/client.yaml') as f:
+    cfg = yaml.safe_load(f)
+env = cfg['environments']['$ENV']['mavlink']
+print(f\"udp:{env['ip']}:{env['port']}\")
+")
+
+echo "[run_uav] MAVLink connection: $CONN"
+
+# Run via aerpawlib
+echo "[run_uav] Starting UAV runner..."
+python3 -m aerpawlib \
+    --script client.uav_runner \
+    --conn "$CONN" \
+    --vehicle drone

geometries/@cone/cone.m

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

geometries/@cone/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, center, radius, height, tag, label)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
         center (1, 3) double;
         radius (1, 1) double;
         height (1, 1) double;
@@ -8,7 +8,7 @@ function obj = initialize(obj, center, radius, height, tag, label)
         label (1, 1) string = "";
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
     end
 
     obj.center = center;

geometries/@cone/plot.m

@@ -1,34 +1,37 @@
-function [obj, f] = plot(obj, ind, f)
+function [obj, f] = plot(obj, ind, f, maxAlt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
+        maxAlt (1, 1) = 10;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'cone')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        obj (1, 1) {mustBeA(obj, "cone")};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
     
     % Create axes if they don't already exist
     f = firstPlotSetup(f);
     
+    scalingFactor = (maxAlt / obj.height);
+
     % Plot cone
-    [X, Y, Z] = cylinder([obj.radius, 0], obj.n);
+    [X, Y, Z] = cylinder([scalingFactor * obj.radius, 0], obj.n);
     
     % Scale to match height
-    Z = Z * obj.height;
+    Z = Z * maxAlt;
     
     % Move to center location
     X = X + obj.center(1);
     Y = Y + obj.center(2);
-    Z = Z + obj.center(3);
+    Z = Z + obj.center(3) - maxAlt;
     
     % Plot
     if isnan(ind)
         o = surf(f.CurrentAxes, X, Y, Z);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
+        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(regionTypeColor(obj.tag), 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
         hold(f.Children(1).Children(ind(1)), "off");
     end
     

geometries/@rectangularPrism/closestToPoint.m

@@ -0,0 +1,19 @@
+function cPos = closestToPoint(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        cPos (:, 3) double;
+    end
+    cPos = NaN(1, 3);
+    for ii = 1:3
+        if pos(ii) < obj.minCorner(ii)
+            cPos(ii) = obj.minCorner(ii);
+        elseif pos(ii) > obj.maxCorner(ii)
+            cPos(ii) = obj.maxCorner(ii);
+        else
+            cPos(ii) = pos(ii);
+        end
+    end
+end

geometries/@rectangularPrism/contains.m

@@ -1,6 +1,6 @@
 function c = contains(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/containsLine.m

@@ -1,6 +1,6 @@
 function c = containsLine(obj, pos1, pos2)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos1 (1, 3) double;
         pos2 (1, 3) double;
     end
@@ -10,32 +10,37 @@ function c = containsLine(obj, pos1, pos2)
     
     d = pos2 - pos1;
 
-    % edge case where the line is parallel to the geometry
-    if abs(d) < 1e-12
-        % check if it happens to start or end inside or outside of
-        % the geometry
-        if obj.contains(pos1) || obj.contains(pos2)
-            c = true;
-        else
-            c = false;
-        end
+    % endpoint contained (trivial case)
+    if obj.contains(pos1) || obj.contains(pos2)
+        c = true;
         return;
     end
 
-    tmin = -inf;        
-    tmax = inf;
-    
-    % Standard case
+    % parameterize the line segment to check for an intersection
+    tMin = 0;
+    tMax = 1;
     for ii = 1:3
-        t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
-        t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii);
-        tmin = max(tmin, min(t1, t2));
-        tmax = min(tmax, max(t1, t2));
-        if tmin > tmax
-            c = false;
-            return;
+        % line is parallel to geometry
+        if abs(d(ii)) < 1e-9
+            if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii)
+                c = false;
+                return;
+            end
+        else
+            t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
+            t2 = (obj.maxCorner(ii) - pos1(ii)) / d(ii);
+
+            tLow  = min(t1, t2);
+            tHigh = max(t1, t2);
+
+            tMin = max(tMin, tLow);
+            tMax = min(tMax, tHigh);
+
+            if tMin > tMax
+                c = false;
+                return;
+            end
         end
     end
-    
-    c = (tmax >= 0) && (tmin <= 1);
-    end
+    c = true;
+end

geometries/@rectangularPrism/distance.m

@@ -1,10 +1,10 @@
 function d = distance(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)
-        d (:, 1) double
+        d (:, 1) double;
     end
     if obj.contains(pos)
         % Queried point is inside geometry

geometries/@rectangularPrism/distanceGradient.m

@@ -0,0 +1,42 @@
+function g = distanceGradient(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        g (:, 3) double
+    end
+    
+    % find nearest point on surface to query position
+    q = min(max(pos, obj.minCorner), obj.maxCorner);
+
+    % Find distance and direction between pos and q
+    v = pos - q;
+    vNorm = norm(v);
+
+    % position is outside geometry
+    if vNorm > 0
+        % gradient is normalized vector from q to p
+        g = v / vNorm;
+        return;
+    end
+
+    % position is on or in geometry
+    % find distances to each face in each dimension
+    distances = [pos(1) - obj.minCorner(1), obj.maxCorner(1) - pos(1), pos(2) - obj.minCorner(2), obj.maxCorner(2) - pos(2), pos(3) - obj.minCorner(3), obj.maxCorner(3) - pos(3)];
+    [~, idx] = min(distances);
+
+    % I think there needs to be additional handling here for the
+    % edge/corner cases, where there are ways to balance or resolve ties
+    % when two faces are equidistant to the query position
+    assert(sum(idx) == idx, "Implement edge case handling");
+
+    % select gradient that brings us quickest to the nearest face
+    g = [ 1,  0,  0; ...
+         -1,  0,  0; ...
+          0,  1,  0; ...
+          0, -1,  0; ...
+          0,  0,  1; ...
+          0,  0, -1;];
+    g = g(idx, :);
+end

geometries/@rectangularPrism/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         bounds (2, 3) double;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
@@ -8,7 +8,7 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
         discretizationStep (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
 
     obj.tag = tag;
@@ -24,6 +24,10 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     % Compute center
     obj.center = obj.minCorner + obj.dimensions ./ 2;
 
+    % Compute a (fake) radius
+    % fully contains the rectangular prism from the center
+    obj.radius = (1/2) * sqrt(sum(obj.dimensions.^2));
+
     % Compute vertices
     obj.vertices = [obj.minCorner;
                     obj.maxCorner(1), obj.minCorner(2:3);

geometries/@rectangularPrism/initializeRandom.m

@@ -1,14 +1,15 @@
-function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain)
+function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
         minDimension (1, 1) double = 10;
-        maxDimension (1, 1) double= 20;
+        maxDimension (1, 1) double = 20;
         domain (1, 1) {mustBeGeometry} = rectangularPrism;
+        minAlt (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
     
     % Produce random bounds based on region type
@@ -27,7 +28,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d
         while ~domain.contains(candidateMaxCorner) || all(domain.objective.groundPos + domain.objective.protectedRange >= candidateMinCorner(1:2), 2) && all(domain.objective.groundPos - domain.objective.protectedRange <= candidateMaxCorner(1:2), 2)
             if ii == 0 || ii > 10
                 candidateMinCorner = domain.random();
-                candidateMinCorner(3) = 0; % bind to floor
+                candidateMinCorner(3) = minAlt; % bind to floor (plus minimum altitude constraint)
                 ii = 1;
             end
 

geometries/@rectangularPrism/plotWireframe.m

@@ -1,12 +1,12 @@
 function [obj, f] = plotWireframe(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -19,10 +19,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
 
     % Plot the boundaries of the geometry into 3D view
     if isnan(ind)
-        o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

geometries/@rectangularPrism/random.m

@@ -1,6 +1,6 @@
 function r = random(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
     arguments (Output)
         r (1, 3) double

geometries/@rectangularPrism/rectangularPrism.m

@@ -3,7 +3,6 @@ classdef rectangularPrism
     properties (SetAccess = private, GetAccess = public)
         % Meta
         tag = REGION_TYPE.INVALID;
-        label = "";
 
         % Spatial
         minCorner = NaN(1, 3);
@@ -11,6 +10,7 @@ classdef rectangularPrism
         dimensions = NaN(1, 3);
         center = NaN;
         footprint = NaN(4, 2);
+        radius = NaN; % fake radius
 
         % Graph
         vertices = NaN(8, 3);
@@ -22,16 +22,25 @@ classdef rectangularPrism
         lines;
     end
     properties (SetAccess = public, GetAccess = public)
+        label = "";
         % Sensing objective (for DOMAIN region type only)
         objective;
     end
 
     methods (Access = public)
+        function obj = rectangularPrism()
+            arguments (Output)
+                obj (1, 1) rectangularPrism
+            end
+            obj.objective = sensingObjective;
+        end
         [obj   ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
         [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
+        [cPos  ] = closestToPoint(obj, pos);
         [d     ] = distance(obj, pos);
+        [g     ] = distanceGradient(obj, pos);
         [c     ] = containsLine(obj, pos1, pos2);
         [obj, f] = plotWireframe(obj, ind, f);
     end

geometries/@spherical/contains.m

@@ -0,0 +1,10 @@
+function c = contains(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        c (:, 1) logical
+    end
+    c = norm(obj.center - pos) <= obj.radius;
+end

geometries/@spherical/containsLine.m

@@ -0,0 +1,28 @@
+function c = containsLine(obj, pos1, pos2)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+        pos1 (1, 3) double;
+        pos2 (1, 3) double;
+    end
+    arguments (Output)
+        c (1, 1) logical
+    end
+    
+    d = pos2 - pos1;
+    f = pos1 - obj.center;
+
+    a = dot(d, d);
+    b = 2 * dot(f, d);
+    c = dot(f, f) - obj.radius^2;
+
+    disc = b^2 - 4*a*c;
+
+    if disc < 0
+        c = false;
+        return;
+    end
+
+    t = [(-b - sqrt(disc)) / (2 * a), (-b + sqrt(disc)) / (2 * a)];
+
+    c = (t(1) >= 0 && t(1) <= 1) || (t(2) >= 0 && t(2) <= 1);
+end

geometries/@spherical/initialize.m

@@ -0,0 +1,37 @@
+function obj = initialize(obj, center, radius, tag, label)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+        center (1, 3) double;
+        radius  (1, 1) double;
+        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        label (1, 1) string = "";
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+    end
+
+    obj.tag = tag;
+    obj.label = label;
+
+    % Define geometry
+    obj.center = center;
+    obj.radius = radius;
+    obj.diameter = 2 * obj.radius;
+
+    % fake vertices in a cross pattern
+    obj.vertices = [obj.center + [obj.radius, 0, 0]; ...
+                    obj.center - [obj.radius, 0, 0]; ...
+                    obj.center + [0, obj.radius, 0]; ...
+                    obj.center - [0, obj.radius, 0]; ...
+                    obj.center + [0, 0, obj.radius]; ...
+                    obj.center - [0, 0, obj.radius]];
+    % fake edges in two perpendicular rings
+    obj.edges = [1, 3; ...
+                 3, 2; ...
+                 2, 4; ...
+                 4, 1; ...
+                 1, 5; ...
+                 5, 2; ...
+                 2, 6; ...
+                 6, 1];
+end

geometries/@spherical/plotWireframe.m

@@ -0,0 +1,43 @@
+function [obj, f] = plotWireframe(obj, ind, f)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
+    end
+
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Create plotting inputs
+    [X, Y, Z] = sphere(8);
+    % Scale
+    X = X * obj.radius;
+    Y = Y * obj.radius;
+    Z = Z * obj.radius;
+    % Shift
+    X = X + obj.center(1);
+    Y = Y + obj.center(2);
+    Z = Z + obj.center(3);
+
+    % Plot the boundaries of the geometry into 3D view
+    if isnan(ind)
+        o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
+    else
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 1);
+        hold(f.Children(1).Children(ind(1)), "off");
+    end
+
+    % Copy to other requested tiles
+    if numel(ind) > 1
+        for ii = 2:size(ind, 2)
+            o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
+        end
+    end
+
+    obj.lines = o;
+end

geometries/@spherical/random.m

@@ -0,0 +1,15 @@
+function r = random(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "spherical")};
+    end
+    arguments (Output)
+        r (1, 3) double
+    end
+    y = (rand - 0.5) * 2; % uniform draw on [-1, 1]
+    R = sqrt(1 - y^2);
+    lon = (rand - 0.5) * 2 * pi; % uniform draw on [-pi, pi]
+    s = [R * sin(lon), y, R * cos(lon)]; % random point on surface
+    r = s * rand^(1/3); % scaled to random normalized radius [0, 1]
+
+    r = obj.center + obj.radius * r;
+end

geometries/@spherical/spherical.m

@@ -0,0 +1,39 @@
+classdef spherical
+    % Rectangular prism geometry
+    properties (SetAccess = private, GetAccess = public)
+        % Spatial
+        center = NaN(1, 3);
+        radius = NaN;
+        diameter = NaN;
+
+        vertices = NaN(6, 3); % fake vertices
+        edges = NaN(8, 2); % fake edges
+
+        % Plotting
+        lines;
+    end
+    properties (SetAccess = public, GetAccess = public)
+        % Meta
+        tag = REGION_TYPE.INVALID;
+        label = "";
+        
+        % Sensing objective (for DOMAIN region type only)
+        objective;
+    end
+
+    methods (Access = public)
+        function obj = spherical()
+            arguments (Output)
+                obj (1, 1) spherical
+            end
+            obj.objective = sensingObjective;
+        end
+        [obj   ] = initialize(obj, center, radius, tag, label);
+        [r     ] = random(obj);
+        [c     ] = contains(obj, pos);
+        [d     ] = distance(obj, pos);
+        [g     ] = distanceGradient(obj, pos);
+        [c     ] = containsLine(obj, pos1, pos2);
+        [obj, f] = plotWireframe(obj, ind, f);
+    end
+end

geometries/REGION_TYPE.m

@@ -1,19 +1,10 @@
-classdef REGION_TYPE
-    properties
-        id
-        color
-    end
+classdef REGION_TYPE < uint8
     enumeration
-        INVALID (0, [255, 127, 255]); % default value
-        DOMAIN (1, [0, 0, 0]); % domain region
-        OBSTACLE (2, [255, 127, 127]); % obstacle region
-        COLLISION (3, [255, 255, 128]); % collision avoidance region
-        FOV (4, [255, 165, 0]); % field of view region
-    end
-    methods
-        function obj = REGION_TYPE(id, color)
-            obj.id = id;
-            obj.color = color./ 255;
-        end
+        INVALID   (0) % default value
+        DOMAIN    (1) % domain region
+        OBSTACLE  (2) % obstacle region
+        COLLISION (3) % collision avoidance region
+        FOV       (4) % field of view region
+        COMMS     (5) % comunications region
     end
 end

geometries/regionTypeColor.m

@@ -0,0 +1,24 @@
+function color = regionTypeColor(tag)
+% regionTypeColor  Return the RGB color (0-1 range) for a REGION_TYPE value.
+arguments (Input)
+    tag (1, 1) REGION_TYPE
+end
+arguments (Output)
+    color (1, 3) double
+end
+
+switch tag
+    case REGION_TYPE.DOMAIN
+        color = [0, 0, 0] / 255;
+    case REGION_TYPE.OBSTACLE
+        color = [255, 127, 127] / 255;
+    case REGION_TYPE.COLLISION
+        color = [255, 255, 128] / 255;
+    case REGION_TYPE.FOV
+        color = [255, 165, 0] / 255;
+    case REGION_TYPE.COMMS
+        color = [0, 255, 0] / 255;
+    otherwise % INVALID
+        color = [255, 127, 255] / 255;
+end
+end

guidanceModels/gradientAscent.m

@@ -1,26 +0,0 @@
-function nextPos = gradientAscent(sensedValues, sensedPositions, pos, rate)
-    arguments (Input)
-        sensedValues (:, 1) double;
-        sensedPositions (:, 3) double;
-        pos (1, 3) double;
-        rate (1, 1) double = 0.1;
-    end
-    arguments (Output)
-        nextPos(1, 3) double;
-    end
-
-    % As a default, maintain current position
-    if size(sensedValues, 1) == 0 && size(sensedPositions, 1) == 0
-        nextPos = pos;
-        return;
-    end
-    
-    % Select next position by maximum sensed value
-    nextPos = sensedPositions(sensedValues == max(sensedValues), :);
-    nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
-
-    % rate-limit motion
-    v = nextPos - pos;
-    nextPos = pos + (v / norm(v, 2)) * rate;
-
-end

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/5kqAGPAcSSlbCfxgpmA59U0UWcUp.xml

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

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/DtKmIMiY6orJIYug9tOtsq_Fzhcp.xml

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

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/gsyStyyOVyV_ZZ2qqV11kkPreoEp.xml

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

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/z4z4lM08j9WL4zP-VuDL0ahDO0Up.xml

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

resources/project/9I1NhbvmbTiuVshItfSCHZiDxxI/F1_77bY6s29xV3hf65cyjVDQctIp.xml

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

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml

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

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/QQkomTANNGDauE43PAdpZJlxaiQp.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="420d04e4-3880-4a45-8609-11cb30d87302" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="aababef5-9f49-40b0-be1a-63743f2447a1" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml

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