diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 75a132b..84eacde 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -169,11 +169,11 @@ function [obj] = constrainMotion(obj) end end else - % SINR comms: keep the SINR of each maintained link above threshold via - % the barrier h = SINR - gammaLin. Per the connectivity convention the - % lower-index agent (ii) receives the higher-index transmitter (jj). The - % gradient couples the receiver, transmitter, and every interferer, so - % the QP row is dense across all agent blocks. + % 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 @@ -188,19 +188,33 @@ function [obj] = constrainMotion(obj) for ii = 1:(nAgents - 1) for jj = (ii + 1):nAgents if obj.constraintAdjacencyMatrix(ii, jj) - [sinr, grad] = obj.sinrLink(lastPositions, ii, jj); % receiver ii, transmitter jj - hComms(ii, jj) = sinr - gammaCbf; - + % 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)) = -grad(aa, :); + 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 @@ -208,7 +222,15 @@ function [obj] = constrainMotion(obj) 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. diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 96c4eb7..e51cb75 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -143,8 +143,13 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m % 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. - nLNAPairs = nAAPairs; + % 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') diff --git a/@miSim/plotGraph.m b/@miSim/plotGraph.m index 44c6933..50eaf49 100644 --- a/@miSim/plotGraph.m +++ b/@miSim/plotGraph.m @@ -36,15 +36,19 @@ function obj = plotGraph(obj) for kk = 1:nAgents positions(kk, :) = obj.agents{kk}.pos; end - % Directional convention (matches updateAdjacency/constrainMotion): - % the lower-index node is the receiver, the higher-index node the - % transmitter, with interference from all other agents at the receiver. - % Labels are bare numbers (no units) so they stay compact; GraphPlot - % renders them horizontally, left-to-right. + % A link is feasible only if BOTH directions clear the threshold, so + % label each edge with the binding (worse) directional SINR: the min of + % the lower-index-receiver and higher-index-receiver links, with + % interference from all other agents at each receiver. Labels are bare + % numbers (no units) so they stay compact; GraphPlot renders them + % horizontally, left-to-right. edgeLabels = strings(size(endNodes, 1), 1); for ee = 1:size(endNodes, 1) - sinrLin = obj.sinrLink(positions, endNodes(ee, 1), endNodes(ee, 2)); - edgeLabels(ee) = sprintf("%.1f", 10 * log10(sinrLin)); + lo = endNodes(ee, 1); + hi = endNodes(ee, 2); + 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 o(1).EdgeLabel = edgeLabels; % Threshold goes in the existing single-line title (not a subtitle) so diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index 9643d2e..1e47ba1 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -23,9 +23,11 @@ function obj = updateAdjacency(obj) end end else - % SINR model: pair (ii, jj) connected iff the lower-index agent (jj) - % receives the higher-index agent's (ii) signal above the SINR - % threshold, with interference from all other agents at the receiver. + % 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 @@ -33,9 +35,10 @@ function obj = updateAdjacency(obj) end for ii = 2:nAgents for jj = 1:(ii - 1) - sinr = obj.sinrLink(positions, jj, ii); % receiver jj, transmitter ii - if sinr < gammaLin - A(ii, jj) = false; % SINR below threshold + 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 diff --git a/test/test_miSim.m b/test/test_miSim.m index ddb84bd..05af713 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -857,18 +857,19 @@ classdef test_miSim < matlab.unittest.TestCase % fixed-radius variant of this test (so the two agents still cannot % reach their objectives without breaking connectivity). useSinrComms = true; - txPower = 0.1; % transmit power per agent (W) + 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(txPower / (K_pl * effRange^pathLossExponent * noiseW)); % dB + sinrThreshold = 10 * log10(mean(txPower) / (K_pl * effRange^pathLossExponent * noiseW)); % dB - tc.agents{1}.txPower = txPower; - tc.agents{2}.txPower = txPower; + 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);