From efa251d7476cc0e20c34c12a242abead53317e86 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Fri, 26 Jun 2026 22:13:51 -0700 Subject: [PATCH] new SINR threshold comms CBF constraint --- @agent/agent.m | 5 +- @agent/initialize.m | 4 +- @miSim/constrainMotion.m | 77 +++++++++++++++++------ @miSim/initialize.m | 22 ++++++- @miSim/initializeFromCsv.m | 44 +++++++++++++- @miSim/initializeFromInits.m | 36 ++++++++++- @miSim/miSim.m | 12 +++- @miSim/plotGraph.m | 48 +++++++++++---- @miSim/sinrLink.m | 101 +++++++++++++++++++++++++++++++ @miSim/updateAdjacency.m | 40 +++++++++--- @miSim/writeInits.m | 3 + aerpaw/config/scenario.csv | 4 +- aerpaw/controller.m | 2 +- aerpaw/guidance_step.m | 41 ++++++++++--- aerpaw/impl/controller_impl.cpp | 67 +++++++++++++++++++- aerpaw/impl/controller_impl.h | 9 ++- aerpaw/results/resultsAnalysis.m | 20 +++++- test/test_miSim.m | 65 +++++++++++++++++++- 18 files changed, 534 insertions(+), 66 deletions(-) create mode 100644 @miSim/sinrLink.m diff --git a/@agent/agent.m b/@agent/agent.m index 6945d1c..f16a323 100644 --- a/@agent/agent.m +++ b/@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); diff --git a/@agent/initialize.m b/@agent/initialize.m index a5da81d..24a36cb 100644 --- a/@agent/initialize.m +++ b/@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; diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 5555782..75a132b 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -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 diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 91c5fe6..96c4eb7 100644 --- a/@miSim/initialize.m +++ b/@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) 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') diff --git a/@miSim/initializeFromCsv.m b/@miSim/initializeFromCsv.m index f5c5857..310ddf7 100644 --- a/@miSim/initializeFromCsv.m +++ b/@miSim/initializeFromCsv.m @@ -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 diff --git a/@miSim/initializeFromInits.m b/@miSim/initializeFromInits.m index 792ec27..ff3300d 100644 --- a/@miSim/initializeFromInits.m +++ b/@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,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 diff --git a/@miSim/miSim.m b/@miSim/miSim.m index e9b8f6b..3002e9f 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -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); diff --git a/@miSim/plotGraph.m b/@miSim/plotGraph.m index 49a2ac6..6fb5fea 100644 --- a/@miSim/plotGraph.m +++ b/@miSim/plotGraph.m @@ -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 \ No newline at end of file +end diff --git a/@miSim/sinrLink.m b/@miSim/sinrLink.m new file mode 100644 index 0000000..8e58d30 --- /dev/null +++ b/@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 diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index df463f0..9643d2e 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -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 diff --git a/@miSim/writeInits.m b/@miSim/writeInits.m index cb0ddcb..28a9e96 100644 --- a/@miSim/writeInits.m +++ b/@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,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 diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index 4f685c0..b17a01b 100644 --- a/aerpaw/config/scenario.csv +++ b/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 -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 \ No newline at end of file +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, 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 \ No newline at end of file diff --git a/aerpaw/controller.m b/aerpaw/controller.m index da202a4..12db28c 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -50,7 +50,7 @@ else 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); diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m index f674c1c..cdc2aa7 100644 --- a/aerpaw/guidance_step.m +++ b/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 -% 41-44 objectiveVar (2x2, col-major) -% 45 sensorPerformanceMinimum -% 46 useDoubleIntegrator -% 47 dampingCoeff -% 48 useFixedTopology +% 39 numObjectiveComponents +% 40-43 objectivePos [x1,y1,x2,y2] +% 44-51 objectiveVar [per component] +% 52 sensorPerformanceMinimum +% 53 useDoubleIntegrator +% 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 diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index 2cf0124..0d15213 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/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]; - int nf = splitCSVRow(copy, fields, 32); + char* fields[40]; + 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; } diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index b569e98..741f722 100644 --- a/aerpaw/impl/controller_impl.h +++ b/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 diff --git a/aerpaw/results/resultsAnalysis.m b/aerpaw/results/resultsAnalysis.m index 6eb4570..67d98d9 100644 --- a/aerpaw/results/resultsAnalysis.m +++ b/aerpaw/results/resultsAnalysis.m @@ -45,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 @@ -54,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(); diff --git a/test/test_miSim.m b/test/test_miSim.m index 28989a8..ddb84bd 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -815,7 +815,7 @@ 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 @@ -843,6 +843,67 @@ classdef test_miSim < matlab.unittest.TestCase % 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.1; % 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 + + tc.agents{1}.txPower = txPower; + tc.agents{2}.txPower = txPower; + + % 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_fixed_radius(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)); % defined such that they cannot reach their objective without breaking connectivity @@ -851,7 +912,7 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize the simulation 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); - + % Run the simulation tc.testClass = tc.testClass.run(); end