new SINR threshold comms CBF constraint
This commit is contained in:
1 parent
b53f62391b
commit
efa251d747
18 files changed
+534
-66
No files matched your search
+59
-18
@@ -141,31 +141,72 @@ function [obj] = constrainMotion(obj)
|
||||
% Add communication network constraints
|
||||
hComms = NaN(nAgents, nAgents);
|
||||
hComms(logical(eye(nAgents))) = 0;
|
||||
for ii = 1:(nAgents - 1)
|
||||
for jj = (ii + 1):nAgents
|
||||
if obj.constraintAdjacencyMatrix(ii, jj)
|
||||
paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
|
||||
r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
|
||||
hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
|
||||
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)
|
||||
paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
|
||||
r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
|
||||
hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
|
||||
|
||||
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
|
||||
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
|
||||
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
|
||||
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
|
||||
|
||||
% One-step forward invariance: b = h/dt ensures h cannot
|
||||
% go negative in a single timestep (linear approximation)
|
||||
v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
|
||||
hMin = -4 * r_comms * v_max_ij * obj.timestep;
|
||||
if norm(A(kk, :)) < 1e-9
|
||||
b(kk) = 0;
|
||||
else
|
||||
b(kk) = max(hMin, hComms(ii, jj)) / obj.timestep;
|
||||
% One-step forward invariance: b = h/dt ensures h cannot
|
||||
% go negative in a single timestep (linear approximation)
|
||||
v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
|
||||
hMin = -4 * r_comms * v_max_ij * obj.timestep;
|
||||
if norm(A(kk, :)) < 1e-9
|
||||
b(kk) = 0;
|
||||
else
|
||||
b(kk) = max(hMin, hComms(ii, jj)) / obj.timestep;
|
||||
end
|
||||
|
||||
kk = kk + 1;
|
||||
end
|
||||
end
|
||||
end
|
||||
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.
|
||||
% 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)
|
||||
[sinr, grad] = obj.sinrLink(lastPositions, ii, jj); % receiver ii, transmitter jj
|
||||
hComms(ii, jj) = sinr - gammaCbf;
|
||||
|
||||
kk = kk + 1;
|
||||
% 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, :);
|
||||
end
|
||||
if norm(A(kk, :)) < 1e-9
|
||||
b(kk) = 0;
|
||||
else
|
||||
b(kk) = hComms(ii, jj) / 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));
|
||||
end
|
||||
|
||||
+20
-2
@@ -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)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
@@ -15,6 +15,12 @@ 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;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||
@@ -98,6 +104,14 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
|
||||
obj.useFixedTopology = useFixedTopology;
|
||||
obj.optimizeSensorPointing = optimizeSensorPointing;
|
||||
|
||||
% Set communications model and SINR parameters
|
||||
obj.useSinrComms = useSinrComms;
|
||||
obj.sinrThreshold = sinrThreshold;
|
||||
obj.pathLossExponent = pathLossExponent;
|
||||
obj.ambientTemp = ambientTemp;
|
||||
obj.centerFreq = centerFreq;
|
||||
obj.bandwidth = bandwidth;
|
||||
|
||||
% Compute adjacency matrix and network topology
|
||||
obj = obj.updateAdjacency();
|
||||
if obj.useFixedTopology
|
||||
@@ -126,7 +140,11 @@ 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.
|
||||
nLNAPairs = nAAPairs;
|
||||
obj.numBarriers = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
|
||||
|
||||
if coder.target('MATLAB')
|
||||
|
||||
@@ -107,6 +107,46 @@ 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
|
||||
|
||||
% ---- Build domain --------------------------------------------------------
|
||||
dom = rectangularPrism;
|
||||
dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
|
||||
@@ -134,6 +174,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 +194,7 @@ end
|
||||
% ---- Initialise simulation (plots and video disabled) --------------------
|
||||
obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
|
||||
MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false, ...
|
||||
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
|
||||
@@ -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,41 @@ 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
|
||||
|
||||
% ---- 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);
|
||||
|
||||
end
|
||||
+11
-1
@@ -21,6 +21,15 @@ 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
|
||||
artifactName = "";
|
||||
f; % main plotting tiled layout figure
|
||||
fPerf; % performance plot figure
|
||||
@@ -70,13 +79,14 @@ 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);
|
||||
[obj] = initializeFromCsv(obj, csvPath);
|
||||
[obj] = initializeFromInits(obj, initsPath);
|
||||
[obj] = plotFromSimHist(obj, initsPath, histPath);
|
||||
[obj] = run(obj);
|
||||
[obj] = lesserNeighbor(obj);
|
||||
[obj] = constrainMotion(obj);
|
||||
[sinr, grad] = sinrLink(obj, positions, rxIdx, txIdx);
|
||||
[obj] = partition(obj);
|
||||
[obj] = updateAdjacency(obj);
|
||||
[obj] = plot(obj);
|
||||
|
||||
+36
-12
@@ -9,20 +9,44 @@ function obj = plotGraph(obj)
|
||||
% Form graph from adjacency matrix
|
||||
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");
|
||||
o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
|
||||
hold(obj.f.CurrentAxes, "off");
|
||||
ax = obj.f.CurrentAxes;
|
||||
else
|
||||
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "on");
|
||||
o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
|
||||
hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
|
||||
if size(obj.networkGraphIndex, 2) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
|
||||
end
|
||||
ax = obj.f.Children(1).Children(obj.networkGraphIndex(1));
|
||||
end
|
||||
|
||||
% Plot graph object
|
||||
hold(ax, "on");
|
||||
o = plot(ax, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
|
||||
hold(ax, "off");
|
||||
if ~isnan(obj.networkGraphIndex) && size(obj.networkGraphIndex, 2) > 1
|
||||
for ii = 2:size(obj.networkGraphIndex, 2)
|
||||
o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
|
||||
end
|
||||
end
|
||||
|
||||
% In SINR comms mode, label each edge with the connection's SINR (dB) and
|
||||
% report the connectivity threshold as a subtitle on the tile. The
|
||||
% fixed-radius model is left unchanged (no edge labels, no subtitle).
|
||||
if obj.useSinrComms
|
||||
endNodes = G.Edges.EndNodes; % each row [lo hi]; graph stores lo < hi
|
||||
nAgents = size(obj.agents, 1);
|
||||
positions = zeros(nAgents, 3);
|
||||
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.
|
||||
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 dB", 10 * log10(sinrLin));
|
||||
end
|
||||
o(1).EdgeLabel = edgeLabels;
|
||||
subtitle(ax, sprintf("SINR threshold: %.1f dB", obj.sinrThreshold));
|
||||
end
|
||||
|
||||
obj.graphPlot = o;
|
||||
end
|
||||
end
|
||||
@@ -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
|
||||
@@ -6,16 +6,38 @@ function obj = updateAdjacency(obj)
|
||||
obj (1, 1) {mustBeA(obj, "miSim")};
|
||||
end
|
||||
|
||||
% Initialize assuming only self-connections
|
||||
A = true(size(obj.agents, 1));
|
||||
nAgents = size(obj.agents, 1);
|
||||
|
||||
% Check lower triangle off-diagonal connections
|
||||
for ii = 2:size(A, 1)
|
||||
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;
|
||||
% Initialize assuming all agents are connected
|
||||
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)
|
||||
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: 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.
|
||||
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)
|
||||
sinr = obj.sinrLink(positions, jj, ii); % receiver jj, transmitter ii
|
||||
if sinr < gammaLin
|
||||
A(ii, jj) = false; % SINR below threshold
|
||||
continue;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
@@ -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,8 @@ 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
|
||||
"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
|
||||
|
||||
Reference in new issue
Block a user