codegen fixes, bug fixes, gets running on testbed environment

@agent/agent.m

@@ -17,8 +17,8 @@ classdef agent
         fovGeometry;
 
         % Communication
-        commsGeometry = spherical;
+        commsGeometry;
-        lesserNeighbors = [];
+        lesserNeighbors = zeros(1, 0);
 
         % Performance
         performance = 0;
@@ -34,6 +34,17 @@ classdef agent
     end
 
     methods (Access = public)
+        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)

@agent/initialize.m

@@ -23,7 +23,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     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));

@agent/partition.m

@@ -8,28 +8,32 @@ function [partitioning] = partition(obj, agents, objective)
         partitioning (:, :) double;
     end
 
-    % Assess sensing performance of each agent at each sample point
+    nAgents = size(agents, 1);
-    % in the domain
+    gridM   = size(objective.X, 1);
-    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, "UniformOutput", false);
+    gridN   = size(objective.X, 2);
-    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    nPoints = gridM * gridN;
-    agentPerformances = cat(3, agentPerformances{:});
 
-    % Get highest performance value at each point
+    % Assess sensing performance of each agent at each sample point.
-    [~, idx] = max(agentPerformances, [], 3);
+    % agentPerf is (nPoints x nAgents+1): the extra column is the
-
+    % minimum threshold that must be exceeded for any assignment.
-    % Collect agent indices in the same way as performance
+    agentPerf = zeros(nPoints, nAgents + 1);
-    indices = 1:size(agents, 1);
+    for aa = 1:nAgents
-    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+        p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
-    if size(agentInds, 1) ~= size(agents, 1)
+            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
-        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
+        agentPerf(:, aa) = p(:);
     end
-    agentInds = num2cell(agentInds, 2:3);
+    agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;
-    agentInds = cellfun(@(x) squeeze(x), agentInds, "UniformOutput", false);
-    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
-    agentInds = cat(3, agentInds{:});
 
-    % Use highest performing agent's index to form partitions
+    % Find which agent has highest performance at each point.
-    [m, n, ~] = size(agentInds);
+    % If the threshold column wins (idx == nAgents+1) the point is unassigned (0).
-    [jj, kk] = ndgrid(1:m, 1:n);
+    [~, idx] = max(agentPerf, [], 2);
-    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+
+    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/run.m

@@ -13,7 +13,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
 
     % Collect objective function values across partition
     partitionMask = partitioning == index;
-    if ~unique(partitionMask)
+    if ~any(partitionMask(:))
         % This agent has no partition, maintain current state
         return;
     end
@@ -32,10 +32,10 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
         pos = obj.pos + delta * deltaApplicator(ii, 1:3);
         
         % Compute performance values on partition
-        if ii < 5
+        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.
+            % 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
@@ -64,17 +64,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     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
-    rateFactor = targetRate / norm(gradC);
+    gradNorm = norm(gradC);
 
-    % Compute unconstrained next position
+    % Compute unconstrained next position.
-    pNext = obj.pos + rateFactor * gradC;
+    % 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-10
+        pNext = obj.pos;
+    else
+        pNext = obj.pos + (targetRate / gradNorm) * gradC;
+    end
 
     % Move to next position
     obj.lastPos = obj.pos;

@miSim/constrainMotion.m

@@ -6,55 +6,60 @@ function [obj] = constrainMotion(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    if size(obj.agents, 1) < 2
+    nAgents = size(obj.agents, 1);
+
+    if nAgents < 2
         nAAPairs = 0;
     else
-        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
+        nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
     end
 
-    agents = [obj.agents{:}];
+    % Compute velocity matrix from unconstrained gradient-ascent step
-    v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
+    v = zeros(nAgents, 3);
-    if all(isnan(v), "all") || all(v == zeros(size(obj.agents, 1), 3), "all")
+    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 = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
+    nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
-    nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
+    nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
-    nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - size(obj.agents, 1);
+    nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
     total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
     kk = 1;
-    A = zeros(total, 3 * size(obj.agents, 1));
+    A = zeros(total, 3 * nAgents);
     b = zeros(total, 1);
 
     % Set up collision avoidance constraints
-    h = NaN(size(obj.agents, 1));
+    h = NaN(nAgents, nAgents);
-    h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
+    h(logical(eye(nAgents))) = 0; % self value is 0
-    for ii = 1:(size(obj.agents, 1) - 1)
+    for ii = 1:(nAgents - 1)
-        for jj = (ii + 1):size(obj.agents, 1)
+        for jj = (ii + 1):nAgents
-            h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
+            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 * (agents(ii).pos - agents(jj).pos);
+            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 * h(ii, jj)^obj.barrierExponent;
+            b(kk) = obj.barrierGain * max(0, h(ii, jj))^obj.barrierExponent;
             kk = kk + 1;
         end
     end
 
-    hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
+    hObs = NaN(nAgents, size(obj.obstacles, 1));
     % Set up obstacle avoidance constraints
-    for ii = 1:size(obj.agents, 1)
+    for ii = 1:nAgents
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
+            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
 
-            hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
+            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 * (agents(ii).pos - cPos);
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
-            b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
+            b(kk) = obj.barrierGain * max(0, hObs(ii, jj))^obj.barrierExponent;
 
             kk = kk + 1;
         end
@@ -63,58 +68,61 @@ function [obj] = constrainMotion(obj)
     % 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
-    for ii = 1:size(obj.agents, 1)
+    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 = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
+        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 * h_xMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
+        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 * h_xMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y minimum
-        h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
+        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 * h_yMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
+        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 * h_yMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z minimum
-        h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
+        h_zMin = (obj.agents{ii}.pos(3) - obj.domain.minCorner(3)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
-        b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
+        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 * h_zMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
     end
 
-    % Save off h function values (ignoring network constraints which may evolve in time)
+    if coder.target('MATLAB')
-    obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; 
+        % 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(size(obj.agents, 1));
+    hComms = NaN(nAgents, nAgents);
-    hComms(logical(eye(size(obj.agents, 1)))) = 0;
+    hComms(logical(eye(nAgents))) = 0;
-    for ii = 1:(size(obj.agents, 1) - 1)
+    for ii = 1:(nAgents - 1)
-        for jj = (ii + 1):size(obj.agents, 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(agents(ii).pos - agents(jj).pos)^2;
+                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 * (agents(ii).pos - agents(jj).pos);
+                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 * hComms(ii, jj)^obj.barrierExponent;
+                b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent;
 
                 kk = kk + 1;
             end
@@ -122,22 +130,28 @@ function [obj] = constrainMotion(obj)
     end
 
     % Solve QP program generated earlier
-    vhat = reshape(v', 3 * size(obj.agents, 1), 1);
+    vhat = reshape(v', 3 * nAgents, 1);
-    H = 2 * eye(3 * size(obj.agents, 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));
-    vNew = reshape(vNew, 3, size(obj.agents, 1))';
+    end
+    vNew = reshape(vNew, 3, nAgents)';
 
     if exitflag <= 0
+        if coder.target('MATLAB')
             warning("QP failed, continuing with unconstrained solution...")
+        end
         vNew = v;
     end
 

@miSim/initialize.m

@@ -20,14 +20,20 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     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;
@@ -37,14 +43,24 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Define domain
     obj.domain = domain;
 
-    % Add geometries representing obstacles within the domain
+    % Add geometries representing obstacles within the domain, pre-allocating
-    obj.obstacles = obstacles;
+    % 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
 
     % Add an additional obstacle spanning the domain's footprint to
     % represent the minimum allowable altitude
     if minAlt > 0
-        obj.obstacles{end + 1, 1} = rectangularPrism;
+        minAltObstacle = rectangularPrism;
-        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        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
@@ -56,12 +72,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     for ii = 1:size(obj.agents, 1)
         % Agent
         if isempty(char(obj.agents{ii}.label))
-            obj.agents{ii}.label = sprintf("Agent %d", ii);
+            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", ii);
+            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
         end
     end
 
@@ -76,15 +92,18 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+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.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
+    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);
@@ -95,3 +114,4 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Run validations
         obj.validate();
     end
+end

@miSim/lesserNeighbor.m

@@ -9,42 +9,48 @@ function obj = lesserNeighbor(obj)
     % initialize solution with self-connections only
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
-    for ii = 1: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
-        lesserNeighbors = [];
+        lnBuf = zeros(1, nAgents);
+        lnCount = 0;
         for jj = 1:(ii - 1)
             if obj.adjacency(ii, jj)
-                lesserNeighbors = [lesserNeighbors, jj];
+                lnCount = lnCount + 1;
+                lnBuf(lnCount) = jj;
             end
         end
-        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
+        obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
 
         % Early exit for isolated agents
-        if isempty(obj.agents{ii}.lesserNeighbors)
+        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 in each agent's subgraph
+        % Find connected components; store only the max global index per
-        % TODO: rewrite this using matlab "conncomp" function?
+        % component (the only value needed below) to avoid a cell array
-        visited = false(size(subgraphAdjacency, 1), 1);
+        visited = false(1, lnCount);
-        components = {};
+        maxInComponent = zeros(1, lnCount);
-        for jj = 1:size(subgraphAdjacency, 1)
+        numComponents = 0;
+        for jj = 1:lnCount
             if ~visited(jj)
                 reachable = bfs(subgraphAdjacency, jj);
                 visited(reachable) = true;
-                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
+                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:size(components, 2)
+        for jj = 1:numComponents
-            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
+            maxIdx = maxInComponent(jj);
-            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
+            constraintAdjacencyMatrix(ii, maxIdx) = true;
+            constraintAdjacencyMatrix(maxIdx, ii) = true;
         end
     end
     obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
@@ -53,24 +59,34 @@ end
 function cComp = bfs(subgraphAdjacency, startIdx)
     n = size(subgraphAdjacency, 1);
     visited = false(1, n);
-    queue = startIdx;
+
-    cComp = startIdx;
+    % 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 ~isempty(queue)
+    while qHead < qTail
-        current = queue(1);
+        current = queue(qHead);
-        queue(1) = [];
+        qHead = qHead + 1;
 
         % Find all neighbors of current node in the subgraph
         neighbors = find(subgraphAdjacency(current, :));
-        
+        for kk = 1:numel(neighbors)
-        for neighbor = neighbors
+            neighbor = neighbors(kk);
             if ~visited(neighbor)
                 visited(neighbor) = true;
-                cComp = [cComp, neighbor];
+                cCompBuf(cSize + 1) = neighbor;
-                queue = [queue, neighbor];
+                cSize = cSize + 1;
+                queue(qTail) = neighbor;
+                qTail = qTail + 1;
             end
         end
     end
-    cComp = sort(cComp);
+    cComp = sort(cCompBuf(1:cSize));
 end

@miSim/miSim.m

@@ -2,17 +2,17 @@ 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
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
-        domain = rectangularPrism;
+        domain;
-        objective = sensingObjective;
+        objective;
-        obstacles = cell(0, 1); % geometries that define obstacles within the domain
+        obstacles; % geometries that define obstacles within the domain
-        agents = cell(0, 1); % agents that move within the domain
+        agents; % agents that move within the domain
-        adjacency = NaN; % Adjacency matrix representing communications network graph
+        adjacency = false(0, 0); % Adjacency matrix representing communications network graph
-        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
+        constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections
-        partitioning = NaN;
+        partitioning = zeros(0, 0);
         perf; % sensor performance timeseries array
         performance = 0; % simulation performance timeseries vector
         barrierGain = NaN; % CBF gain parameter
@@ -54,6 +54,15 @@ classdef miSim
     end
 
     methods (Access = public)
+        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] = run(obj);
         [obj] = lesserNeighbor(obj);

@miSim/run.m

@@ -6,21 +6,24 @@ function [obj] = run(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
+    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);
         obj.timestepIndex = ii;
+        if coder.target('MATLAB')
             fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
-        % Before moving
             % Validate current simulation configuration
             obj.validate();
+        end
 
         % Update partitioning before moving (this one is strictly for
         % plotting purposes, the real partitioning is done by the agents)
@@ -39,7 +42,7 @@ function [obj] = run(obj)
         % CBF constraints solved by QP
         obj = constrainMotion(obj);
 
-        % After moving
+        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);
 
@@ -58,9 +61,12 @@ function [obj] = run(obj)
                 v.writeVideo(I);
             end
         end
+    end
 
+    if coder.target('MATLAB')
         if obj.makeVideo
             % Close video file
             v.close();
         end
     end
+end

@sensingObjective/initialize.m

@@ -30,8 +30,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
 
     % Evaluate function over grid points
-    obj.objectiveFunction = objectiveFunction;
+    obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X));
-    obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
     
     % Normalize
     obj.values = obj.values ./ max(obj.values, [], "all");

@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/sensingObjective.m

@@ -4,7 +4,6 @@ classdef sensingObjective
         label = "";
         groundPos = [NaN, NaN];
         discretizationStep = NaN;
-        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
         X = [];
         Y = [];
         values = [];
@@ -14,7 +13,8 @@ classdef sensingObjective
 
     methods (Access = public)
         [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
-        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
+        [obj] = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
+        [obj] = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end
 end

aerpaw/MESSAGE_TYPE.m

@@ -5,5 +5,8 @@ classdef MESSAGE_TYPE < uint8
         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

@@ -41,6 +41,9 @@ class MessageType(IntEnum):
     READY            = 3
     RTL              = 4
     LAND             = 5
+    GUIDANCE_TOGGLE  = 6
+    REQUEST_POSITION = 7
+    POSITION         = 8
 
 
 AERPAW_DIR = Path(__file__).parent.parent
@@ -177,6 +180,7 @@ class UAVRunner(BasicRunner):
             return
 
         log_task = None
+        nav_task = None
         try:
             # Takeoff to above AERPAW minimum altitude
             print("[UAV] Taking off...")
@@ -186,32 +190,64 @@ class UAVRunner(BasicRunner):
             # Start GPS logging in background
             log_task = asyncio.create_task(_gps_log_loop(drone))
 
-            # Command loop - handle TARGET, RTL, LAND, READY from controller
+            # 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.TARGET:
+                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)
-                    waypoint_num += 1
-                    print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x}, y={enu_y}, z={enu_z}")
 
                     # 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: non-blocking — cancel previous nav and start new
+                        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)
+                        nav_task = asyncio.create_task(drone.goto_coordinates(target))
+                        # No ACK/READY in guidance mode
+                    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(f"[UAV] Sent 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(f"[UAV] Sent READY")
+                        print("[UAV] Sent READY")
 
                 elif msg_type == MessageType.RTL:
                     await send_message_type(writer, MessageType.ACK)
@@ -246,6 +282,9 @@ class UAVRunner(BasicRunner):
             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)

aerpaw/compile.sh

@@ -8,6 +8,8 @@ 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" \
@@ -16,6 +18,7 @@ g++ -I/home/kdee/matlab/R2025a/extern/include \
     "$IMPL/controller_impl.cpp" \
     "$CODEGEN"/*.cpp \
     -o "$BUILD/controller_app" \
+    -fopenmp \
     -lpthread
 
 echo "Built: $BUILD/controller_app"

aerpaw/config/server.yaml

@@ -17,15 +17,7 @@
 #   Max distance from origin: ~22m (all waypoints within geofence)
 #
 targets:
-  # UAV 1: straight line along x-axis at y=5, alt=45m
+  # UAV 1 Initial Position — west side of guidance domain
-  - [0.0,    5.0,  45.0]
+  - [5.0,   10.0,  45.0]
-  - [4.0,    5.0,  45.0]
+  # UAV 2 Initial Position — east side of guidance domain
-  - [8.0,    5.0,  45.0]
+  - [15.0,  10.0,  30.0]
-  - [12.0,   5.0,  45.0]
-  - [16.0,   5.0,  45.0]
-  # UAV 2: triangular path diverging/converging, alt=30m
-  - [0.0,   -5.0,  30.0]
-  - [4.0,  -15.0,  30.0]
-  - [8.0,  -20.0,  30.0]
-  - [12.0, -15.0,  30.0]
-  - [16.0,  -5.0,  30.0]

aerpaw/controller.m

@@ -83,7 +83,69 @@ for w = 1:numWaypoints
     end
 end
 
-% Wait for user input before closing experiment
+% ---- Phase 2: miSim guidance loop ----------------------------------------
+% Guidance parameters (adjust here and recompile as needed)
+MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+GUIDANCE_RATE_MS   = int32(5000); % ms between iterations (0.2 Hz default)
+
+% Wait for user input to start guidance loop
+if coder.target('MATLAB')
+    input('Press Enter to start guidance loop: ', 's');
+else
+    coder.ceval('waitForUserInput');
+end
+
+% 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));
+end
+guidance_step(positions(1:numClients, :), true);
+
+% Main guidance loop
+for step = 1:MAX_GUIDANCE_STEPS
+    % Query 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));
+    end
+
+    % Run one guidance step: feed GPS positions in, get targets out
+    nextPositions = guidance_step(positions(1:numClients, :), false);
+
+    % Send target to each client (no ACK/READY expected in guidance mode)
+    for i = 1:numClients
+        target = nextPositions(i, :);
+        if ~coder.target('MATLAB')
+            coder.ceval('sendTarget', int32(i), coder.ref(target));
+        else
+            disp(['[guidance] target UAV ', num2str(i), ': ', num2str(target)]);
+        end
+    end
+
+    % Wait for the guidance rate interval before the next iteration
+    if ~coder.target('MATLAB')
+        coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS));
+    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
+% --------------------------------------------------------------------------
+
+% Wait for user input before closing experiment (RTL + LAND)
 if coder.target('MATLAB')
     input('Press Enter to close experiment (RTL + LAND): ', 's');
 else

aerpaw/guidance_step.m

@@ -0,0 +1,179 @@
+function nextPositions = guidance_step(currentPositions, isInit)
+% 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)         % first call: initialise state
+%   nextPos = guidance_step(gpsPos, false)     % subsequent calls: run one step
+%
+% Inputs:
+%   currentPositions  (MAX_CLIENTS × 3) double  ENU [east north up] metres
+%   isInit            (1,1)             logical  true on first call only
+%
+% Output:
+%   nextPositions     (MAX_CLIENTS × 3) double  guidance targets, ENU metres
+%
+% Codegen notes:
+%   - Persistent variable 'sim' holds the miSim object between calls.
+%   - Plotting/video are disabled (makePlots=false, makeVideo=false) for
+%     deployment. The coder.target('MATLAB') guards in the miSim/agent
+%     class files must be in place before codegen will succeed.
+%   - objectiveFunctionWrapper returns a function handle which is not
+%     directly codegen-compatible; the MATLAB path uses it normally. The
+%     compiled path requires an equivalent C impl (see TODO below).
+
+coder.extrinsic('disp', 'objectiveFunctionWrapper');
+
+% =========================================================================
+% Tunable guidance parameters — adjust here and recompile as needed.
+% =========================================================================
+MAX_CLIENTS = int32(4);      % must match MAX_CLIENTS in controller.m
+
+% Domain bounds in ENU metres [east, north, up]
+DOMAIN_MIN = [  0.0,   0.0,  25.0];
+DOMAIN_MAX = [ 20.0,  20.0,  55.0];
+MIN_ALT    = 25.0;           % hard altitude floor (m)
+
+% Sensing objective: bivariate Gaussian centred at [east, north]
+OBJECTIVE_GROUND_POS = [10.0, 10.0];
+DISCRETIZATION_STEP  = 0.1;  % objective grid step (m) — coarser = faster
+PROTECTED_RANGE      = 1.0;  % objective centre must be >this from domain edge
+
+% Agent safety geometry
+COLLISION_RADIUS = 3.0;      % spherical collision radius (m)
+COMMS_RANGE      = 60.0;     % communications range (m)
+
+% Gradient-ascent parameters
+INITIAL_STEP_SIZE = 1;     % step size at iteration 0 (decays to 0 at MAX_ITER)
+MAX_ITER          = 100;     % guidance steps (sets decay rate)
+
+% Sensor model (sigmoidSensor)
+ALPHA_DIST = 60.0;           % effective sensing distance (m) — set beyond max domain slant range (~53 m)
+BETA_DIST  = 0.2;            % distance sigmoid steepness — gentle, tilt drives the coverage gradient
+ALPHA_TILT = 10.0;           % max useful tilt angle (degrees)
+BETA_TILT  = 1.0;            % tilt sigmoid steepness
+
+% Safety filter — Control Barrier Function (CBF/QP)
+BARRIER_GAIN     = 100;
+BARRIER_EXPONENT = 3;
+
+% Simulation timestep (s) — should match GUIDANCE_RATE_MS / 1000 in controller.m
+TIMESTEP = 5.0;
+% =========================================================================
+
+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...');
+    end
+
+    % --- Build domain geometry ---
+    dom = rectangularPrism;
+    dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
+
+    % --- Build sensing objective ---
+    dom.objective = sensingObjective;
+    if coder.target('MATLAB')
+        % objectiveFunctionWrapper returns a function handle — MATLAB only.
+        objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3*eye(2));
+        dom.objective = dom.objective.initialize(objFcn, dom, ...
+            DISCRETIZATION_STEP, PROTECTED_RANGE);
+    else
+        % Evaluate bivariate Gaussian inline (codegen-compatible; no function handle).
+        % Must build the same grid that initializeWithValues uses internally.
+        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);
+        objValues = exp(-0.5 * (dx .* dx + dy .* dy));
+        dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
+            DISCRETIZATION_STEP, PROTECTED_RANGE);
+    end
+
+    % --- Build shared sensor model ---
+    sensor = sigmoidSensor;
+    sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT);
+
+    % --- Initialise agents from GPS positions ---
+    agentList = cell(numAgents, 1);
+    for ii = 1:numAgents
+        pos  = currentPositions(ii, :);
+        geom = spherical;
+        geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ...
+                               sprintf("UAV %d Collision", ii));
+        ag = agent;
+        ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ...
+                           INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+        agentList{ii} = ag;
+    end
+
+    % --- Initialise simulation (plots and video disabled) ---
+    sim = miSim;
+    sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
+                         MIN_ALT, TIMESTEP, MAX_ITER, cell(0, 1), false, false);
+
+    if coder.target('MATLAB')
+        disp('[guidance_step] Initialisation complete.');
+    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

@@ -9,6 +9,7 @@
 #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"
@@ -120,6 +121,9 @@ static const char* messageTypeName(uint8_t msgType) {
         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";
     }
 }
@@ -190,7 +194,17 @@ int waitForAllMessageType(int numClients, int expectedType) {
             if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
                 uint8_t msgType;
                 int len = recv(clientSockets[i], &msgType, 1, 0);
-                if (len != 1) return 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";
 
@@ -207,6 +221,66 @@ int waitForAllMessageType(int numClients, int expectedType) {
 
 // Wait for user to press Enter
 void waitForUserInput() {
-    std::cout << "Press Enter to close experiment (RTL + LAND)...\n";
+    std::cout << "Press Enter to continue...\n";
     std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
 }
+
+// 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

@@ -23,6 +23,12 @@ 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

aerpaw/results/plotGpsCsvs.m

@@ -6,9 +6,10 @@ c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors
 seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
 
 % Paths to logs
-gpsCsvs = ["GPS_DATA_0c8d904aa159_2026-02-16_13:26:33.csv"; ...
+gpsCsvs = fullfile ("sandbox", "test1", ...
-           "GPS_DATA_8e4f52dac04d_2026-02-16_13:26:33.csv"; ...
+                   ["GPS_DATA_0c8d904aa159_2026-02-24_21:33:25.csv"; ...
-          ];
+                    "GPS_DATA_8e4f52dac04d_2026-02-24_21:33:25.csv"; ...
+                   ]);
 
 G = cell(size(gpsCsvs));
 for ii = 1:size(gpsCsvs, 1)
@@ -16,7 +17,7 @@ for ii = 1:size(gpsCsvs, 1)
     G{ii} = readGpsCsv(gpsCsvs(ii));
 
     % Plot recorded trajectory
-    geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 3, "MarkerSize", 5);
+    geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);
 
 end
 hold(gf, "off");

geometries/@cone/plot.m

@@ -31,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
         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/plotWireframe.m

@@ -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/rectangularPrism.m

@@ -28,6 +28,12 @@ classdef rectangularPrism
     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);

geometries/@spherical/plotWireframe.m

@@ -25,10 +25,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", 1);
+        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
 

geometries/@spherical/spherical.m

@@ -2,12 +2,12 @@ classdef spherical
     % Rectangular prism geometry
     properties (SetAccess = private, GetAccess = public)
         % Spatial
-        center = NaN;
+        center = NaN(1, 3);
         radius = NaN;
         diameter = NaN;
 
-        vertices; % fake vertices
+        vertices = NaN(6, 3); % fake vertices
-        edges; % fake edges
+        edges = NaN(8, 2); % fake edges
 
         % Plotting
         lines;
@@ -22,6 +22,12 @@ classdef spherical
     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);

geometries/REGION_TYPE.m

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

resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMd.xml

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

resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMd.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMp.xml

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

resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4d.xml

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

resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4p.xml

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

test/test_miSim.m

@@ -207,6 +207,12 @@ classdef test_miSim < matlab.unittest.TestCase
                             break;
                         end
                     end
+
+                    % Make sure candidate clears domain floor
+                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
+                        violation = true;
+                    end
+                    
                     if violation
                         continue;
                     end
@@ -340,6 +346,12 @@ classdef test_miSim < matlab.unittest.TestCase
                             break;
                         end
                     end
+
+                    % Make sure candidate clears domain floor
+                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
+                        violation = true;
+                    end
+
                     if violation
                         continue;
                     end

util/validators/arguments/mustBeGeometry.m

@@ -1,10 +1,11 @@
 function mustBeGeometry(geometry)
-    validGeometries = ["rectangularPrism"; "spherical"];
+    if isa(geometry, 'cell')
-    if isa(geometry, "cell")
         for ii = 1:size(geometry, 1)
-            assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii);
+            assert(isa(geometry{ii}, 'rectangularPrism') || isa(geometry{ii}, 'spherical'), ...
+                   'Geometry in index %d is not a valid geometry class', ii);
         end
     else
-        assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class");
+        assert(isa(geometry, 'rectangularPrism') || isa(geometry, 'spherical'), ...
+               'Geometry is not a valid geometry class');
     end
 end

util/validators/arguments/mustBeSensor.m

@@ -1,10 +1,11 @@
 function mustBeSensor(sensorModel)
-    validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
+    if isa(sensorModel, 'cell')
-    if isa(sensorModel, "cell")
         for ii = 1:size(sensorModel, 1)
-            assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
+            assert(isa(sensorModel{ii}, 'sigmoidSensor'), ...
+                   'Sensor in index %d is not a valid sensor class', ii);
         end
     else
-        assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class");
+        assert(isa(sensorModel, 'sigmoidSensor'), ...
+               'Sensor is not a valid sensor class');
     end
 end