improved sigmoid sensor parameter plotting for thesis figure

Revert "AERPAW experiment updates to be brought into main"

.gitignore

@@ -4,6 +4,7 @@
 *.autosave
 *.slx.r*
 *.mdl.r*
+*.bak
 
 # Derived content-obscured files
 *.p

@agent/agent.m

@@ -19,7 +19,8 @@ classdef agent
         fovGeometry;
 
         % Communication
-        commsGeometry;
+        commsGeometry;          % fixed-radius comms model (spherical, radius = comRange)
+        txPower = 0.1;          % transmit power (W) for the SINR comms model (isotropic)
         lesserNeighbors = zeros(1, 0);
 
         % Performance
@@ -49,7 +50,7 @@ classdef agent
             obj.fovGeometry = cone;
             obj.commsGeometry = spherical;
         end
-        [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
+        [obj] = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry, txPower);
         [obj] = run(obj, domain, partitioning, t, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents);
         [partitioning, agents] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);

@agent/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry, txPower)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
@@ -10,6 +10,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
         initialMaxAngleStepSize (1, 1) double = 5.0;
         label (1, 1) string = "";
         plotCommsGeometry (1, 1) logical = false;
+        txPower (1, 1) double = 0.1;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "agent")};
@@ -25,6 +26,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     obj.plotCommsGeometry = plotCommsGeometry;
     obj.initialStepSize = initialStepSize;
     obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
+    obj.txPower = txPower;
     obj.stepDecayRate = obj.initialStepSize / maxIter;
     obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
 

@miSim/constrainMotion.m

@@ -141,6 +141,9 @@ function [obj] = constrainMotion(obj)
     % Add communication network constraints
     hComms = NaN(nAgents, nAgents);
     hComms(logical(eye(nAgents))) = 0;
+    if ~obj.useSinrComms
+        % Fixed-radius comms: keep each maintained pair within the smaller of
+        % the two comms radii via the barrier h = r_comms^2 - dist^2.
         for ii = 1:(nAgents - 1)
             for jj = (ii + 1):nAgents
                 if obj.constraintAdjacencyMatrix(ii, jj)
@@ -165,9 +168,69 @@ function [obj] = constrainMotion(obj)
                 end
             end
         end
+    else
+        % SINR comms: a link is feasible only if each agent receives the other
+        % above threshold, so every maintained pair is held with TWO barrier
+        % rows, h = SINR - gammaCbf, one per receiver direction. Each gradient
+        % couples the receiver, transmitter, and every interferer, so the QP
+        % rows are dense across all agent blocks.
+        % Maintain SINR a margin above the connectivity threshold so that
+        % discrete-time overshoot of the (convex) SINR does not transiently
+        % drop a maintained link below threshold. Mirrors the fixed-radius
+        % barrier's 0.9 padding: the CBF holds SINR >= gammaCbf > gammaLin,
+        % while connectivity (updateAdjacency) still uses gammaLin.
+        paddingDb = 3.0;
+        gammaCbf = 10^((obj.sinrThreshold + paddingDb) / 10);
+        lastPositions = zeros(nAgents, 3);
+        for ll = 1:nAgents
+            lastPositions(ll, :) = obj.agents{ll}.lastPos;
+        end
+        for ii = 1:(nAgents - 1)
+            for jj = (ii + 1):nAgents
+                if obj.constraintAdjacencyMatrix(ii, jj)
+                    % Direction 1: lower-index agent (ii) receives higher (jj).
+                    % Upper triangle of hComms stores this direction's barrier.
+                    [sinr1, grad1] = obj.sinrLink(lastPositions, ii, jj);
+                    hComms(ii, jj) = sinr1 - gammaCbf;
+                    % CBF row: A = -grad(h), b = h/dt (one-step forward invariance)
+                    for aa = 1:nAgents
+                        A(kk, (3 * aa - 2):(3 * aa)) = -grad1(aa, :);
+                    end
+                    if norm(A(kk, :)) < 1e-9
+                        b(kk) = 0;
+                    else
+                        b(kk) = hComms(ii, jj) / obj.timestep;
+                    end
+                    kk = kk + 1;
+
+                    % Direction 2: higher-index agent (jj) receives lower (ii).
+                    % Lower triangle of hComms stores this direction's barrier.
+                    [sinr2, grad2] = obj.sinrLink(lastPositions, jj, ii);
+                    hComms(jj, ii) = sinr2 - gammaCbf;
+                    for aa = 1:nAgents
+                        A(kk, (3 * aa - 2):(3 * aa)) = -grad2(aa, :);
+                    end
+                    if norm(A(kk, :)) < 1e-9
+                        b(kk) = 0;
+                    else
+                        b(kk) = hComms(jj, ii) / obj.timestep;
+                    end
+                    kk = kk + 1;
+                end
+            end
+        end
+    end
 
     if coder.target('MATLAB')
-	obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
+        if obj.useSinrComms
+            % Both directions are maintained: log the upper triangle
+            % (lower-index receiver) then the lower triangle (higher-index
+            % receiver), matching the doubled comms-barrier allocation.
+            commsLog = [hComms(triu(true(size(hComms)), 1)); hComms(tril(true(size(hComms)), -1))];
+        else
+            commsLog = hComms(triu(true(size(hComms)), 1));
+        end
+        obj.barriers(idx:(idx + numel(commsLog) - 1), obj.timestepIndex) = commsLog;
     end
 
     % Double-integrator: transform QP from velocity to acceleration space.

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
@@ -15,6 +15,13 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         dampingCoeff (1, 1) double = 2.0;
         useFixedTopology (1, 1) logical = false;
         optimizeSensorPointing (1, 1) logical = false;
+        useSinrComms (1, 1) logical = false;
+        sinrThreshold (1, 1) double = 0;
+        pathLossExponent (1, 1) double = 2.0;
+        ambientTemp (1, 1) double = 290.0;
+        centerFreq (1, 1) double = 2.4e9;
+        bandwidth (1, 1) double = 20e6;
+        useRoutingTopology (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, "miSim")};
@@ -72,18 +79,20 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
     % Set labels for agents and collision geometries in cases where they
-    % were not provieded at the time of their initialization
+    % were not provieded at the time of their initialization.
+    % Guarded by coder.target: label is a fixed-size "" in codegen, and
+    % sprintf returns variable-length — incompatible even in a dead branch.
+    % On the compiled path agents always receive explicit labels from the caller.
+    if coder.target('MATLAB')
         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
+    end
 
     % Set CBF parameters
     obj.barrierGain = barrierGain;
@@ -96,14 +105,60 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.useFixedTopology = useFixedTopology;
     obj.optimizeSensorPointing = optimizeSensorPointing;
 
-    % Compute adjacency matrix and network topology
+    % Set communications model and SINR parameters
+    obj.useSinrComms = useSinrComms;
+    obj.sinrThreshold = sinrThreshold;
+    obj.pathLossExponent = pathLossExponent;
+    obj.ambientTemp = ambientTemp;
+    obj.centerFreq = centerFreq;
+    obj.bandwidth = bandwidth;
+
+    % Set topology-selection algorithm; fail fast on invalid mode combinations
+    assert(~(useRoutingTopology && useFixedTopology), ...
+        "useRoutingTopology and useFixedTopology are mutually exclusive topology selectors");
+    assert(~useRoutingTopology || useSinrComms, ...
+        "useRoutingTopology requires the SINR comms model (useSinrComms)");
+    obj.useRoutingTopology = useRoutingTopology;
+
+    % Compute adjacency matrix and network topology. The Capacity-Aware
+    % Lesser Sink Neighbor algorithm replaces lesser-neighbor when selected
+    % (MATLAB sim only; the compiled path never sets the flag).
     obj = obj.updateAdjacency();
     if obj.useFixedTopology
         obj.constraintAdjacencyMatrix = obj.adjacency;
+    elseif coder.target('MATLAB') && obj.useRoutingTopology
+        obj = obj.lesserSinkNeighbor();
     else
         obj = obj.lesserNeighbor();
     end
 
+    % Guard against the SINR CBF deadlock band. Connectivity admits links
+    % with SINR anywhere above sinrThreshold, but constrainMotion maintains
+    % every selected link at sinrThreshold + 3 dB (paddingDb — keep in
+    % sync) in BOTH directions. A link selected inside that 3 dB band is
+    % born with a violated barrier; the QP can then go infeasible and hold
+    % positions, and since a frozen geometry re-selects the same link, the
+    % fleet deadlocks permanently. This catches the initial configuration;
+    % geometry evolution can still enter the band mid-run (fixing that
+    % requires pad-aware selection or a bounded-recovery barrier floor).
+    if coder.target('MATLAB') && obj.useSinrComms
+        gammaCbfLin = 10^((obj.sinrThreshold + 3.0) / 10);
+        posChk = zeros(size(obj.agents, 1), 3);
+        for kk = 1:size(obj.agents, 1)
+            posChk(kk, :) = obj.agents{kk}.pos;
+        end
+        for ii = 1:(size(obj.agents, 1) - 1)
+            for jj = (ii + 1):size(obj.agents, 1)
+                if obj.constraintAdjacencyMatrix(ii, jj)
+                    sinrBinding = min(obj.sinrLink(posChk, ii, jj), obj.sinrLink(posChk, jj, ii));
+                    assert(sinrBinding >= gammaCbfLin, ...
+                        "Initial topology link %d-%d has SINR %.1f dB, inside the CBF maintenance band (sinrThreshold %.1f dB + 3 dB padding): the QP starts with a violated comms barrier and can deadlock. Lower sinrThreshold or adjust the initial geometry.", ...
+                        ii, jj, 10 * log10(sinrBinding), obj.sinrThreshold);
+                end
+            end
+        end
+    end
+
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
 
@@ -124,7 +179,16 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     end
     nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
     nADPairs = size(obj.agents, 1) * 6; % agents x (4 walls + 1 floor + 1 ceiling)
-    nLNAPairs = sum(triu(obj.constraintAdjacencyMatrix, 1), "all");
+    % Comms barriers: size to the upper bound (full mesh) rather than the initial
+    % constraint-adjacency count. Connectivity is dynamic (lesser-neighbor and,
+    % especially, SINR) and can later maintain more pairs than at init; sizing to
+    % nAAPairs prevents the QP A/b matrices from overflowing mid-run. SINR mode
+    % enforces both directions per link, so it needs two barrier rows per pair.
+    if obj.useSinrComms
+        nLNAPairs = 2 * nAAPairs;
+    else
+        nLNAPairs = nAAPairs;
+    end
     obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
 
     if coder.target('MATLAB')

@miSim/initializeFromCsv.m

@@ -107,6 +107,53 @@ else
     USE_FIXED_TOPOLOGY = false;
 end
 
+% SINR communications model (optional columns — backward compatible with
+% older CSVs that predate the SINR model; defaults keep the fixed-radius path)
+if isfield(scenario, 'txPower')
+    TX_POWER_VEC = scenario.txPower;                       % 1×N (Watts)
+    assert(numel(TX_POWER_VEC) == numAgents, ...
+        "txPower has %d values but expected %d (one per UAV)", numel(TX_POWER_VEC), numAgents);
+else
+    TX_POWER_VEC = 0.1 * ones(1, numAgents);
+end
+if isfield(scenario, 'useSinrComms')
+    USE_SINR_COMMS = logical(scenario.useSinrComms);
+else
+    USE_SINR_COMMS = false;
+end
+if isfield(scenario, 'sinrThreshold')
+    SINR_THRESHOLD = scenario.sinrThreshold;               % dB
+else
+    SINR_THRESHOLD = 0;
+end
+if isfield(scenario, 'pathLossExponent')
+    PATH_LOSS_EXPONENT = scenario.pathLossExponent;
+else
+    PATH_LOSS_EXPONENT = 2.0;
+end
+if isfield(scenario, 'ambientTemp')
+    AMBIENT_TEMP = scenario.ambientTemp;                   % Kelvin
+else
+    AMBIENT_TEMP = 290.0;
+end
+if isfield(scenario, 'centerFreq')
+    CENTER_FREQ = scenario.centerFreq;                     % Hz
+else
+    CENTER_FREQ = 2.4e9;
+end
+if isfield(scenario, 'bandwidth')
+    BANDWIDTH = scenario.bandwidth;                        % Hz
+else
+    BANDWIDTH = 20e6;
+end
+
+% Routing topology selection (optional column; MATLAB sim only)
+if isfield(scenario, 'useRoutingTopology')
+    USE_ROUTING_TOPOLOGY = logical(scenario.useRoutingTopology);
+else
+    USE_ROUTING_TOPOLOGY = false;
+end
+
 % ---- Build domain --------------------------------------------------------
 dom = rectangularPrism;
 dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
@@ -134,6 +181,7 @@ for ii = 1:numAgents
     ag = agent;
     ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
                        INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+    ag.txPower = TX_POWER_VEC(ii); % SINR comms model transmit power (W)
     agentList{ii} = ag;
 end
 
@@ -153,6 +201,8 @@ end
 % ---- Initialise simulation (plots and video disabled) --------------------
 obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
                      MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
+                     USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY, false, ...
+                     USE_SINR_COMMS, SINR_THRESHOLD, PATH_LOSS_EXPONENT, AMBIENT_TEMP, CENTER_FREQ, BANDWIDTH, ...
+                     USE_ROUTING_TOPOLOGY);
 
 end

@miSim/initializeFromInits.m

@@ -47,6 +47,9 @@ for ii = 1:numAgents
     ag = agent;
     ag = ag.initialize(pos, geom, sensor, inits.comRange(ii), inits.maxIter, ...
                        inits.initialStepSize(ii), sprintf("UAV %d", ii));
+    if isfield(inits, 'txPower')
+        ag.txPower = inits.txPower(ii); % SINR comms model transmit power (W)
+    end
     agentList{ii} = ag;
 end
 
@@ -78,10 +81,47 @@ if isfield(inits, 'useFixedTopology')
 else
     useFixedTopology = false;
 end
+if isfield(inits, 'useSinrComms')
+    useSinrComms = logical(inits.useSinrComms);
+else
+    useSinrComms = false;
+end
+if isfield(inits, 'sinrThreshold')
+    sinrThreshold = inits.sinrThreshold;
+else
+    sinrThreshold = 0;
+end
+if isfield(inits, 'pathLossExponent')
+    pathLossExponent = inits.pathLossExponent;
+else
+    pathLossExponent = 2.0;
+end
+if isfield(inits, 'ambientTemp')
+    ambientTemp = inits.ambientTemp;
+else
+    ambientTemp = 290.0;
+end
+if isfield(inits, 'centerFreq')
+    centerFreq = inits.centerFreq;
+else
+    centerFreq = 2.4e9;
+end
+if isfield(inits, 'bandwidth')
+    bandwidth = inits.bandwidth;
+else
+    bandwidth = 20e6;
+end
+if isfield(inits, 'useRoutingTopology')
+    useRoutingTopology = logical(inits.useRoutingTopology);
+else
+    useRoutingTopology = false;
+end
 
 % ---- Initialize simulation (plots and video disabled) ------------------------
 obj = obj.initialize(dom, agentList, inits.barrierGain, inits.barrierExponent, ...
                      inits.minAlt, inits.timestep, inits.maxIter, obstacleList, ...
-                     false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology);
+                     false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology, false, ...
+                     useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, ...
+                     useRoutingTopology);
 
 end

@miSim/lesserSinkNeighbor.m

@@ -0,0 +1,372 @@
+function obj = lesserSinkNeighbor(obj)
+% LESSERSINKNEIGHBOR  Capacity-Aware Lesser Sink Neighbor algorithm (CALSN).
+%
+% Implements the algorithm specified in new/new_routing_algorithm.pdf as the
+% selectable alternative to lesserNeighbor. UAVs 1 and N are base stations;
+% every other UAV generates r = 1 unit of data per step, and all data must
+% reach a base station. The algorithm outputs both a sparse maintained
+% topology and a routing table.
+%
+% 1. Base-station potentials phi via distributed Bellman-Ford over the
+%    current communication neighborhoods (obj.adjacency), with link cost
+%    c_ij = 1/(C_ij + eps), where C_ij = bandwidth * log2(1 + SINR_ij) is
+%    the Shannon capacity of direction i -> j.
+% 2. Lesser sink neighbors L_i replace LNA's "j < i" rule:
+%        L_i = { j in N_i : phi_j < phi_i, or phi_j == phi_i and j < i }.
+%    The equal-potential lexicographic tie-break is a documented deviation
+%    from the spec's strict inequality: at an exact potential watershed
+%    (e.g. a symmetric chain) the strict rule would split A_control into
+%    one tree per base station; the tie-break keeps the airborne graph
+%    connected, which the mission requires.
+% 3. LNA-style sparsity: one parent per connected component of the
+%    L_i-induced subgraph (only links with positive capacity in at least
+%    one direction count as edges), chosen by
+%        p = argmin_{j in C} ( phi_j + 1/(C_ij + eps) ).
+%    A_control(i,p) = A_control(p,i) = 1 (diagonal = I).
+%    BASIN EXTENSION (documented deviation): with two potential minima
+%    (the two bases), one-parent-per-component alone yields a spanning
+%    forest with up to two disjoint base-rooted trees whenever a UAV's
+%    lesser sink neighbors collapse into a single component (dense usable
+%    graphs). To restore LNA's global-connectivity property, each UAV
+%    selects one representative per component AND per base-station BASIN
+%    (the base a neighbor's potential descent terminates at). Interior
+%    UAVs are unaffected (their whole neighborhood shares one basin);
+%    watershed UAVs whose neighborhoods span both basins gain a second
+%    parent, which is exactly the bridge between the two trees. A final
+%    stitch guard covers degenerate geometries: if A_control still has
+%    multiple components, the lowest-cost feasible inter-component link
+%    is added (maintained for connectivity only; carries no flow).
+% 4. Routing, children first (decreasing phi; equal phi processed in
+%    decreasing index order so tie-break children precede their parents):
+%    aggregate demand D_i = r + inflows, split capacity-proportionally
+%    over parents; saturated links are capped at capacity and the residual
+%    redistributed over unsaturated parents; any demand beyond the total
+%    parent capacity is reported as unrouted and the UAV marked infeasible.
+%
+% Outputs (set on obj):
+%   constraintAdjacencyMatrix  A_control: undirected maintained topology
+%   routingFlows               F: flow on each directed link (units of r)
+%   routingFractions           R = F/D: share of the sender's demand, 0..1
+%   routingAdjacencyMatrix     F > 0 (directed, child -> parent)
+%   routingPotentials          phi (base-station potential per UAV)
+%   routingUnrouted            unrouted demand per UAV (zeros when feasible)
+
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+
+    nAgents = size(obj.agents, 1);
+    r = 1.0;         % data generated per non-base UAV per step (abstract unit)
+    EPS_CAP = 1e-12; % epsilon in c = 1/(C + eps); avoids division by zero
+    TOL = 1e-12;     % residual-demand tolerance for the saturation loop
+
+    isBase = false(nAgents, 1);
+    isBase(1) = true;
+    isBase(nAgents) = true; % base stations are UAVs 1 and N
+
+    positions = zeros(nAgents, 3);
+    for kk = 1:nAgents
+        positions(kk, :) = obj.agents{kk}.pos;
+    end
+
+    % Directed Shannon link capacities over the current communication
+    % neighborhoods: C(i, j) = capacity of sending from i to j (receiver j)
+    C = zeros(nAgents);
+    for ii = 1:nAgents
+        for jj = 1:nAgents
+            if jj ~= ii && obj.adjacency(ii, jj)
+                C(ii, jj) = obj.bandwidth * log2(1 + obj.sinrLink(positions, jj, ii));
+            end
+        end
+    end
+
+    % ---- Base-station potentials (Bellman-Ford, at most N-1 sweeps) -----
+    phi = inf(nAgents, 1);
+    phi(isBase) = 0;
+    for sweep = 1:max(nAgents - 1, 1)
+        changed = false;
+        for ii = 1:nAgents
+            if isBase(ii)
+                continue;
+            end
+            newPhi = inf;
+            for jj = 1:nAgents
+                if jj == ii || ~obj.adjacency(ii, jj)
+                    continue;
+                end
+                newPhi = min(newPhi, phi(jj) + 1 / (C(ii, jj) + EPS_CAP));
+            end
+            if newPhi ~= phi(ii)
+                phi(ii) = newPhi;
+                changed = true;
+            end
+        end
+        if ~changed
+            break;
+        end
+    end
+
+    % ---- Base-station basins (for the basin extension) -------------------
+    % Each finite-potential UAV descends its argmin neighbor chain; phi is
+    % strictly decreasing along it, so the chain terminates at a base. The
+    % basin label records which base that is.
+    descent = zeros(nAgents, 1);
+    for ii = 1:nAgents
+        if isBase(ii) || isinf(phi(ii))
+            continue;
+        end
+        bestMetric = inf;
+        for jj = 1:nAgents
+            if jj == ii || ~obj.adjacency(ii, jj)
+                continue;
+            end
+            metric = phi(jj) + 1 / (C(ii, jj) + EPS_CAP);
+            if metric < bestMetric
+                bestMetric = metric;
+                descent(ii) = jj;
+            end
+        end
+    end
+    basin = zeros(nAgents, 1);
+    basin(1) = 1;
+    basin(nAgents) = nAgents;
+    for ii = 1:nAgents
+        if isBase(ii) || isinf(phi(ii))
+            continue;
+        end
+        kk = ii;
+        while ~isBase(kk)
+            kk = descent(kk);
+        end
+        basin(ii) = kk;
+    end
+
+    % ---- Parent selection (one per lesser-sink component and basin) ------
+    Acontrol = logical(eye(nAgents));
+    isParent = false(nAgents); % isParent(i, p): p is a selected parent of i
+    for ii = 1:nAgents
+        if isBase(ii) || isinf(phi(ii))
+            % Base stations select no parents; an infinite potential means
+            % no current local route toward a base station
+            continue;
+        end
+
+        % Lesser sink neighbors (with the equal-potential tie-break)
+        lsnBuf = zeros(1, nAgents);
+        lsnCount = 0;
+        for jj = 1:nAgents
+            if jj == ii || ~obj.adjacency(ii, jj)
+                continue;
+            end
+            if phi(jj) < phi(ii) || (phi(jj) == phi(ii) && jj < ii)
+                lsnCount = lsnCount + 1;
+                lsnBuf(lsnCount) = jj;
+            end
+        end
+        if lsnCount == 0
+            continue;
+        end
+        lsn = lsnBuf(1:lsnCount);
+
+        % Subgraph induced by the lesser sink neighbors in the usable
+        % communication graph: only links with positive capacity in at
+        % least one direction count as edges (a zero-capacity link cannot
+        % carry data, so it must not merge two components)
+        subgraphAdjacency = false(lsnCount);
+        for aa = 1:lsnCount
+            for bb = (aa + 1):lsnCount
+                ja = lsn(aa);
+                jb = lsn(bb);
+                if obj.adjacency(ja, jb) && (C(ja, jb) > 0 || C(jb, ja) > 0)
+                    subgraphAdjacency(aa, bb) = true;
+                    subgraphAdjacency(bb, aa) = true;
+                end
+            end
+        end
+
+        % One parent per connected component and per base-station basin
+        % present in it (basin extension), selected by capacity and
+        % potential: p = argmin( phi_j + 1/(C_ij + eps) )
+        visited = false(1, lsnCount);
+        for aa = 1:lsnCount
+            if visited(aa)
+                continue;
+            end
+            component = bfs(subgraphAdjacency, aa);
+            visited(component) = true;
+
+            for bb = [1, nAgents] % the two possible basin labels
+                bestMetric = inf;
+                bestParent = 0;
+                for cc = 1:numel(component)
+                    jj = lsn(component(cc));
+                    if basin(jj) ~= bb
+                        continue;
+                    end
+                    metric = phi(jj) + 1 / (C(ii, jj) + EPS_CAP);
+                    if metric < bestMetric
+                        bestMetric = metric;
+                        bestParent = jj;
+                    end
+                end
+                if bestParent > 0
+                    isParent(ii, bestParent) = true;
+                    Acontrol(ii, bestParent) = true;
+                    Acontrol(bestParent, ii) = true;
+                end
+            end
+        end
+    end
+
+    % ---- Connectivity stitch guard ---------------------------------------
+    % The basin extension bridges the two base-rooted trees through
+    % watershed UAVs whenever each base borders a routing basin. Degenerate
+    % geometries (e.g. a base whose only feasible neighbor is the other
+    % base) can still leave A_control split; add the lowest-cost feasible
+    % link between components until one remains. Stitched links are
+    % maintained for connectivity only and carry no routed flow. If the
+    % feasibility graph itself is disconnected no stitch exists; validate()
+    % reports that case.
+    while true
+        comp = zeros(1, nAgents);
+        nComp = 0;
+        for ii = 1:nAgents
+            if comp(ii) == 0
+                nComp = nComp + 1;
+                comp(bfs(Acontrol, ii)) = nComp;
+            end
+        end
+        if nComp <= 1
+            break;
+        end
+        bestMetric = inf;
+        bestU = 0;
+        bestV = 0;
+        for uu = 1:nAgents
+            for vv = (uu + 1):nAgents
+                if comp(uu) ~= comp(vv) && obj.adjacency(uu, vv)
+                    % Bidirectional link cost (finite even for infinite
+                    % potentials, so isolated islands remain stitchable)
+                    metric = 1 / (C(uu, vv) + EPS_CAP) + 1 / (C(vv, uu) + EPS_CAP);
+                    if metric < bestMetric
+                        bestMetric = metric;
+                        bestU = uu;
+                        bestV = vv;
+                    end
+                end
+            end
+        end
+        if bestU == 0
+            break; % feasibility graph disconnected: nothing to stitch with
+        end
+        Acontrol(bestU, bestV) = true;
+        Acontrol(bestV, bestU) = true;
+    end
+
+    % ---- Routing (children first: decreasing potential order) -----------
+    % Equal potentials are processed in decreasing index order: with the
+    % lexicographic tie-break, flow within an equal-potential group goes
+    % from higher to lower index, so higher indices must aggregate first.
+    F = zeros(nAgents);
+    R = zeros(nAgents);
+    unrouted = zeros(nAgents, 1);
+
+    nonBase = find(~isBase);
+    [~, ord] = sortrows([phi(nonBase), nonBase], [-1, -2]);
+    order = nonBase(ord);
+
+    for oo = 1:numel(order)
+        ii = order(oo);
+
+        % Aggregated outgoing demand: own data plus everything received
+        D = r + sum(F(:, ii));
+        parents = find(isParent(ii, :));
+        if D == 0 || isempty(parents)
+            continue;
+        end
+
+        % Capacity-proportional split, saturating full links first and
+        % redistributing the remainder across parents with remaining
+        % capacity. Each pass either routes all residual demand or
+        % saturates at least one parent, so the loop terminates.
+        capP = C(ii, parents);
+        flows = zeros(1, numel(parents));
+        active = true(1, numel(parents));
+        residual = D;
+        while residual > TOL && any(active)
+            totalCap = sum(capP(active));
+            if totalCap <= 0
+                break;
+            end
+            alloc = zeros(1, numel(parents));
+            alloc(active) = residual .* capP(active) ./ totalCap;
+            flows = flows + alloc;
+            saturated = active & (flows > capP);
+            if any(saturated)
+                residual = sum(flows(saturated) - capP(saturated));
+                flows(saturated) = capP(saturated);
+                active(saturated) = false;
+            else
+                residual = 0;
+            end
+        end
+        if residual > TOL
+            % Demand exceeds the total parent capacity: route as much as
+            % possible and report the remainder as unrouted (infeasible)
+            unrouted(ii) = residual;
+            warning("miSim:routingInfeasible", ...
+                "UAV %d demand %.3g exceeds total parent capacity; %.3g units unrouted", ...
+                ii, D, residual);
+        end
+
+        F(ii, parents) = flows;
+        R(ii, parents) = flows ./ D;
+    end
+
+    % ---- Outputs ---------------------------------------------------------
+    obj.constraintAdjacencyMatrix = Acontrol;
+    obj.routingFlows = F;
+    obj.routingFractions = R;
+    obj.routingAdjacencyMatrix = F > 0;
+    obj.routingPotentials = phi;
+    obj.routingUnrouted = unrouted;
+end
+
+function cComp = bfs(subgraphAdjacency, startIdx)
+    % Breadth-first search returning the connected component of startIdx.
+    % Pre-allocated queue and component buffer with head/tail pointers to
+    % avoid element deletion and dynamic array growth (same pattern as the
+    % local bfs in lesserNeighbor.m).
+    n = size(subgraphAdjacency, 1);
+    visited = false(1, n);
+
+    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;
+
+        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

@@ -21,6 +21,27 @@ classdef miSim
         dampingCoeff = 2.0; % velocity-proportional damping for double-integrator mode
         useFixedTopology = false; % false = lesser neighbor (dynamic), true = fixed initial topology
         optimizeSensorPointing = false; % false = fixed sensor tilt/azimuth, true = optimize tilt/azimuth via gradient ascent
+        % Communications model selection and SINR-model parameters.
+        % useSinrComms = false -> fixed-radius comms (comRange / commsGeometry).
+        % useSinrComms = true  -> SINR/path-loss comms (txPower + params below).
+        useSinrComms = false;
+        sinrThreshold = 0;        % SINR threshold for connectivity & CBF maintenance (dB)
+        pathLossExponent = 2.0;   % path-loss exponent n in S = P / (K * d^n)
+        ambientTemp = 290.0;      % ambient temperature (K) for thermal noise N = k_B*T*B
+        centerFreq = 2.4e9;       % carrier frequency (Hz); sets path-loss offset K = (4*pi*f_c/c)^2
+        bandwidth = 20e6;         % channel bandwidth (Hz) for thermal noise N = k_B*T*B
+        % Topology-selection algorithm (requires useSinrComms; MATLAB sim only).
+        % false -> lesser-neighbor; true -> Capacity-Aware Lesser Sink Neighbor
+        % (CALSN, see new/new_routing_algorithm.pdf): base-station potentials
+        % replace the index-based "lesser" rule, and the maintained topology
+        % comes with a routing table (flows F, fractions R) that sends every
+        % UAV's data toward base stations 1 and N.
+        useRoutingTopology = false;
+        routingAdjacencyMatrix = false(0, 0); % directed flow links (i, j) = child i sends to parent j (F > 0)
+        routingFlows = zeros(0, 0);      % F: flow on each directed link (units of r = 1 per UAV per step)
+        routingFractions = zeros(0, 0);  % R = F/D: share of the sender's aggregated demand per link (0..1)
+        routingPotentials = zeros(0, 1); % phi: Bellman-Ford base-station potential per UAV
+        routingUnrouted = zeros(0, 1);   % unrouted demand per UAV (zeros when routing is feasible)
         artifactName = "";
         f; % main plotting tiled layout figure
         fPerf; % performance plot figure
@@ -70,13 +91,15 @@ classdef miSim
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end
-        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology);
+        [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);
         [obj] = initializeFromCsv(obj, csvPath);
         [obj] = initializeFromInits(obj, initsPath);
         [obj] = plotFromSimHist(obj, initsPath, histPath);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
+        [obj] = lesserSinkNeighbor(obj);
         [obj] = constrainMotion(obj);
+        [sinr, grad] = sinrLink(obj, positions, rxIdx, txIdx);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
         [obj] = plot(obj);

@miSim/plotGraph.m

@@ -6,23 +6,82 @@ function obj = plotGraph(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Form graph from adjacency matrix
+    % The maintained links are always drawn as a dotted undirected graph.
+    % In routing (CALSN) mode the flow-carrying links are overlaid as SOLID
+    % DIRECTED edges (arrow = child -> parent, toward a base station) on
+    % the same node layout.
     G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
 
-    % Plot graph object
+    % Resolve the target axes for the network graph tile
     if isnan(obj.networkGraphIndex)
-        hold(obj.f.CurrentAxes, "on");
+        ax = obj.f.CurrentAxes;
-        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");
+        ax = obj.f.Children(1).Children(obj.networkGraphIndex(1));
-        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
+    end
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
+
-        if size(obj.networkGraphIndex, 2) > 1
+    % Plot graph object(s)
-            for ii = 2:size(ind, 2)
+    hold(ax, "on");
-                o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
+    o = plot(ax, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
+    if obj.useRoutingTopology
+        % Flow overlay: reuse the base layout's node coordinates and hide
+        % the overlay's own nodes/labels so only its edges show. Sizeable
+        % arrowheads so the flow direction reads at tile size.
+        D = digraph(obj.routingAdjacencyMatrix);
+        o = [o; plot(ax, D, "XData", o(1).XData, "YData", o(1).YData, ...
+                     "LineStyle", "-", "EdgeColor", "g", "LineWidth", 2, ...
+                     "ArrowSize", 12, "Marker", "none", "NodeLabel", {})];
+    end
+    hold(ax, "off");
+    nBase = size(o, 1);
+    if ~isnan(obj.networkGraphIndex) && size(obj.networkGraphIndex, 2) > 1
+        for ii = 2:size(obj.networkGraphIndex, 2)
+            o = [o; copyobj(o(1:nBase), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
         end
     end
+
+    % In SINR comms mode, label each edge with the connection's SINR and report
+    % the connectivity threshold in the tile title. The fixed-radius model is
+    % left unchanged (no edge labels, title stays as set in firstPlotSetup).
+    if obj.useSinrComms
+        endNodes = G.Edges.EndNodes;
+        nAgents = size(obj.agents, 1);
+        positions = zeros(nAgents, 3);
+        for kk = 1:nAgents
+            positions(kk, :) = obj.agents{kk}.pos;
         end
+        % Labels are bare numbers (no units) so they stay compact; GraphPlot
+        % renders them horizontally, left-to-right. Dotted (maintained) edges
+        % are labelled with the binding (worse) directional SINR — a link is
+        % feasible only if BOTH directions clear the threshold. Links that
+        % also carry flow leave the label to the solid directed edge, which
+        % shows the flow value (units of r) sent child -> parent.
+        edgeLabels = strings(size(endNodes, 1), 1);
+        for ee = 1:size(endNodes, 1)
+            lo = endNodes(ee, 1);
+            hi = endNodes(ee, 2);
+            if obj.useRoutingTopology && (obj.routingAdjacencyMatrix(lo, hi) || obj.routingAdjacencyMatrix(hi, lo))
+                edgeLabels(ee) = ""; % solid overlay carries this link's label
+            else
+                sinrLoRx = obj.sinrLink(positions, lo, hi); % receiver lo (lower index)
+                sinrHiRx = obj.sinrLink(positions, hi, lo); % receiver hi (higher index)
+                edgeLabels(ee) = sprintf("%.1f", 10 * log10(min(sinrLoRx, sinrHiRx)));
+            end
+        end
+        o(1).EdgeLabel = edgeLabels;
+        if obj.useRoutingTopology
+            endNodesD = D.Edges.EndNodes;   % each row [child parent]
+            flowLabels = strings(size(endNodesD, 1), 1);
+            for ee = 1:size(endNodesD, 1)
+                flowLabels(ee) = sprintf("%.2f", obj.routingFlows(endNodesD(ee, 1), endNodesD(ee, 2)));
+            end
+            o(2).EdgeLabel = flowLabels;
+        end
+        % Threshold goes in the existing single-line title (not a subtitle) so
+        % the small tile is not compressed further, and the hover toolbar is
+        % disabled so the R2026 "..." menu stops covering the readout.
+        title(ax, sprintf("Network Graph (\\geq %.1f dB)", obj.sinrThreshold));
+        ax.Toolbar.Visible = "off";
+    end
+
     obj.graphPlot = o;
 end

@miSim/run.m

@@ -42,10 +42,15 @@ function [obj] = run(obj)
         % plotting purposes, the real partitioning is done by the agents)
         [obj.partitioning, obj.agents] = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
-        % Determine desired communications links
+        % Determine desired communications links: the Capacity-Aware Lesser
+        % Sink Neighbor algorithm replaces lesser-neighbor when selected
         if ~obj.useFixedTopology
+            if coder.target('MATLAB') && obj.useRoutingTopology
+                obj = obj.lesserSinkNeighbor();
+            else
                 obj = obj.lesserNeighbor();
             end
+        end
 
         % Log constraint adjacency for this timestep
         if coder.target('MATLAB')

@miSim/sinrLink.m

@@ -0,0 +1,101 @@
+function [sinr, grad] = sinrLink(obj, positions, rxIdx, txIdx)
+% SINRLINK  Linear SINR of the link txIdx -> rxIdx and its position gradient.
+%
+% Computes the signal-to-interference-plus-noise ratio seen at receiver rxIdx
+% for the signal transmitted by txIdx, treating every other agent as an
+% interferer at that receiver. Isotropic transmit/receive (no antenna gains).
+%
+% Returns the linear SINR and (optionally) its gradient with respect to every
+% agent position, grad(a, :) = d(SINR)/d(p_a). The value is used by
+% updateAdjacency (connectivity) and both the value and gradient by
+% constrainMotion (the SINR CBF barrier).
+%
+% Model (linear power domain), receiver r = rxIdx, transmitter t = txIdx:
+%   S    = P_t / (K * d_tr^n)                 % received signal at r from t (W)
+%   I    = sum_{k != t,r} P_k / (K * d_kr^n)  % interference at r from others (W)
+%   N    = k_B * T * B                        % thermal noise (W)
+%   SINR = S / (N + I)
+% with K = (4*pi*f_c/c)^2 and physical constants baked in below. The only
+% tunables are per-agent txPower (P), pathLossExponent (n), ambientTemp (T),
+% all read from obj; the SINR threshold is applied by the callers.
+
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+        positions (:, 3) double;
+        rxIdx (1, 1) double;
+        txIdx (1, 1) double;
+    end
+    arguments (Output)
+        sinr (1, 1) double;
+        grad (:, 3) double;
+    end
+
+    % Universal physical constants
+    k_B = 1.380649e-23;          % Boltzmann constant (J/K)
+    c   = 3e8;                   % speed of light (m/s)
+
+    % System parameters
+    f_c = obj.centerFreq;        % carrier frequency (Hz)
+    B   = obj.bandwidth;         % channel bandwidth (Hz)
+    K   = (4 * pi * f_c / c)^2;  % free-space path-loss reference constant
+
+    n  = obj.pathLossExponent;
+    T  = obj.ambientTemp;
+    d0 = 1.0;                    % near-field distance floor (m)
+
+    nAgents = size(positions, 1);
+    grad = zeros(nAgents, 3);
+
+    r  = rxIdx;
+    t  = txIdx;
+    pr = positions(r, :);
+
+    % Thermal noise (always > 0, so the denominator never vanishes)
+    N = k_B * T * B;
+
+    % Signal term S (source = transmitter t)
+    P_t    = obj.agents{t}.txPower;
+    dvec_t = positions(t, :) - pr;
+    d_t    = max(norm(dvec_t), d0);
+    S      = P_t / (K * d_t^n);
+
+    % Interference I = sum of received powers from every other agent at r
+    I = 0.0;
+    for k = 1:nAgents
+        if k == r || k == t
+            continue;
+        end
+        d_k = max(norm(positions(k, :) - pr), d0);
+        I   = I + obj.agents{k}.txPower / (K * d_k^n);
+    end
+
+    D    = N + I;       % denominator: noise + interference
+    sinr = S / D;
+
+    % ---- Gradient (skip unless requested) -------------------------------
+    % For a received term P_x/(K*d_xr^n):
+    %   d(term)/dp_x = -n*term/d_xr^2 * (p_x - p_r),  d(term)/dp_r = -d(term)/dp_x
+    if nargout > 1
+        % Transmitter affects S only: dSINR/dp_t = (dS/dp_t)/D
+        dS_dpt = (-n * S / (d_t^2)) * dvec_t;
+        grad(t, :) = dS_dpt / D;
+
+        % Interferers affect I only; accumulate the receiver's I-contribution
+        dI_dpr = zeros(1, 3);
+        for k = 1:nAgents
+            if k == r || k == t
+                continue;
+            end
+            dvec_k    = positions(k, :) - pr;
+            d_k       = max(norm(dvec_k), d0);
+            term_k    = obj.agents{k}.txPower / (K * d_k^n);
+            dterm_dpk = (-n * term_k / (d_k^2)) * dvec_k;
+            grad(k, :) = (-S / (D^2)) * dterm_dpk;      % dSINR/dp_k
+            dI_dpr     = dI_dpr - dterm_dpk;            % d(term_k)/dp_r = -d(term_k)/dp_k
+        end
+
+        % Receiver affects both S and I
+        dS_dpr = -dS_dpt;
+        grad(r, :) = dS_dpr / D - (S / (D^2)) * dI_dpr;
+    end
+end

@miSim/updateAdjacency.m

@@ -6,19 +6,44 @@ function obj = updateAdjacency(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Initialize assuming only self-connections
+    nAgents = size(obj.agents, 1);
-    A = true(size(obj.agents, 1));
 
-    % Check lower triangle off-diagonal connections
+    % Initialize assuming all agents are connected
-    for ii = 2:size(A, 1)
+    A = true(nAgents);
+
+    if ~obj.useSinrComms
+        % Fixed-radius model: pair connected iff separation is within the
+        % smaller of the two comms radii.
+        for ii = 2:nAgents
             for jj = 1:(ii - 1)
-            % 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
+    else
+        % SINR model: a link between a pair is feasible only if BOTH directional
+        % SINRs clear the threshold, i.e. each agent receives the other's signal
+        % above gammaLin (with interference from all other agents at the
+        % receiver). Every ordered pair (permutation) is evaluated; a single
+        % failing direction eliminates the undirected link.
+        gammaLin = 10^(obj.sinrThreshold / 10); % dB threshold -> linear
+        positions = zeros(nAgents, 3);
+        for kk = 1:nAgents
+            positions(kk, :) = obj.agents{kk}.pos;
+        end
+        for ii = 2:nAgents
+            for jj = 1:(ii - 1)
+                sinrLoRx = obj.sinrLink(positions, jj, ii); % receiver jj (lower), transmitter ii
+                sinrHiRx = obj.sinrLink(positions, ii, jj); % receiver ii (higher), transmitter jj
+                if sinrLoRx < gammaLin || sinrHiRx < gammaLin
+                    A(ii, jj) = false; % at least one direction below threshold
+                    continue;
+                end
+            end
+        end
+    end
 
     obj.adjacency = A & A';
 end

@miSim/validate.m

@@ -5,11 +5,26 @@ function validate(obj)
     arguments (Output)
     end
 
+    %% Mode Validators
+    if obj.useRoutingTopology && ~obj.useSinrComms
+        error("Routing topology requires SINR comms mode (useSinrComms)");
+    end
+
+    if obj.useRoutingTopology && obj.useFixedTopology
+        error("useRoutingTopology and useFixedTopology are mutually exclusive topology selectors");
+    end
+
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
         error("Network is not connected");
     end
 
+    % In routing (CALSN) mode the MAINTAINED topology itself must form a
+    % single connected component (basin extension + stitch guard invariant)
+    if obj.useRoutingTopology && max(conncomp(graph(obj.constraintAdjacencyMatrix))) ~= 1
+        error("Maintained routing topology (A_control) is not connected");
+    end
+
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
         error("Eliminated network connections that were necessary");
     end

@miSim/writeInits.m

@@ -47,6 +47,7 @@ function writeInits(obj)
     tilt = cellfun(@(x) x.sensorModel.tilt, obj.agents);
     azimuth = cellfun(@(x) x.sensorModel.azimuth, obj.agents);
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
+    txPower = cellfun(@(x) x.txPower, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
     obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, userObstacles, 'UniformOutput', false));
@@ -59,6 +60,9 @@ function writeInits(obj)
                     "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ...
                     "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ...
                     "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ...
+                    "useSinrComms", obj.useSinrComms, "sinrThreshold", obj.sinrThreshold, "pathLossExponent", obj.pathLossExponent, "ambientTemp", obj.ambientTemp, ...
+                    "centerFreq", obj.centerFreq, "bandwidth", obj.bandwidth, "txPower", txPower, ... % SINR comms model parameters
+                    "useRoutingTopology", obj.useRoutingTopology, ... % topology-selection algorithm (CALSN when true)
                     "tilt", tilt, "azimuth", azimuth, ... % joint sensor parameters
                     "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... % sigmoid sensor parameters
                     "lossExponent", lossExponent, "P_TX", P_TX, "BW", BW, "f_c", f_c, "G_RX_dBi", G_RX_dBi, "beamwidthExponent", beamwidthExponent, ... % RF sensor parameters

@rfSensor/RSS.m

@@ -19,6 +19,5 @@ function value = RSS(obj, d, dx, dy, dz)
     cos_theta = (st .* (dx .* sa + dy .* ca) - ct .* dz) ./ max(d, eps);
     cos_theta = max(-1, min(1, cos_theta));
     theta = acosd(cos_theta);
-    gain  = 10 .* obj.beamwidthExponent .* log10((1 + cosd(theta)) ./ 2);
+    value = obj.P_TX_dBm + obj.transmitterGain(theta) + obj.G_RX_dBi - obj.pathLoss(d);
-    value = obj.P_TX_dBm + gain + obj.G_RX_dBi - obj.pathLoss(d);
 end

@rfSensor/halfAngle.m

@@ -5,12 +5,8 @@ function value = halfAngle(obj)
     arguments (Output)
         value (1, 1) double;
     end
-    % Sweep angular offset from boresight by evaluating transmitterGain at
-    % (obj.tilt + dtheta, obj.azimuth). The cosine difference identity guarantees
-    % the resulting angular offset from boresight equals dtheta exactly,
-    % independent of the actual pointing direction.
     dtheta = (0:0.1:179.9)';
-    gain = obj.transmitterGain(obj.tilt + dtheta, obj.azimuth * ones(size(dtheta)));
+    gain = obj.transmitterGain(dtheta);
     target = gain(1) - 3;
     idx = find(gain <= target, 1);
     if isempty(idx) || idx == 1

@rfSensor/initialize.m

@@ -1,7 +1,8 @@
-function obj = initialize(obj, txPower, bandwidth, centerFreq, rxGain_dBi, beamwidthExponent, tilt, azimuth, lossExponent)
+function obj = initialize(obj, txPower, temperature, bandwidth, centerFreq, rxGain_dBi, beamwidthExponent, tilt, azimuth, lossExponent)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "rfSensor")}
         txPower (1, 1) double;
+        temperature (1, 1) double;
         bandwidth (1, 1) double;
         centerFreq (1, 1) double;
         rxGain_dBi (1, 1) double;
@@ -16,6 +17,7 @@ function obj = initialize(obj, txPower, bandwidth, centerFreq, rxGain_dBi, beamw
 
     %% Provided values
     obj.P_TX = txPower; % Transmit power (W)
+    obj.T_0 = temperature; % Temperature (K)
     obj.BW = bandwidth; % Bandwidth (Hz)
     obj.f_c = centerFreq; % Center frequency (Hz)
     obj.G_RX_dBi = rxGain_dBi; % Receiving Antenna Gain (dBi)
@@ -29,4 +31,5 @@ function obj = initialize(obj, txPower, bandwidth, centerFreq, rxGain_dBi, beamw
     %% Computed values
     obj.P_TX_dBm = 10*log10(obj.P_TX/1e-3); % Transmit power in dBm
     obj.N = obj.k_B * obj.T_0 * obj.BW; % Thermal noise
+    obj.constantGainTerm_dB = 10 .* log10(2 * (obj.beamwidthExponent + 1)); % used in RSS calculation, only need to compute once
 end

@rfSensor/pathLoss.m

@@ -8,6 +8,6 @@ function L_FSPL_dB = pathLoss(obj, d)
     end
 
     % Free Space Path Loss (dB); d clamped away from zero (log undefined at d=0)
-    L_FSPL_dB = obj.lossExponent * 10 * log10(max(d, eps)) + 20 * log10(obj.f_c) + 20 * log10((4*pi)/obj.c);
+    L_FSPL_dB = obj.lossExponent * 10 * log10(max(d, eps)) + 20 * log10((4 * pi * obj.f_c) / obj.c);
 
 end

@rfSensor/plotParameters.m

@@ -23,8 +23,11 @@ function f = plotParameters(obj)
         % For agent at altitude alt, ground target at tilt T has slant distance:
         D = alt ./ cosd(T);
 
-        % Compute RSS for each (d, t, a) triple
+        % Convert LOS (D, T, A) to displacement vectors for RSS
-        rss = obj.RSS(D(:), T(:), A(:));
+        dx = D .* sind(T) .* sind(A);
+        dy = D .* sind(T) .* cosd(A);
+        dz = -D .* cosd(T);   % negative: ground targets are below agent
+        rss = obj.RSS(D(:), dx(:), dy(:), dz(:));
         Fslice = reshape(rss, size(D));
 
         % Disc geometry: t=0 (nadir) -> center, t~90 (horizon) -> edge

@rfSensor/plotPerformance.m

@@ -34,10 +34,6 @@ function f = plotPerformance(obj, altitude, otherSensorsPos, otherSensors)
     SINR = reshape(SINR, size(targetPosX));
     SNR = reshape(SNR, size(targetPosX));
 
-    % normalize in linear scale
-    SINR = 10.^(SINR/10); SINR = SINR ./ max(SINR(:)); SINR = 10 * log10(SINR);
-    SNR  = 10.^(SNR/10);  SNR  = SNR  ./ max(SNR(:));  SNR  = 10 * log10(SNR);
-
     % Collect sensor positions and boresight parameters for overlay
     sensorXY      = [0, 0; otherSensorsPos(:, 1:2)];
     sensorTilts   = [obj.tilt;    cellfun(@(s) s.tilt,    otherSensors)];
@@ -50,7 +46,8 @@ function f = plotPerformance(obj, altitude, otherSensorsPos, otherSensors)
     nexttile;
     imagesc(distances, distances, SNR);
     axis("image"); set(gca, 'YDir', 'normal');
-    colorbar; xlabel("X (m)"); ylabel("Y (m)");
+    cb = colorbar; cb.Label.String = "SNR (dB)";
+    xlabel("X (m)"); ylabel("Y (m)");
     title("Linearly Normalized SNR (dB)");
     subtitle("No interfering sources");
     addSensorOverlay(gca, sensorXY(1, 1:2), sensorTilts(1, 1), sensorAzimuths(1, 1), tailScale);
@@ -58,7 +55,8 @@ function f = plotPerformance(obj, altitude, otherSensorsPos, otherSensors)
     nexttile;
     imagesc(distances, distances, SINR);
     axis("image"); set(gca, 'YDir', 'normal');
-    colorbar; xlabel("X (m)"); ylabel("Y (m)");
+    cb = colorbar; cb.Label.String = "SNR (dB)";
+    xlabel("X (m)"); ylabel("Y (m)");
     title("Linearly Normalized SINR (dB)");
     subtitle(sprintf("%d interfering source(s)", size(otherSensorsPos, 1)));
     addSensorOverlay(gca, sensorXY, sensorTilts, sensorAzimuths, tailScale);

@rfSensor/rfSensor.m

@@ -3,7 +3,7 @@ classdef rfSensor
         % Physical parameters
         c = 3e8; % Speed of light (m/s)
         k_B = 1.38e-23 % Boltzmann constant (W/Hz/K) for thermal noise model
-        T_0 = 300; % Ambient temperature (Kelvin) for thermal noise model
+        T_0 = NaN; % Ambient temperature (Kelvin) for thermal noise model
         lossExponent = NaN; % Path loss exponent (2 for free space, up to 6 for the lossiest environments)
         % Sensor parameters
         P_TX = NaN; % Transmit power (Watts)
@@ -14,6 +14,7 @@ classdef rfSensor
         % Values computed at initialization
         P_TX_dBm = NaN; % Transmit power (dBm)
         N = NaN; % Thermal noise
+        constantGainTerm_dB = NaN; % used in RSS calculation, only need to compute once
         % Cached state (per timestep)
     end
     properties (Access = public)
@@ -23,7 +24,7 @@ classdef rfSensor
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, txPower, bandwidth, centerFreq, rxGain, beamwidthExponent, tilt, azimuth); % initialize sensor, define parameters
+        [obj] = initialize(obj, txPower, temperature, bandwidth, centerFreq, rxGain, beamwidthExponent, tilt, azimuth); % initialize sensor, define parameters
         [SINR, SNR, obj, otherSensors] = sensorPerformance(obj, agentPos, targetPos, otherSensorsPos, otherSensors); % determine sensor performance for a given single sensor and target geometry
         [d, dx, dy, dz] = computePointToPoints(obj, agentPos, targetPos);
         [value] = halfAngle(obj); % tilt angle (deg) at which sensor performance is halved
@@ -34,7 +35,7 @@ classdef rfSensor
     end
     methods (Access = private)
         x = RSS(obj, d, dx, dy, dz); % Received signal strength (function of distance and tilt angle)
-        G_TX_dB = transmitterGain(obj, t, a); % Antenna gain for a given TX/RX pair 
+        G_TX_dB = transmitterGain(obj, theta); % Antenna gain for a given angle from boresight (degrees)
         L_FSPL_dB = pathLoss(obj, d); % Free space path loss for a given TX/RX pair
     end
 end

@rfSensor/transmitterGain.m

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

@sensingObjective/plot.m

@@ -36,7 +36,14 @@ function f = plot(obj, ind, f)
     % Add to other perspectives
     if size(ind, 2) > 1
         for ii = 2:size(ind, 2)
-            copyobj(o, f.Children(1).Children(ind(ii)));
+            axCopy = f.Children(1).Children(ind(ii));
+            copyobj(o, axCopy);
+            % copyobj does not carry color scaling. The objective values are
+            % sum-normalized (tiny), so without matching CLim the copied surface
+            % is squished to one color once the destination axes' auto CLim is
+            % widened by later surfaces (e.g. agent geometries). Pin it to the
+            % source range so the heatmap renders as a visible gradient.
+            clim(axCopy, cRange);
         end
     end
 end

@sigmoidSensor/plotParameters.m

@@ -1,22 +1,32 @@
-function f = plotParameters(obj)
+function f = plotParameters(obj, f)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
         f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Distance and tilt sample points
-    d = 0:(obj.alphaDist / 100):(2*obj.alphaDist);
+    d = 0:(obj.alphaDist / 1000):(2*obj.alphaDist);
-    t = -90:1:90;
+    t = -90:0.1:90;
 
     % Sample membership functions
     d_x = obj.distanceMembership(d);
     t_x = obj.tiltMembership(t);
 
+    % pad ends of sampled results with zeros at +/- some big number
+    big = 1e6 * max(d);
+    d = [-big, d, big];
+    t = [-big, t, big];
+    d_x = [0, d_x', 0];
+    t_x = [0, t_x', 0];
+
     % Plot resultant sigmoid curves
-    f = figure;
+    hold(gca(f), "on");
+    if isempty(gca(f).Children)
         tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
+    end
 
     % Distance
     nexttile(1, [1,  1]);
@@ -24,9 +34,12 @@ function f = plotParameters(obj)
     title("Distance Membership Sigmoid");
     xlabel("Distance (m)");
     ylabel("Membership");
+    ld = legend();
     hold("on");
     plot(d, d_x, "LineWidth", 2);
     hold("off");
+    xData = cell2mat(arrayfun(@(x) x.XData(2:(end - 1)), f.Children.Children(end).Children, 'UniformOutput', false));
+    xlim([min(xData, [], "all"), max(xData, [], "all")]);
     ylim([0, 1]);
     
     % Tilt
@@ -35,8 +48,21 @@ function f = plotParameters(obj)
     title("Tilt Membership Sigmoid");
     xlabel("Tilt (deg)");
     ylabel("Membership");
+    lt = legend();
     hold("on");
     plot(t, t_x, "LineWidth", 2);
     hold("off");
+    xlim([-90, 90]);
     ylim([0, 1]);
+
+    % add legends
+    nexttile(1, [1,  1]);
+    f.Children.Children(3).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
+    f.Children.Children(1).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
+
+    nexttile(2, [1,  1]);
+    f.Children.Children(3).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
+    f.Children.Children(1).String{end} = sprintf("Sensor %d", length(f.Children.Children(end).Children));
+
+    hold(gca(f), "off");
 end

@sigmoidSensor/sigmoidSensor.m

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

aerpaw/MESSAGE_TYPE.m

@@ -8,5 +8,6 @@ classdef MESSAGE_TYPE < uint8
         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)
+        TAKEOFF          (9)  % Server->Client: take off to 25 m, ACK then READY on arrival
     end
 end

aerpaw/client/uav_runner.py

@@ -44,6 +44,7 @@ class MessageType(IntEnum):
     GUIDANCE_TOGGLE  = 6
     REQUEST_POSITION = 7
     POSITION         = 8
+    TAKEOFF          = 9
 
 
 AERPAW_DIR = Path('/root/miSim/aerpaw')
@@ -184,13 +185,11 @@ class UAVRunner(BasicRunner):
         log_task = None
         nav_task = None
         try:
-            # Takeoff to above AERPAW minimum altitude
+            # Takeoff is now driven by a TAKEOFF command from the controller
-            print("[UAV] Taking off...")
+            # (staggered in time between UAVs), not performed at startup. Start
-            await drone.takeoff(25)
+            # GPS logging immediately so the ground/idle period is captured too.
-            print("[UAV] Takeoff complete, waiting for commands...")
-
-            # Start GPS logging in background
             log_task = asyncio.create_task(_gps_log_loop(drone))
+            print("[UAV] Connected; waiting for commands...")
 
             # Command loop - handle all messages from controller
             waypoint_num = 0
@@ -213,6 +212,27 @@ class UAVRunner(BasicRunner):
                         await send_message_type(writer, MessageType.ACK)
                         print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
 
+                elif msg_type == MessageType.TAKEOFF:
+                    # aerpawlib only switches the vehicle to GUIDED inside
+                    # _initialize_postarm(), which await_ready_to_move() runs only
+                    # when the vehicle is DISARMED at the first movement command.
+                    # The safety pilot arms all UAVs at experiment start, so a UAV
+                    # whose takeoff is deliberately delayed (for time separation)
+                    # is already armed by now and would skip that path -> no GUIDED
+                    # switch -> simple_takeoff is ignored in ALT_HOLD and it never
+                    # leaves the ground. Run the post-arm init explicitly to
+                    # guarantee GUIDED mode (and home capture) before taking off;
+                    # takeoff()'s own gate then sees the vehicle armed and does not
+                    # re-initialize.
+                    await send_message_type(writer, MessageType.ACK)
+                    print("[UAV] Sent ACK (takeoff)")
+                    print("[UAV] Initializing (GUIDED) and taking off to 25 m...")
+                    await drone._initialize_postarm()
+                    await drone.takeoff(25)
+                    print("[UAV] Takeoff complete")
+                    await send_message_type(writer, MessageType.READY)
+                    print("[UAV] Sent READY")
+
                 elif msg_type == MessageType.REQUEST_POSITION:
                     # Respond immediately with current ENU position relative to origin
                     pos = drone.position
@@ -288,6 +308,11 @@ class UAVRunner(BasicRunner):
         except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
             print(f"[UAV] Error: {e}")
 
+        except Exception as e:
+            import traceback
+            print(f"[UAV] Unhandled exception: {type(e).__name__}: {e}")
+            traceback.print_exc()
+
         finally:
             if nav_task is not None and not nav_task.done():
                 nav_task.cancel()

aerpaw/config/scenario.csv

@@ -1,2 +1,2 @@
-timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology, txPower, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth
-1, 100, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 80.0", "0.25, 0.25", "8.0, 8.0", "0.1, 0.1", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "66.6, 66.6", "55, 35, 35, 55", 0.15, "15.0, 15.0, 50.0, 40.0, 15.0, 50.0", 1, "0.0, 35.0, 0.0", "50, 40.0, 60", 1, 2.0, 1
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1, "0.2, 0.1", 0, 5.0, 2.5, 290.0, 2.4e9, 20e6

aerpaw/config/scenario_columns.csv

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

aerpaw/config/scenario_lock.csv

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

aerpaw/config/scenario_walls.csv

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

aerpaw/controller.m

@@ -7,58 +7,50 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-% Two waypoints per UAV: altitude-staggered transit + final position
+% Three waypoints per UAV: stage (climb) → traverse (XY) → settle (descend)
-MAX_TARGETS = MAX_CLIENTS * 2;
+MAX_TARGETS = MAX_CLIENTS * 3;
+
+% Altitude-staggered flyout/flyback currently supports exactly 2 UAVs
+if numClients ~= int32(2)
+    error('Altitude-staggered flyout/flyback requires exactly 2 UAVs');
+end
+
+% Minimum vertical separation maintained between UAVs during XY transit (metres)
+TRANSIT_SEP = 30.0;
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
-numWaypoints = int32(0);
+numWaypoints = int32(0); %#ok<NASGU>
-totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
+totalLoaded  = int32(0);
 
-% Load initial UAV positions from scenario CSV
+% Experiment start positions from scenario CSV (N x 3)
+scenarioPositions = zeros(MAX_CLIENTS, 3);
+
+% Load experiment start positions from scenario CSV
 if coder.target('MATLAB')
     disp('Loading initial positions from scenario.csv (simulation)...');
     tmpSim = miSim;
     sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
     flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
-    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
+    posMatrix = reshape(flatPos, 3, [])';   % N×3
     totalLoaded = int32(size(posMatrix, 1));
+    scenarioPositions(1:totalLoaded, :) = posMatrix;
+    % MATLAB path: send directly to scenario positions in one waypoint
     targets(1:totalLoaded, :) = posMatrix;
     numWaypoints = int32(1);
     disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
 else
     coder.cinclude('controller_impl.h');
     filename = ['config/scenario.csv', char(0)];
+    % Load into targets temporarily; copy to scenarioPositions below
     totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
                 coder.ref(targets), int32(MAX_TARGETS));
-    numWaypoints = totalLoaded / int32(numClients);
+    scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :);
-end
+    numWaypoints = int32(3);
-
-% In the compiled path, inject altitude-staggered transit waypoints so UAVs
-% are vertically separated while flying horizontally to their start positions.
-% ArduPilot reaches target altitude before horizontal movement, so UAV i is at
-% altitude (TRANSIT_ALT_BASE + (i-1)*TRANSIT_ALT_STEP) throughout its transit,
-% preventing collisions regardless of horizontal path geometry.
-% TRANSIT_ALT_STEP must exceed 2 * max(collisionRadius).
-% Waypoint 1: each UAV flies to (finalX, finalY) at its unique transit altitude.
-% Waypoint 2: each UAV adjusts to its actual target altitude.
-% These constants are also used for the altitude-staggered return before RTL.
-TRANSIT_ALT_BASE = 25.0;  % must match drone.takeoff() altitude in uav_runner.py
-TRANSIT_ALT_STEP = 25;    % vertical separation per UAV (m); must exceed 2*collisionRadius
-if ~coder.target('MATLAB')
-    for ii = double(totalLoaded):-1:1
-        transitRow = (ii - 1) * 2 + 1;
-        finalRow   = (ii - 1) * 2 + 2;
-        finalPos   = targets(ii, :);
-        transitAlt = TRANSIT_ALT_BASE + (ii - 1) * TRANSIT_ALT_STEP;
-        targets(finalRow, :)   = finalPos;
-        targets(transitRow, :) = [finalPos(1), finalPos(2), transitAlt];
-    end
-    numWaypoints = int32(2);
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 55;
+NUM_SCENARIO_PARAMS = 65;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
@@ -92,43 +84,126 @@ for i = 1:numClients
     end
 end
 
-% Waypoint loop: send each waypoint to all clients, wait for all to arrive
+% Query takeoff-pad GPS positions before sending any waypoints.
+% These are also the flyback targets so each UAV lands where it took off.
+initialPositions = zeros(MAX_CLIENTS, 3);
+if ~coder.target('MATLAB')
+    coder.ceval('sendRequestPositions', int32(numClients));
+    coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS));
+else
+    % Simulation: treat scenario positions as the takeoff locations
+    initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
+end
+
+% ---- Build altitude-staggered flyout waypoints --------------------------------
+% Each UAV follows three phases:
+%   WP1: Climb to transit altitude at pad XY
+%   WP2: Traverse to final XY at transit altitude  (>= TRANSIT_SEP vertical gap)
+%   WP3: Descend to final altitude at final XY
+%
+% Transit altitude assignment: the UAV with the highest final altitude keeps
+% that altitude as its transit altitude; the other UAV climbs TRANSIT_SEP above
+% it, guaranteeing >= 30 m vertical separation during the simultaneous XY
+% traverse (WP2).  Both transit altitudes are well above minAlt.
+if ~coder.target('MATLAB')
+    if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
+        higherIdx = int32(1);
+        lowerIdx  = int32(2);
+    else
+        higherIdx = int32(2);
+        lowerIdx  = int32(1);
+    end
+
+    transitAlt = zeros(1, MAX_CLIENTS);
+    transitAlt(lowerIdx)  = scenarioPositions(lowerIdx, 3);
+    transitAlt(higherIdx) = scenarioPositions(lowerIdx, 3) + TRANSIT_SEP;
+
+    for iUAV = 1:numClients
+        base = double(iUAV - 1) * double(numWaypoints);
+        targets(base + 1, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), transitAlt(iUAV)];
+        targets(base + 2, :) = [scenarioPositions(iUAV,1), scenarioPositions(iUAV,2), transitAlt(iUAV)];
+        targets(base + 3, :) =  scenarioPositions(iUAV, :);
+    end
+end
+% -------------------------------------------------------------------------------
+
+% Waypoint loop
+if coder.target('MATLAB')
+    % Simulation: simple lockstep (collisions are not modelled here).
     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(['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...');
+        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+    end
 else
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
+    % ---- Staggered flyout -----------------------------------------------------
-                    int32(MESSAGE_TYPE.ACK));
+    % Separate the two UAVs in TIME so they are never doing the same vertical
+    % climb at the same moment over their ~10 m-separated pads (which can trip
+    % the proximity guard on a spurious GPS sample). The UAV that transits
+    % highest leads by exactly one event.
+    %
+    % Events per UAV: 1 = TAKEOFF (25 m), 2 = WP1 (climb at pad), 3 = WP2
+    % (traverse), 4 = WP3 (descend). At each step the higher UAV does event
+    % `step` while the lower UAV does event `step-1`; both are dispatched, then
+    % we collect ACK and then READY (arrival) from each active UAV.
+    numEvents = double(numWaypoints) + 1;   % 3 waypoints + 1 takeoff
+    for step = 1:(numEvents + 1)
+        hEvent = step;
+        lEvent = step - 1;
+        hActive = (hEvent >= 1) && (hEvent <= numEvents);
+        lActive = (lEvent >= 1) && (lEvent <= numEvents);
+
+        % --- Dispatch this step's event to each active UAV ---
+        if hActive
+            if hEvent == 1
+                coder.ceval('sendMessageType', higherIdx, int32(MESSAGE_TYPE.TAKEOFF));
+            else
+                hRow = (double(higherIdx) - 1) * double(numWaypoints) + (hEvent - 1);
+                hTarget = targets(hRow, :);
+                coder.ceval('sendTarget', higherIdx, coder.ref(hTarget));
+            end
+        end
+        if lActive
+            if lEvent == 1
+                coder.ceval('sendMessageType', lowerIdx, int32(MESSAGE_TYPE.TAKEOFF));
+            else
+                lRow = (double(lowerIdx) - 1) * double(numWaypoints) + (lEvent - 1);
+                lTarget = targets(lRow, :);
+                coder.ceval('sendTarget', lowerIdx, coder.ref(lTarget));
+            end
         end
 
-    % Wait for READY from all clients (all arrived at waypoint w)
+        % --- Collect ACK (command received) from each active UAV ---
-    if coder.target('MATLAB')
+        if hActive
-        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.ACK));
-    else
+        end
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
+        if lActive
-                    int32(MESSAGE_TYPE.READY));
+            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.ACK));
+        end
+
+        % --- Collect READY (arrived at event) from each active UAV ---
+        if hActive
+            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.READY));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.READY));
+        end
     end
 end
 
 % ---- Phase 2: miSim guidance loop ----------------------------------------
-% Guidance parameters (adjust here and recompile as needed)
+% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2))
-MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+% so the controller and the agent step-decay schedule stay in sync.
+if coder.target('MATLAB')
+    MAX_GUIDANCE_STEPS = int32(sc.maxIter);
+else
+    MAX_GUIDANCE_STEPS = int32(scenarioParams(2));
+end
 
 % Enter guidance mode on all clients
 if ~coder.target('MATLAB')
@@ -141,8 +216,8 @@ if ~coder.target('MATLAB')
     coder.ceval('sendRequestPositions', int32(numClients));
     coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
 else
-    % Simulation: seed positions from CSV waypoints so agents don't start at origin
+    % Simulation: seed positions from scenario positions so agents don't start at origin
-    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
 end
 guidance_step(positions(1:numClients, :), true, ...
               scenarioParams, obstacleMin, obstacleMax, numObstacles);
@@ -197,21 +272,79 @@ if ~coder.target('MATLAB')
 end
 % --------------------------------------------------------------------------
 
-% Altitude-staggered return: separate UAVs vertically before issuing RTL,
+% ---- Altitude-staggered flyback -------------------------------------------
-% mirroring the initial positioning stagger so UAVs transit laterally at
+% Mirror of flyout: stage (climb) → traverse (XY) → settle (descend to pad).
-% unique altitudes and cannot collide during the return flight.
+% Transit altitudes are based on post-guidance positions so the UAV that ended
+% higher stays highest, guaranteeing >= TRANSIT_SEP vertical clearance during
+% the simultaneous XY traverse.
+NUM_RETURN_WP = int32(3);
+returnTargets = zeros(MAX_TARGETS, 3);
+
 if ~coder.target('MATLAB')
-    for i = 1:numClients
+    if positions(1, 3) >= positions(2, 3)
-        transitAlt = TRANSIT_ALT_BASE + (double(i) - 1) * TRANSIT_ALT_STEP;
+        higherRetIdx = int32(1);
-        target = [positions(i, 1), positions(i, 2), transitAlt];
+        lowerRetIdx  = int32(2);
-        coder.ceval('sendTarget', int32(i), coder.ref(target));
-    end
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
     else
-    disp('Altitude-staggered return (simulation): UAVs commanded to transit altitudes.');
+        higherRetIdx = int32(2);
+        lowerRetIdx  = int32(1);
     end
 
+    returnTransitAlt = zeros(1, MAX_CLIENTS);
+    returnTransitAlt(lowerRetIdx)  = positions(lowerRetIdx, 3);
+    returnTransitAlt(higherRetIdx) = positions(lowerRetIdx, 3) + TRANSIT_SEP;
+
+    for iUAV = 1:numClients
+        retBase = double(iUAV - 1) * double(NUM_RETURN_WP);
+        returnTargets(retBase + 1, :) = [positions(iUAV,1),         positions(iUAV,2),         returnTransitAlt(iUAV)];
+        returnTargets(retBase + 2, :) = [initialPositions(iUAV,1),  initialPositions(iUAV,2),  returnTransitAlt(iUAV)];
+        % Final return waypoint: pad XY from the pre-takeoff GPS query, but a fixed
+        % 25 m up. That query ran while the UAV was on the ground, so its ENU
+        % up-component is ~0 (often slightly negative) — using it directly commands
+        % a negative altitude. RTL/LAND perform the actual touchdown from here.
+        returnTargets(retBase + 3, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), 25.0];
+    end
+
+    % Staggered flyback: same higher-leads-by-one pattern as the flyout, but
+    % over the 3 return waypoints only (the UAVs are already airborne, so there
+    % is no TAKEOFF event). The higher UAV climbs/traverses/descends one event
+    % ahead of the lower, so they never share the same descent over their pads.
+    numReturnEvents = double(NUM_RETURN_WP);   % 3
+    for step = 1:(numReturnEvents + 1)
+        hEvent = step;
+        lEvent = step - 1;
+        hActive = (hEvent >= 1) && (hEvent <= numReturnEvents);
+        lActive = (lEvent >= 1) && (lEvent <= numReturnEvents);
+
+        if hActive
+            hRow = (double(higherRetIdx) - 1) * double(NUM_RETURN_WP) + hEvent;
+            hTarget = returnTargets(hRow, :);
+            coder.ceval('sendTarget', higherRetIdx, coder.ref(hTarget));
+        end
+        if lActive
+            lRow = (double(lowerRetIdx) - 1) * double(NUM_RETURN_WP) + lEvent;
+            lTarget = returnTargets(lRow, :);
+            coder.ceval('sendTarget', lowerRetIdx, coder.ref(lTarget));
+        end
+
+        if hActive
+            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.ACK));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.ACK));
+        end
+
+        if hActive
+            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.READY));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.READY));
+        end
+    end
+else
+    disp('Altitude-staggered return (simulation): UAVs commanded back to takeoff positions.');
+end
+% --------------------------------------------------------------------------
+
 % Send RTL command to all clients
 for i = 1:numClients
     if coder.target('MATLAB')

aerpaw/guidance_step.m

@@ -17,7 +17,8 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                       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):
+%                       Index mapping (1-based; authoritative map in
+%                       aerpaw/impl/controller_impl.h):
 %                         1  timestep            9-12  collisionRadius[1:4]
 %                         2  maxIter            13-16  comRange[1:4]
 %                         3  minAlt             17-20  alphaDist[1:4]
@@ -26,12 +27,20 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
 %                         6  initialStepSize    29-32  betaTilt[1:4]
 %                         7  barrierGain        33-35  domainMin
 %                         8  barrierExponent    36-38  domainMax
-%                                               39-40  objectivePos
+%                                               39     numObjectiveComponents
-%                                               41-44  objectiveVar (2x2, col-major)
+%                                               40-43  objectivePos [x1,y1,x2,y2]
-%                                               45     sensorPerformanceMinimum
+%                                               44-51  objectiveVar [per component]
-%                                               46     useDoubleIntegrator
+%                                               52     sensorPerformanceMinimum
-%                                               47     dampingCoeff
+%                                               53     useDoubleIntegrator
-%                                               48     useFixedTopology
+%                                               54     dampingCoeff
+%                                               55     useFixedTopology
+%                                               56-59  txPower[1:4]
+%                                               60     useSinrComms
+%                                               61     sinrThreshold (dB)
+%                                               62     pathLossExponent
+%                                               63     ambientTemp (K)
+%                                               64     centerFreq (Hz)
+%                                               65     bandwidth (Hz)
 %   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
 %   obstacleMax       (MAX_OBSTACLES × 3) double
 %   numObstacles      (1,1) int32                 actual obstacle count
@@ -101,6 +110,13 @@ if isInit
         USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(53));
         DAMPING_COEFF               = scenarioParams(54);
         USE_FIXED_TOPOLOGY          = logical(scenarioParams(55));
+        TX_POWER_VEC                = scenarioParams(56:59); % per-UAV [1:MAX_CLIENTS] (W)
+        USE_SINR_COMMS              = logical(scenarioParams(60));
+        SINR_THRESHOLD              = scenarioParams(61);    % dB
+        PATH_LOSS_EXPONENT          = scenarioParams(62);
+        AMBIENT_TEMP                = scenarioParams(63);    % Kelvin
+        CENTER_FREQ                 = scenarioParams(64);    % Hz
+        BANDWIDTH                   = scenarioParams(65);    % Hz
 
         % --- Build domain geometry ---
         dom = rectangularPrism;
@@ -141,6 +157,7 @@ if isInit
             ag = agent;
             ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
                                INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+            ag.txPower = TX_POWER_VEC(ii); % SINR comms model transmit power (W)
             agentList{ii} = ag;
         end
 
@@ -158,7 +175,8 @@ if isInit
         sim = miSim;
         sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
                              MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
-                             USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY);
+                             USE_DOUBLE_INTEGRATOR, DAMPING_COEFF, USE_FIXED_TOPOLOGY, false, ...
+                             USE_SINR_COMMS, SINR_THRESHOLD, PATH_LOSS_EXPONENT, AMBIENT_TEMP, CENTER_FREQ, BANDWIDTH);
     end
 
     % On the init call return current positions unchanged
@@ -187,8 +205,13 @@ else
     % 2. Advance timestep counter
     sim.timestepIndex = sim.timestepIndex + 1;
 
-    % 3. Update communications topology (Lesser Neighbour Assignment)
+    % 3. Update communications topology (Lesser Neighbour Assignment).
+    % Refresh the adjacency graph from current positions first so connectivity
+    % (fixed-radius or SINR) tracks the agents as they move; otherwise the graph
+    % would be frozen at init on the compiled path (the MATLAB sim refreshes it
+    % in @miSim/run.m). Skip when topology is fixed.
     if ~sim.useFixedTopology
+        sim = sim.updateAdjacency();
         sim = sim.lesserNeighbor();
     end
 

aerpaw/impl/controller_impl.cpp

@@ -238,8 +238,8 @@ int loadScenario(const char* filename, double* params) {
     strncpy(copy, line, sizeof(copy) - 1);
     copy[sizeof(copy) - 1] = '\0';
 
-    char* fields[32];
+    char* fields[40];
-    int nf = splitCSVRow(copy, fields, 32);
+    int nf = splitCSVRow(copy, fields, 40);
     if (nf < 26) {
         fprintf(stderr, "loadScenario: expected >=26 columns, got %d\n", nf);
         return 0;
@@ -376,8 +376,71 @@ int loadScenario(const char* filename, double* params) {
         params[54] = atof(trimField(tmp));
     }
 
+    // ---- SINR communications model (columns 26–30; optional) ----
+    // Defaults mirror @miSim/initializeFromCsv.m so a CSV predating the SINR
+    // model still loads (useSinrComms stays 0 -> fixed-radius path is used).
+    //   txPower[1:4] -> params[55:58];  useSinrComms -> 59;
+    //   sinrThreshold (dB) -> 60;  pathLossExponent -> 61;  ambientTemp (K) -> 62;
+    //   centerFreq (Hz) -> 63;  bandwidth (Hz) -> 64.
+    params[55] = 0.1; params[56] = 0.1; params[57] = 0.1; params[58] = 0.1;
+    params[59] = 0.0;     // useSinrComms
+    params[60] = 0.0;     // sinrThreshold (dB)
+    params[61] = 2.0;     // pathLossExponent
+    params[62] = 290.0;   // ambientTemp (K)
+    params[63] = 2.4e9;   // centerFreq (Hz)
+    params[64] = 20e6;    // bandwidth (Hz)
+
+    // txPower: column 26 (per-UAV, up to MAX_CLIENTS_PER_PARAM values, zero-padded)
+    if (nf > 26) {
+        char tmp[256]; strncpy(tmp, fields[26], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        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) {
+            for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) params[55 + k] = vals[k];
+        }
+    }
+
+    // useSinrComms: column 27
+    if (nf > 27) {
+        char tmp[64]; strncpy(tmp, fields[27], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[59] = atof(trimField(tmp));
+    }
+    // sinrThreshold: column 28 (dB)
+    if (nf > 28) {
+        char tmp[64]; strncpy(tmp, fields[28], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[60] = atof(trimField(tmp));
+    }
+    // pathLossExponent: column 29
+    if (nf > 29) {
+        char tmp[64]; strncpy(tmp, fields[29], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[61] = atof(trimField(tmp));
+    }
+    // ambientTemp: column 30 (K)
+    if (nf > 30) {
+        char tmp[64]; strncpy(tmp, fields[30], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[62] = atof(trimField(tmp));
+    }
+    // centerFreq: column 31 (Hz)
+    if (nf > 31) {
+        char tmp[64]; strncpy(tmp, fields[31], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[63] = atof(trimField(tmp));
+    }
+    // bandwidth: column 32 (Hz)
+    if (nf > 32) {
+        char tmp[64]; strncpy(tmp, fields[32], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[64] = atof(trimField(tmp));
+    }
+
     printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g], %d objective component(s)\n",
            params[32], params[33], params[34], params[35], params[36], params[37], (int)params[38]);
+    printf("Comms model: %s (SINR threshold %g dB, path-loss exp %g, T=%g K, f_c=%g Hz, BW=%g Hz)\n",
+           params[59] != 0.0 ? "SINR" : "fixed-radius", params[60], params[61], params[62], params[63], params[64]);
     return 1;
 }
 
@@ -512,6 +575,7 @@ static const char* messageTypeName(uint8_t msgType) {
         case 6: return "GUIDANCE_TOGGLE";
         case 7: return "REQUEST_POSITION";
         case 8: return "POSITION";
+        case 9: return "TAKEOFF";
         default: return "UNKNOWN";
     }
 }
@@ -614,6 +678,33 @@ int waitForAllMessageType(int numClients, int expectedType) {
     return 1;
 }
 
+// Wait for a specific message type from a SINGLE client (blocking).
+// Used by the staggered flyout/flyback, where the controller waits on one UAV
+// while the other is still flying its own (offset) event. Skips past any
+// unexpected byte and returns 0 on disconnect/error, 1 on success.
+int waitForClientMessageType(int clientId, int expectedType) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
+    int sock = clientSockets[clientId - 1];
+    uint8_t expected = (uint8_t)expectedType;
+
+    while (true) {
+        uint8_t msg;
+        int len = recv(sock, &msg, 1, MSG_WAITALL);
+        if (len <= 0) {
+            std::cerr << logPrefix() << "waitForClientMessageType: client " << clientId
+                      << " disconnected while waiting for " << messageTypeName(expected) << "\n";
+            return 0;
+        }
+        if (msg == expected) {
+            std::cout << logPrefix() << "Received " << messageTypeName(expected)
+                      << " from client " << clientId << "\n";
+            return 1;
+        }
+        std::cerr << logPrefix() << "Unexpected " << messageTypeName(msg) << " from client "
+                  << clientId << " (expected " << messageTypeName(expected) << ")\n";
+    }
+}
+
 // Broadcast GUIDANCE_TOGGLE to all clients
 void sendGuidanceToggle(int numClients) {
     for (int i = 1; i <= numClients; i++) {

aerpaw/impl/controller_impl.h

@@ -34,7 +34,14 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   52    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
 //   53    dampingCoeff
 //   54    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
-#define NUM_SCENARIO_PARAMS 55
+//   55-58 txPower[1:4]         (per-UAV transmit power, Watts; SINR comms model)
+//   59    useSinrComms         (0=fixed-radius comms, 1=SINR/path-loss comms)
+//   60    sinrThreshold        (SINR connectivity/CBF threshold, dB)
+//   61    pathLossExponent     (path-loss exponent n)
+//   62    ambientTemp          (ambient temperature, K; thermal noise)
+//   63    centerFreq           (carrier frequency, Hz; path-loss offset)
+//   64    bandwidth            (channel bandwidth, Hz; thermal noise)
+#define NUM_SCENARIO_PARAMS 65
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8
@@ -61,6 +68,9 @@ int loadObstacles(const char* filename,
 int sendMessageType(int clientId, int msgType);
 int sendTarget(int clientId, const double* coords);
 int waitForAllMessageType(int numClients, int expectedType);
+// Block until a SINGLE client sends expectedType (used by the staggered
+// flyout/flyback to wait on one UAV while the other is mid-flight).
+int waitForClientMessageType(int clientId, int expectedType);
 
 // Guidance loop operations
 void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration

aerpaw/results/plotGpsLogs.m

@@ -78,6 +78,11 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
             d = d(~isnan(d));
 
             fprintf("Minimum distance between agents %d and %d is %2.3f\n", ii, jj, min(d));
+            figure;
+            plot(G{1}.Timestamp(1:length(d)), d);
+            xlabel("Time"); ylabel("Distance"); title("UAV0 and UAV1 distance"); grid("on");
+            fprintf("Average distance between agents %d and %d is %2.3f\n", ii, jj, mean(d));
+            fprintf("Median distance between agents %d and %d is %2.3f\n", ii, jj, median(d));
             if min(d) < 6
                 warning("Minimum distance between agents %d and %d of %2.3f is questionable for AERPAW", ii, jj, min(d));
             end
@@ -86,6 +91,7 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
         % 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
+    fprintf("Flight duration is %s\n", string(G{1}.Timestamp(end) - G{1}.Timestamp(1), 'mm:ss'));
     
     % Plot domain
     altOffset = 1; % to avoid clipping into the ground when displayed
@@ -102,9 +108,14 @@ function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel, plotWholeFlight)
     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];
+    if length(params.objectivePos) > 2
+        objectivePos = reshape(params.objectivePos, [2, length(params.objectivePos) / 2])';
+    end
+    objectivePos = [objectivePos, zeros(size(objectivePos, 1), 1)];
     llaObj = enu2lla(objectivePos, lla0, "flat");
-    geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
+    for ii = 1:size(llaObj, 1)
+        geoplot3(gf, [llaObj(ii, 1), llaObj(ii, 1)], [llaObj(ii, 2), llaObj(ii, 2)], [llaObj(ii, 3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
+    end
     
     % Plot obstacles
     for ii = 1:params.numObstacles

aerpaw/results/resultsAnalysis.m

@@ -1,5 +1,5 @@
 %% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
+resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "columns_simulated"); % Define path to results copied from AERPAW platform
 
 % Check timeline in controller logs
 controller = controllerAnalysis(resultsPath);
@@ -25,7 +25,14 @@ makeVideo = true;
 % Define scenario according to CSV specification
 domain = rectangularPrism;
 domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
-domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [1, 2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
+if length(params.objectiveVar) > 4 && length(params.objectivePos) > 2
+    objectiveSigma = permute(reshape(params.objectiveVar, [length(params.objectiveVar)/4 2 2]), [3 1 2]);
+    objectivePos = reshape(params.objectivePos, [length(params.objectivePos)/2, 2])';
+else
+    objectiveSigma = reshape(params.objectiveVar, [1, 2, 2]);
+    objectivePos = params.objectivePos;
+end
+domain.objective = domain.objective.initialize(objectiveFunctionWrapper(objectivePos, objectiveSigma), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
 
 agents = cell(size(params.initialPositions, 2) / 3, 1);
 for ii = 1:size(agents, 1)
@@ -38,6 +45,9 @@ for ii = 1:size(agents, 1)
     collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
 
     agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, 5.0, sprintf("Agent %d", ii), plotCommsGeometry);
+    if isfield(params, 'txPower')
+        agents{ii}.txPower = params.txPower(ii); % SINR comms model transmit power (W)
+    end
 end
 
 % Create obstacles
@@ -47,9 +57,22 @@ for ii = 1:size(obstacles, 1)
     obstacles{ii} = obstacles{ii}.initialize([params.obstacleMin((((ii - 1) * 3) + 1):(ii * 3)); params.obstacleMax((((ii - 1) * 3) + 1):(ii * 3))], "OBSTACLE", sprintf("Obstacle %d", ii));
 end
 
-% Set up simulation
+% SINR communications model parameters (optional CSV columns; default to the
+% fixed-radius path so existing scenarios are unaffected)
+if isfield(params, 'useSinrComms');     useSinrComms = logical(params.useSinrComms); else; useSinrComms = false; end
+if isfield(params, 'sinrThreshold');    sinrThreshold = params.sinrThreshold;        else; sinrThreshold = 0;        end
+if isfield(params, 'pathLossExponent'); pathLossExponent = params.pathLossExponent;  else; pathLossExponent = 2.0;    end
+if isfield(params, 'ambientTemp');      ambientTemp = params.ambientTemp;            else; ambientTemp = 290.0;      end
+if isfield(params, 'centerFreq');       centerFreq = params.centerFreq;              else; centerFreq = 2.4e9;       end
+if isfield(params, 'bandwidth');        bandwidth = params.bandwidth;                else; bandwidth = 20e6;         end
+
+% Set up simulation. The four args after makeVideo (useDoubleIntegrator,
+% dampingCoeff, useFixedTopology, optimizeSensorPointing) are kept at their
+% prior effective defaults to preserve existing analysis behavior; only the
+% SINR comms parameters are threaded through from the scenario.
 sim = miSim;
-sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo);
+sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo, ...
+                     false, 2.0, false, false, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);
 
 % Save simulation parameters to output file
 sim.writeInits();
@@ -57,6 +80,9 @@ sim.writeInits();
 % Run
 sim = sim.run();
 
+% Save results
+sim = sim.teardown();
+
 %% Plot AERPAW trajectory logs onto simulated result for comparison
 % Duplicate plot to overlay with logged trajectories
 comparison = figure;

test/parametricTestSuite.m

@@ -42,7 +42,6 @@ classdef parametricTestSuite < matlab.unittest.TestCase
                 objectivePos = params.objectivePos;
             end
             tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(objectivePos, objectiveSigma), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum, objectivePos, objectiveSigma);
-
             agents = cell(size(params.initialPositions, 2) / 3, 1);
             for ii = 1:size(agents, 1)
                 agents{ii} = agent;

test/test_miSim.m

@@ -32,8 +32,8 @@ classdef test_miSim < matlab.unittest.TestCase
         % Agents
         initialStepSize = 0.2; % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter.
         initialMaxAngleStepSize = 0.1; % angular step size (degrees) for tilt/azimuth gradient ascent per timestep.
-        minAgents = 3; % Minimum number of agents to be randomly generated
+        minAgents = 7; % Minimum number of agents to be randomly generated
-        maxAgents = 4; % Maximum number of agents to be randomly generated
+        maxAgents = 7; % Maximum number of agents to be randomly generated
         useDoubleIntegrator = false;
         dampingCoeff = 2;
         agents = cell(0, 1);
@@ -57,6 +57,7 @@ classdef test_miSim < matlab.unittest.TestCase
         opticalPartitioningMin = 1e-6;
         % rfSensor
         P_TX = 1e-3; % Transmit power (Watts)
+        T_0 = 300; % Temperature (K)
         BW = 20e6; % Bandwidth (Hz)
         f_c = 3e9; % Center frequency (Hz)
         G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
@@ -104,6 +105,13 @@ classdef test_miSim < matlab.unittest.TestCase
     methods (Test)
         % Test methods
         function miSim_initialization(tc)
+            % Test flag: set true to exercise the SINR-aware routing
+            % overlay on this random scenario. Routing requires SINR comms, so
+            % the flag switches the whole SINR stack on (agents keep their
+            % default 0.1 W txPower); the permissive -30 dB threshold keeps
+            % random geometries fully connected.
+            useRoutingTopology = true;
+
             % randomly create obstacles
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
             tc.obstacles = cell(nGeom, 1);
@@ -239,8 +247,12 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
+            if useRoutingTopology
+                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, true, -30.0, 2.0, 290, 2.4e9, 20e6, true);
+            else
                 tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
             end
+        end
         function miSim_run_rf_sensor(tc)
             % randomly create obstacles
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
@@ -323,7 +335,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     % Initialize candidate agent sensor model
                     tc.sensor = rfSensor;
                     tilt = 0; azimuth = 0;
-                    tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);
+                    tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);
 
                     % Initialize candidate agent
                     newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
@@ -390,6 +402,13 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.testClass = tc.testClass.run();
         end
         function miSim_run(tc)
+            % Test flag: set true to exercise the min-max-workload routing
+            % overlay on this random scenario. Routing requires SINR comms, so
+            % the flag switches the whole SINR stack on (agents keep their
+            % default 0.1 W txPower); the permissive -30 dB threshold keeps
+            % random geometries fully connected.
+            useRoutingTopology = true;
+
             % randomly create obstacles
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
             tc.obstacles = cell(nGeom, 1);
@@ -526,7 +545,11 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
+            if useRoutingTopology
+                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, true, -30.0, 2.0, 290, 2.4e9, 20e6, true);
+            else
                 tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
+            end
 
             % Write out initialization state
             tc.testClass.writeInits();
@@ -651,7 +674,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 3], reshape([15, 4; 4, 15], [1 2 2])), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
         
             % Initialize agent collision geometry
             tc.agents = {agent};
@@ -659,7 +682,7 @@ classdef test_miSim < matlab.unittest.TestCase
             geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
 
             tc.sensor = rfSensor;
-            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 45, 45, tc.lossExponent);
+            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 45, 45, tc.lossExponent);
 
             % Initialize agents
             tc.maxIter = 75;
@@ -717,7 +740,7 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agent sensor model
 
             tc.sensor = rfSensor;
-            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 0, 0, tc.lossExponent);
+            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 0, 0, tc.lossExponent);
 
             % Initialize agents
             tc.maxIter = 75;
@@ -814,7 +837,395 @@ classdef test_miSim < matlab.unittest.TestCase
             % Run the simulation
             tc.testClass.run();
         end
-        function test_communications_constraint(tc)
+        function test_communications_constraint_SINR_threshold(tc)
+            % No obstacles
+            % Fixed two agents initial conditions
+            % Negligible collision geometries
+            % Non-standard domain with two objectives that will try to pull the
+            % agents apart
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
+        
+            % Initialize agent collision geometry
+            tc.agents = {agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
+            d = [1, 0, 0];
+            geometry1 = spherical;
+            geometry2 = geometry1;
+            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
+            
+            % Initialize agent sensor model
+            tc.sensor = sigmoidSensor;
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
+            
+            % Initialize obstacles
+            tc.obstacles = {};
+            
+            % Initialize agents
+            tc.maxIter = 50;
+            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode; kept for parity with the fixed-radius variant
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % SINR communications model parameters. With isotropic links and only
+            % two agents (no interference) the link is noise-limited:
+            %   SINR = Ptx / ((4*pi*fc/c)^2 * d^n * kB*T*B)
+            % Pick the threshold so the SINR connectivity boundary sits at an
+            % effective range of ~4 m, mirroring the commsRange used by the
+            % fixed-radius variant of this test (so the two agents still cannot
+            % reach their objectives without breaking connectivity).
+            useSinrComms     = true;
+            txPower          = [0.01, 0.01];     % transmit power per agent (W)
+            pathLossExponent = 2.0;
+            ambientTemp      = 290;     % K
+            centerFreq       = 2.4e9;   % Hz
+            bandwidth        = 20e6;    % Hz
+
+            effRange         = 4.0;     % desired effective comms range (m)
+            K_pl   = (4 * pi * centerFreq / 3e8)^2;            % free-space path-loss reference
+            noiseW = 1.380649e-23 * ambientTemp * bandwidth;   % thermal noise (W)
+            sinrThreshold = 10 * log10(mean(txPower) / (K_pl * effRange^pathLossExponent * noiseW)); % dB
+
+            tc.agents{1}.txPower = txPower(1);
+            tc.agents{2}.txPower = txPower(2);
+
+            % Initialize the simulation in SINR comms mode
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);
+
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
+        function test_communications_constraint_SINR_threshold_3_agents(tc)
+            % No obstacles
+            % Fixed three agents initial conditions
+            % Negligible collision geometries
+            % Non-standard domain with two objectives that will try to pull the
+            % agents apart
+            tc.minDimension = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
+
+            % Initialize agent collision geometry
+            tc.agents = {agent; agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
+            d = [2.0, 0, 0];  % 2 m collinear spacing (above the 1 m path-loss floor)
+            geometry1 = spherical;
+            geometry2 = geometry1;
+            geometry3 = geometry1;
+            geometry1 = geometry1.initialize(tc.domain.center - d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
+            geometry2 = geometry2.initialize(tc.domain.center, tc.collisionRanges(2), REGION_TYPE.COLLISION);
+            geometry3 = geometry3.initialize(tc.domain.center + d, tc.collisionRanges(3), REGION_TYPE.COLLISION);
+
+            % Initialize agent sensor model
+            tc.sensor = sigmoidSensor;
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
+
+            % Initialize obstacles
+            tc.obstacles = {};
+
+            % Initialize agents
+            tc.maxIter = 50;
+            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode; kept for parity with the fixed-radius variant
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + d, geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+            % SINR communications model parameters. Three collinear, equally
+            % spaced agents with homogeneous transmit power form an
+            % INTERFERENCE-limited network (co-agent interference swamps thermal
+            % noise), unlike the two-agent noise-limited case. Two structural
+            % facts set the connectivity boundaries:
+            %   * A chain link (1-2 or 2-3) is limited by the MIDDLE agent, which
+            %     receives one end while the other end interferes at the SAME
+            %     range: signal = interference  ->  0 dB.
+            %   * The end-to-end (1,3) link has its transmitter at 2x the range
+            %     while the middle agent interferes at 1x:  (1/2)^n  ->  -6.02 dB.
+            % Homogeneous power is required for the symmetry: unequal power would
+            % push one chain link's middle-receiver SINR below 0 dB and break it.
+            % Placing the threshold between the two boundaries (a couple dB above
+            % the -6 dB direct link, a few dB below the 0 dB chain links) makes
+            % exactly the two adjacent links {1-2, 2-3} feasible and rejects 1-3,
+            % giving the desired 1 -> 2 -> 3 line. The +3 dB CBF padding then
+            % still lands below the 0 dB chain binding, so the QP starts feasible.
+            useSinrComms     = true;
+            txPower          = [0.1, 0.1, 0.1];   % homogeneous transmit power (W)
+            pathLossExponent = 2.0;
+            ambientTemp      = 290;     % K
+            centerFreq       = 2.4e9;   % Hz
+            bandwidth        = 20e6;    % Hz
+
+            endBindingDb  = 10 * log10(0.5^pathLossExponent);  % (1,3) link SINR: (1/2)^n = -6.02 dB
+            sinrThreshold = endBindingDb + 2.0;                % ~ -4.02 dB: 2 dB above the rejected
+                                                               % 1-3 link, ~4 dB below the 0 dB chain
+
+            tc.agents{1}.txPower = txPower(1);
+            tc.agents{2}.txPower = txPower(2);
+            tc.agents{3}.txPower = txPower(3);
+
+            % Initialize the simulation in SINR comms mode
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);
+
+            % The initial SINR topology must be exactly the 1 -> 2 -> 3 line:
+            % two links (1-2 and 2-3), with the direct 1-3 link rejected.
+            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
+                [ 1, 1, 0; ...
+                  1, 1, 1; ...
+                  0, 1, 1;]));
+
+            % Run the simulation
+            tc.testClass = tc.testClass.run();
+        end
+        function test_routing_topology_convergence_demo(tc)
+            % Demonstration of the Capacity-Aware Lesser Sink Neighbor
+            % (CALSN) topology + routing algorithm: four agents with
+            % sigmoid sensors in a cube domain converge on a single
+            % sensing objective while the communication links evolve.
+            %
+            % Agents start scattered in a quadrilateral near one corner;
+            % the objective sits in the opposite corner. Base stations are
+            % agents 1 and 4. As the fleet migrates and contracts around
+            % the objective, pairwise SINRs — and with them the Shannon
+            % capacities, base-station potentials, parent choices, and
+            % capacity-proportional flow splits — change every step, so
+            % the network-graph tile shows the maintained (dotted) and
+            % flow-carrying (solid, arrowed, flow-labelled) links
+            % re-routing dynamically. validate() enforces the connected-
+            % topology invariant on every step of the run.
+            tc.minDimension = 10; % cube domain edge length
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % Single sensing objective in the far corner
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
+
+            % Four agents scattered (non-collinear) near the (low-x, low-y)
+            % corner, co-altitude at z = 2. Base 1 trails the fleet and
+            % base 4 leads it, with the relays staggered between: relay 3
+            % starts too far from base 1 for a direct link to be worth
+            % using, so its initial basin-1 route is MULTI-HOP via relay 2
+            % and re-parents to the base directly as the fleet contracts.
+            tc.agents = {agent; agent; agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
+            positions = [1.0, 2.0, 2; ...
+                         3.0, 3.5, 2; ...
+                         5.5, 3.0, 2; ...
+                         5.0, 5.5, 2;];
+
+            % Initialize agent sensor model (sigmoid)
+            tc.sensor = sigmoidSensor;
+            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
+
+            % Initialize obstacles
+            tc.obstacles = {};
+
+            % Initialize agents: 50 steps is long enough to watch the
+            % fleet converge on the objective and settle (the gradient
+            % step decays linearly to zero over maxIter)
+            tc.maxIter = 80;
+            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode
+            for aa = 1:4
+                geometry = spherical;
+                geometry = geometry.initialize(positions(aa, :), tc.collisionRanges(aa), REGION_TYPE.COLLISION);
+                tc.agents{aa} = tc.agents{aa}.initialize(positions(aa, :), geometry, tc.sensor, tc.commsRanges(aa), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+                tc.agents{aa}.txPower = 0.1;
+            end
+
+            % SINR communications model. The threshold must sit >= 3 dB
+            % below the worst link CALSN may select, because the CBF holds
+            % every maintained link at threshold + 3 dB in BOTH directions:
+            % a link selected inside that 3 dB band is born with a violated
+            % barrier and can deadlock the QP (agents hold position, the
+            % geometry never changes, and the same link is re-selected
+            % forever). The staggered start's weakest candidate link (the
+            % long 1-4 base-to-base diagonal) sits at ~ -9 dB and the
+            % weakest link CALSN actually selects at ~ -4 dB, so -14 dB
+            % leaves the -11 dB CBF floor a comfortable margin.
+            useSinrComms       = true;
+            pathLossExponent   = 2.0;
+            ambientTemp        = 290;     % K
+            centerFreq         = 2.4e9;   % Hz
+            bandwidth          = 20e6;    % Hz
+            sinrThreshold      = -14.0;   % dB
+            useRoutingTopology = true;
+
+            % Initialize the simulation with the CALSN topology selector
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);
+
+            % Initial routing invariants: bases at zero potential, relays
+            % finite; maintained topology one connected component; both
+            % generated units delivered to the bases with nothing unrouted
+            phi = tc.testClass.routingPotentials;
+            tc.assertEqual(phi([1, 4]), [0; 0]);
+            tc.assertTrue(all(isfinite(phi([2, 3]))) && all(phi([2, 3]) > 0));
+            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
+            F = tc.testClass.routingFlows;
+            for aa = [2, 3]
+                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
+            end
+            bases = [1, 4];
+            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
+            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
+
+            % Run the simulation (per-step CALSN re-solve; validate()
+            % checks feasibility- and maintained-graph connectivity at
+            % every iteration)
+            tc.testClass = tc.testClass.run();
+
+            % Convergence: total sensing performance improved over the run
+            tc.assertGreaterThan(tc.testClass.performance(end), tc.testClass.performance(2));
+
+            % Dynamic evolution: the maintained topology changed at least
+            % once relative to the initial configuration
+            hist = tc.testClass.constraintAdjacencyHist;
+            changed = false;
+            for kk = 2:size(hist, 3)
+                if any(hist(:, :, kk) ~= hist(:, :, 1), "all")
+                    changed = true;
+                    break;
+                end
+            end
+            tc.assertTrue(changed, "Maintained topology never changed during the run");
+
+            % Final routing state is still a valid delivery tree: one
+            % connected component, conservation at the generating UAVs,
+            % both units delivered, nothing unrouted
+            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
+            F = tc.testClass.routingFlows;
+            for aa = [2, 3]
+                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
+            end
+            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
+            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
+        end
+        function test_routing_topology_convergence_demo_rfSensor(tc)
+            % Demonstration of the Capacity-Aware Lesser Sink Neighbor
+            % (CALSN) topology + routing algorithm: four agents with
+            % sigmoid sensors in a cube domain converge on a single
+            % sensing objective while the communication links evolve.
+            %
+            % Agents start scattered in a quadrilateral near one corner;
+            % the objective sits in the opposite corner. Base stations are
+            % agents 1 and 4. As the fleet migrates and contracts around
+            % the objective, pairwise SINRs — and with them the Shannon
+            % capacities, base-station potentials, parent choices, and
+            % capacity-proportional flow splits — change every step, so
+            % the network-graph tile shows the maintained (dotted) and
+            % flow-carrying (solid, arrowed, flow-labelled) links
+            % re-routing dynamically. validate() enforces the connected-
+            % topology invariant on every step of the run.
+            tc.minDimension = 10; % cube domain edge length
+            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % Single sensing objective in the far corner
+            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
+
+            % Four agents scattered (non-collinear) near the (low-x, low-y)
+            % corner, co-altitude at z = 2. Base 1 trails the fleet and
+            % base 4 leads it, with the relays staggered between: relay 3
+            % starts too far from base 1 for a direct link to be worth
+            % using, so its initial basin-1 route is MULTI-HOP via relay 2
+            % and re-parents to the base directly as the fleet contracts.
+            tc.agents = {agent; agent; agent; agent;};
+            tc.collisionRanges = .25 * ones(size(tc.agents));
+            positions = [1.0, 2.0, 2; ...
+                         3.0, 3.5, 2; ...
+                         5.5, 3.0, 2; ...
+                         5.0, 5.5, 2;];
+
+            % Initialize agent sensor model (sigmoid)
+            tc.sensor = rfSensor;
+            tilt = 0; azimuth = 0;
+            tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);
+
+            % Initialize obstacles
+            tc.obstacles = {};
+
+            % Initialize agents: 50 steps is long enough to watch the
+            % fleet converge on the objective and settle (the gradient
+            % step decays linearly to zero over maxIter)
+            tc.maxIter = 80;
+            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode
+            for aa = 1:4
+                geometry = spherical;
+                geometry = geometry.initialize(positions(aa, :), tc.collisionRanges(aa), REGION_TYPE.COLLISION);
+                tc.agents{aa} = tc.agents{aa}.initialize(positions(aa, :), geometry, tc.sensor, tc.commsRanges(aa), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+                tc.agents{aa}.txPower = 0.1;
+            end
+
+            % SINR communications model. The threshold must sit >= 3 dB
+            % below the worst link CALSN may select, because the CBF holds
+            % every maintained link at threshold + 3 dB in BOTH directions:
+            % a link selected inside that 3 dB band is born with a violated
+            % barrier and can deadlock the QP (agents hold position, the
+            % geometry never changes, and the same link is re-selected
+            % forever). The staggered start's weakest candidate link (the
+            % long 1-4 base-to-base diagonal) sits at ~ -9 dB and the
+            % weakest link CALSN actually selects at ~ -4 dB, so -14 dB
+            % leaves the -11 dB CBF floor a comfortable margin.
+            useSinrComms       = true;
+            pathLossExponent   = 2.0;
+            ambientTemp        = 290;     % K
+            centerFreq         = 2.4e9;   % Hz
+            bandwidth          = 20e6;    % Hz
+            sinrThreshold      = -14.0;   % dB
+            useRoutingTopology = true;
+
+            % Initialize the simulation with the CALSN topology selector
+            tc.optimizeSensorPointing = true;
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);
+
+            % Initial routing invariants: bases at zero potential, relays
+            % finite; maintained topology one connected component; both
+            % generated units delivered to the bases with nothing unrouted
+            phi = tc.testClass.routingPotentials;
+            tc.assertEqual(phi([1, 4]), [0; 0]);
+            tc.assertTrue(all(isfinite(phi([2, 3]))) && all(phi([2, 3]) > 0));
+            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
+            F = tc.testClass.routingFlows;
+            for aa = [2, 3]
+                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
+            end
+            bases = [1, 4];
+            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
+            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
+
+            % Run the simulation (per-step CALSN re-solve; validate()
+            % checks feasibility- and maintained-graph connectivity at
+            % every iteration)
+            tc.testClass = tc.testClass.run();
+
+            % Convergence: total sensing performance improved over the run
+            tc.assertGreaterThan(tc.testClass.performance(end), tc.testClass.performance(2));
+
+            % Dynamic evolution: the maintained topology changed at least
+            % once relative to the initial configuration
+            hist = tc.testClass.constraintAdjacencyHist;
+            changed = false;
+            for kk = 2:size(hist, 3)
+                if any(hist(:, :, kk) ~= hist(:, :, 1), "all")
+                    changed = true;
+                    break;
+                end
+            end
+            tc.assertTrue(changed, "Maintained topology never changed during the run");
+
+            % Final routing state is still a valid delivery tree: one
+            % connected component, conservation at the generating UAVs,
+            % both units delivered, nothing unrouted
+            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
+            F = tc.testClass.routingFlows;
+            for aa = [2, 3]
+                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
+            end
+            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
+            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
+        end
+        function test_communications_constraint_fixed_radius(tc)
             % No obstacles
             % Fixed two agents initial conditions
             % Negligible collision geometries

test/test_rfSensor.m

@@ -15,28 +15,30 @@ classdef test_rfSensor < matlab.unittest.TestCase
         function plot_RSS(tc)
             % Plot sensor performance with no sources of interference
             P_TX = 1e-3; % Transmit power (Watts)
+            T_0 = 300; % Temperature (K)
             BW = 20e6; % Bandwidth (Hz)
             f_c = 2e9; % Center frequency (Hz)
             G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
             beamwidthExponent = 6;
             lossExponent = 2;
 
-            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            tc.testClass = tc.testClass.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
 
             tc.testClass.plotParameters();
         end
         function plot_SNR(tc)
             % Plot sensor performance with no sources of interference
             P_TX = 1e-3; % Transmit power (Watts)
+            T_0 = 300; % Temperature (K)
             BW = 20e6; % Bandwidth (Hz)
             f_c = 2e9; % Center frequency (Hz)
             G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
             beamwidthExponent = 6;
             lossExponent = 2;
 
-            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 30, 135, lossExponent);
+            tc.testClass = tc.testClass.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 30, 135, lossExponent);
 
-            altitude = 30;
+            altitude = 20;
 
             % Boresight azimuth=135° (between +X at 90° and -Y at 180°) → hotspot at +X,-Y.
             % SNR at (5,-5) should be higher than at (5,+5).
@@ -51,13 +53,14 @@ classdef test_rfSensor < matlab.unittest.TestCase
         function plot_SINR_one_interferer(tc)
             % Plot sensor performance with no sources of interference
             P_TX = 1e-3; % Transmit power (Watts)
+            T_0 = 300; % Temperature (K)
             BW = 20e6; % Bandwidth (Hz)
             f_c = 2e9; % Center frequency (Hz)
             G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
             beamwidthExponent = 6;
             lossExponent = 2;
 
-            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            tc.testClass = tc.testClass.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
 
             altitude = 30;
             otherSensorsPos = [6, -4, -1]; % relative to main sensor
@@ -70,13 +73,14 @@ classdef test_rfSensor < matlab.unittest.TestCase
         function plot_SINR_heterogenous_interferers(tc)
             % Plot sensor performance with no sources of interference
             P_TX = 1e-3; % Transmit power (Watts)
+            T_0 = 300; % Temperature (K)
             BW = 20e6; % Bandwidth (Hz)
             f_c = 2e9; % Center frequency (Hz)
             G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
             beamwidthExponent = 6;
             lossExponent = 2;
 
-            tc.testClass = tc.testClass.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            tc.testClass = tc.testClass.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
 
             altitude = 30;
             otherSensorsPos = [6, -4, -1; -2, 6, 0]; % relative to main sensor
@@ -85,13 +89,14 @@ classdef test_rfSensor < matlab.unittest.TestCase
             otherSensors{2} = rfSensor;
 
             % Must use same center frequency and bandwidth for interference sources
-            otherSensors{1} = otherSensors{1}.initialize(10 * P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            otherSensors{1} = otherSensors{1}.initialize(10 * P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
-            otherSensors{2} = otherSensors{2}.initialize(10 * P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+            otherSensors{2} = otherSensors{2}.initialize(10 * P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
 
             tc.testClass.plotPerformance(altitude, otherSensorsPos, otherSensors);
         end
         function plot_SINR_heterogenous_interferers_efficiently(tc)
             P_TX = 1e-3;
+            T_0 = 300;
             BW = 20e6;
             f_c = 2e9;
             G_RX_dBi = 3;
@@ -100,11 +105,11 @@ classdef test_rfSensor < matlab.unittest.TestCase
             lossExponent = 2;
 
             sensor1 = rfSensor;
-            sensor1 = sensor1.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
+            sensor1 = sensor1.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
             sensor2 = rfSensor;
-            sensor2 = sensor2.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
+            sensor2 = sensor2.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
             sensor3 = rfSensor;
-            sensor3 = sensor3.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
+            sensor3 = sensor3.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
 
             pos1 = [0,  0,  altitude];
             pos2 = [6, -4,  altitude - 1];
@@ -141,6 +146,7 @@ classdef test_rfSensor < matlab.unittest.TestCase
         end
         function plot_SINR_heterogenous_interferers_3d(tc)
             P_TX = 1e-3;
+            T_0 = 300;
             BW = 20e6;
             f_c = 2e9;
             G_RX_dBi = 3;
@@ -149,11 +155,11 @@ classdef test_rfSensor < matlab.unittest.TestCase
             lossExponent = 2;
 
             sensor1 = rfSensor;
-            sensor1 = sensor1.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
+            sensor1 = sensor1.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(1), 15, 45, lossExponent);
             sensor2 = rfSensor;
-            sensor2 = sensor2.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
+            sensor2 = sensor2.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(2), 10, 150, lossExponent);
             sensor3 = rfSensor;
-            sensor3 = sensor3.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
+            sensor3 = sensor3.initialize(P_TX, T_0, BW, f_c, G_RX_dBi, beamwidthExponent(3), 20, 200, lossExponent);
 
             pos1 = [0,  0,  altitude];
             pos2 = [6, -4,  altitude - 5];

test/test_sigmoidSensor.m

@@ -57,6 +57,23 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
             % Performance at great tilt should be 0
             tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], [5, 5, 0]), 0, "AbsTol", 1e-9);
         end
+        function test_thesis_graphic(tc)
+            s = [sigmoidSensor, sigmoidSensor];
+
+            alphaDist = [2, 8];
+            betaDist = [25, 1];
+            alphaTilt = [50, 15]; % degrees
+            betaTilt = [0.2, 3];
+
+            s(1) = s(1).initialize(alphaDist(1), betaDist(1), alphaTilt(1), betaTilt(1));
+            s(2) = s(2).initialize(alphaDist(2), betaDist(2), alphaTilt(2), betaTilt(2));
+
+            % Plot
+            f = s(1).plotParameters;
+            f = s(2).plotParameters(f);
+
+            legend(["Sensor 1"; "Sensor 2"]);
+        end
     end
 
 end