diff --git a/@agent/agent.m b/@agent/agent.m
index 1fc1ecd..e8db7ed 100644
--- a/@agent/agent.m
+++ b/@agent/agent.m
@@ -32,7 +32,9 @@ classdef agent
properties (SetAccess = private, GetAccess = public)
initialStepSize = NaN;
+ initialMaxAngleStepSize = NaN;
stepDecayRate = NaN;
+ angleStepDecayRate = NaN;
end
methods (Access = public)
diff --git a/@agent/initialize.m b/@agent/initialize.m
index 47c3e98..a5da81d 100644
--- a/@agent/initialize.m
+++ b/@agent/initialize.m
@@ -1,4 +1,4 @@
-function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
arguments (Input)
obj (1, 1) {mustBeA(obj, "agent")};
pos (1, 3) double;
@@ -7,6 +7,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
comRange (1, 1) double;
maxIter (1, 1) double;
initialStepSize (1, 1) double = 0.2;
+ initialMaxAngleStepSize (1, 1) double = 5.0;
label (1, 1) string = "";
plotCommsGeometry (1, 1) logical = false;
end
@@ -23,7 +24,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
obj.label = label;
obj.plotCommsGeometry = plotCommsGeometry;
obj.initialStepSize = initialStepSize;
+ obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
obj.stepDecayRate = obj.initialStepSize / maxIter;
+ obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
% Initialize performance vector
if coder.target('MATLAB')
diff --git a/@agent/run.m b/@agent/run.m
index 340b4cf..5852520 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -1,14 +1,14 @@
-function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
+function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing)
arguments (Input)
obj (1, 1) {mustBeA(obj, "agent")};
domain (1, 1) {mustBeGeometry};
partitioning (:, :) double;
timestepIndex (1, 1) double;
index (1, 1) double;
- agents (:, 1) {mustBeA(agents, "cell")};
useDoubleIntegrator (1, 1) logical = false;
dampingCoeff (1, 1) double = 2.0;
dt (1, 1) double = 1.0;
+ optimizeSensorPointing (1, 1) logical = false;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, "agent")};
@@ -33,34 +33,32 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
maskedX = domain.objective.X(partitionMask);
maskedY = domain.objective.Y(partitionMask);
- % Compute agent performance at the current position and each delta position +/- X, Y, Z
- delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
- deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
- C_delta = NaN(7, 1); % agent performance at delta steps in each direction
- for ii = 1:7
+ if optimizeSensorPointing
+ % Stash actual current sensor model tilt/azimuth before messing with it
+ % in these following hypotheticals
+ tilt = obj.sensorModel.tilt;
+ azimuth = obj.sensorModel.azimuth;
+ end
+
+ % Compute agent performance at the current position and each delta position +/- X, Y, Z, tilt, azimuth
+ deltaPos = domain.objective.discretizationStep; % smallest possible step size that gets different results
+ if optimizeSensorPointing
+ deltaAngle = atan2d(domain.objective.discretizationStep, obj.pos(3)); % smallest possible angle derived from smallest possible step size and current height
+ end
+ deltaApplicator = [0, 0, 0, 0, 0; 1, 0, 0, 0, 0; -1, 0, 0, 0, 0; 0, 1, 0, 0, 0; 0, -1, 0, 0, 0; 0, 0, 1, 0, 0; 0, 0, -1, 0, 0; 0, 0, 0, 1, 0; 0, 0, 0, -1, 0; 0, 0, 0, 0, 1; 0, 0, 0, 0, -1;]; % none, +X, -X, +Y, -Y, +Z, -Z, +tilt, -tilt, +azimuth, -azimuth
+ C_delta = NaN(size(deltaApplicator, 1), 1); % agent performance at delta steps in each direction
+ for ii = 1:size(deltaApplicator, 1)
+ if ~optimizeSensorPointing && ii > 7; break; end
% Apply delta to position
- pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+ pos = obj.pos + deltaPos * deltaApplicator(ii, 1:3);
+ if optimizeSensorPointing
+ % Apply delta to tilt and azimuth
+ obj.sensorModel.tilt = tilt + deltaAngle * deltaApplicator(ii, 4);
+ obj.sensorModel.azimuth = azimuth + deltaAngle * deltaApplicator(ii, 5);
+ end
% Compute performance values on partition
- if ii < 6
- % Compute sensing performance
- sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
- % Objective performance does not change for 0, +/- X, +/- Y steps.
- % Those values are computed once before the loop and are only
- % recomputed when +/- Z steps are applied
- else
- % Redo partitioning for Z stepping only
- partitioning = obj.partition(agents, domain.objective);
-
- % Recompute partiton-derived performance values for objective
- partitionMask = partitioning == index;
- objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
-
- % Recompute partiton-derived performance values for sensing
- maskedX = domain.objective.X(partitionMask);
- maskedY = domain.objective.Y(partitionMask);
- sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
- end
+ sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
% Rearrange data into image arrays
F = NaN(size(partitionMask));
@@ -73,37 +71,54 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
C_delta(ii) = sum(C(~isnan(C)));
end
+ if optimizeSensorPointing
+ % Reset sensor model to actual tilt and azimuth angles
+ obj.sensorModel.tilt = tilt;
+ obj.sensorModel.azimuth = azimuth;
+ end
+
% Store agent performance at current time and place
if coder.target('MATLAB')
obj.performance(timestepIndex + 1) = C_delta(1);
end
% Compute gradient by finite central differences
- gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
+ gradC = [(C_delta(2)-C_delta(3))/(2*deltaPos), (C_delta(4)-C_delta(5))/(2*deltaPos), (C_delta(6)-C_delta(7))/(2*deltaPos)];
+ if optimizeSensorPointing
+ gradC(4) = (C_delta(8) -C_delta(9)) /(2*deltaAngle);
+ gradC(5) = (C_delta(10)-C_delta(11))/(2*deltaAngle);
+ end
% Compute scaling factor
- targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
- gradNorm = norm(gradC);
+ targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+ gradPosNorm = norm(gradC(1:3));
% Compute unconstrained next state
if useDoubleIntegrator
% Double-integrator: gradient produces desired acceleration with damping
- if gradNorm < 1e-100
- a_gradient = zeros(1, 3);
+ if gradPosNorm < 1e-100
+ a_gradient = zeros(1, 5);
else
% Scale so steady-state step ≈ targetRate (matching SI behavior)
- a_gradient = (targetRate * dampingCoeff / (gradNorm * dt)) * gradC;
+ a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
end
% Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
- obj.vel = (obj.vel + a_gradient * dt) / (1 + dampingCoeff * dt);
+ obj.vel = (obj.vel + a_gradient(1:3) * dt) / (1 + dampingCoeff * dt);
obj.pos = obj.lastPos + obj.vel * dt;
else
% Single-integrator: gradient directly sets position step
- if gradNorm >= 1e-100
- obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
+ if gradPosNorm >= 1e-100
+ obj.pos = obj.pos + (targetPosRate / gradPosNorm) * gradC(1:3);
end
end
+ % Update tilt and azimuth, saturating at the decaying maximum allowed step size
+ if optimizeSensorPointing
+ maxAngleStep = obj.initialMaxAngleStepSize - obj.angleStepDecayRate * timestepIndex;
+ obj.sensorModel.tilt = obj.sensorModel.tilt + sign(gradC(4)) * min(abs(gradC(4)), maxAngleStep);
+ obj.sensorModel.azimuth = obj.sensorModel.azimuth + sign(gradC(5)) * min(abs(gradC(5)), maxAngleStep);
+ end
+
% Reinitialize collision geometry in the new position
d = obj.pos - obj.collisionGeometry.center;
if isa(obj.collisionGeometry, "rectangularPrism")
diff --git a/@agent/updatePlots.m b/@agent/updatePlots.m
index 6548a43..b0b8b7f 100644
--- a/@agent/updatePlots.m
+++ b/@agent/updatePlots.m
@@ -7,45 +7,68 @@ function updatePlots(obj)
% Find change in agent position since last timestep
deltaPos = obj.pos - obj.lastPos;
- if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
- % Agent did not move, so nothing has to move on the plots
+ posChanged = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
+ orientChanged = obj.sensorModel.tilt ~= obj.fovGeometry.tilt || ...
+ obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
+
+ if ~posChanged && ~orientChanged
return;
end
- % Scatterplot point positions
- for ii = 1:size(obj.scatterPoints, 1)
- obj.scatterPoints(ii).XData = obj.pos(1);
- obj.scatterPoints(ii).YData = obj.pos(2);
- obj.scatterPoints(ii).ZData = obj.pos(3);
- end
-
- % Collision geometry edges
- for jj = 1:size(obj.collisionGeometry.lines, 2)
- % Update plotting
- for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
- obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
- obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
- obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
+ if posChanged
+ % Scatterplot point positions
+ for ii = 1:size(obj.scatterPoints, 1)
+ obj.scatterPoints(ii).XData = obj.pos(1);
+ obj.scatterPoints(ii).YData = obj.pos(2);
+ obj.scatterPoints(ii).ZData = obj.pos(3);
end
- end
- % Communications geometry edges
- if obj.plotCommsGeometry
- for jj = 1:size(obj.commsGeometry.lines, 2)
+ % Collision geometry edges
+ for jj = 1:size(obj.collisionGeometry.lines, 2)
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
+
+ % Communications geometry edges
+ if obj.plotCommsGeometry
+ for jj = 1:size(obj.commsGeometry.lines, 2)
+ for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
+ obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
+ obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
+ obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
+ end
+ end
+ end
end
- % Update FOV geometry surfaces
- for jj = 1:size(obj.fovGeometry.surface, 2)
- % Update each plot
- % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
- obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
- obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
- obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
+ % FOV cone: recompute full mesh whenever position or orientation changes
+ if ~isempty(obj.fovGeometry.surface)
+ % Sync fovGeometry state to current agent position and sensor orientation
+ obj.fovGeometry = obj.fovGeometry.initialize( ...
+ obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
+ obj.fovGeometry.tag, obj.fovGeometry.label, ...
+ obj.sensorModel.tilt, obj.sensorModel.azimuth);
+
+ % Recompute cone mesh (mirrors cone.plot logic)
+ maxAlt = obj.fovGeometry.surface(1).Parent.ZLim(2);
+ scalingFactor = maxAlt / obj.fovGeometry.height;
+ [X, Y, Z] = cylinder([scalingFactor * obj.fovGeometry.radius, 0], obj.fovGeometry.n);
+ Z = Z * maxAlt;
+ Ry = [cosd(obj.fovGeometry.tilt), 0, -sind(obj.fovGeometry.tilt); 0, 1, 0; sind(obj.fovGeometry.tilt), 0, cosd(obj.fovGeometry.tilt)];
+ Rz = [sind(obj.fovGeometry.azimuth), -cosd(obj.fovGeometry.azimuth), 0; cosd(obj.fovGeometry.azimuth), sind(obj.fovGeometry.azimuth), 0; 0, 0, 1];
+ R = Rz * Ry;
+ pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
+ X = reshape(pts(1, :), size(X)) + obj.pos(1);
+ Y = reshape(pts(2, :), size(Y)) + obj.pos(2);
+ Z = reshape(pts(3, :) + maxAlt, size(Z)) + obj.pos(3) - maxAlt;
+
+ for jj = 1:size(obj.fovGeometry.surface, 2)
+ obj.fovGeometry.surface(jj).XData = X;
+ obj.fovGeometry.surface(jj).YData = Y;
+ obj.fovGeometry.surface(jj).ZData = Z;
+ end
end
-end
\ No newline at end of file
+end
diff --git a/@miSim/initialize.m b/@miSim/initialize.m
index 08f045f..060bd5f 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)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo, useDoubleIntegrator, dampingCoeff, useFixedTopology, optimizeSensorPointing)
arguments (Input)
obj (1, 1) {mustBeA(obj, "miSim")};
domain (1, 1) {mustBeGeometry};
@@ -14,6 +14,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
useDoubleIntegrator (1, 1) logical = false;
dampingCoeff (1, 1) double = 2.0;
useFixedTopology (1, 1) logical = false;
+ optimizeSensorPointing (1, 1) logical = false;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, "miSim")};
@@ -93,6 +94,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
obj.useDoubleIntegrator = useDoubleIntegrator;
obj.dampingCoeff = dampingCoeff;
obj.useFixedTopology = useFixedTopology;
+ obj.optimizeSensorPointing = optimizeSensorPointing;
% Compute adjacency matrix and network topology
obj = obj.updateAdjacency();
diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index 627ae4e..e9b8f6b 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -20,6 +20,7 @@ classdef miSim
useDoubleIntegrator = false; % false = single-integrator, true = double-integrator dynamics
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
artifactName = "";
f; % main plotting tiled layout figure
fPerf; % performance plot figure
diff --git a/@miSim/run.m b/@miSim/run.m
index 3427ae9..40ab031 100644
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -42,7 +42,7 @@ function [obj] = run(obj)
% Moving
% Iterate over agents to simulate their unconstrained motion
for jj = 1:size(obj.agents, 1)
- obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep);
+ obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.useDoubleIntegrator, obj.dampingCoeff, obj.timestep, obj.optimizeSensorPointing);
end
% Adjust motion determined by unconstrained gradient ascent using
diff --git a/@rfSensor/rfSensor.m b/@rfSensor/rfSensor.m
index 8cd2131..8417303 100644
--- a/@rfSensor/rfSensor.m
+++ b/@rfSensor/rfSensor.m
@@ -10,8 +10,6 @@ classdef rfSensor
BW = NaN; % Bandwidth (Hz)
f_c = NaN; % Center frequency (Hz)
G_RX_dBi = NaN; % Receiver antenna gain
- tilt = NaN; % Antenna boresight tilt (deg): 0=nadir, 90=horizon
- azimuth = NaN; % Antenna boresight azimuth (deg): 0=+y, 90=+x, 180=-y, 270=-x
beamwidthExponent = NaN; % Antenna beamwidth exponent for cosine radiation pattern, larger exponent -> narrower beam
% Values computed at initialization
P_TX_dBm = NaN; % Transmit power (dBm)
@@ -19,6 +17,10 @@ classdef rfSensor
% Cached state (per timestep)
rssCache (:,1) double = double.empty(0,1); % linear-scale RSS to last ground targets grid
end
+ properties (Access = public)
+ tilt = NaN; % Antenna boresight tilt (deg): 0=nadir, 90=horizon
+ azimuth = NaN; % Antenna boresight azimuth (deg): 0=+y, 90=+x, 180=-y, 270=-x
+ end
methods (Access = public)
[obj] = initialize(obj, txPower, bandwidth, centerFreq, rxGain, beamwidthExponent, tilt, azimuth); % initialize sensor, define parameters
diff --git a/@sensingObjective/plot.m b/@sensingObjective/plot.m
index ee5ed39..baf31b3 100644
--- a/@sensingObjective/plot.m
+++ b/@sensingObjective/plot.m
@@ -11,19 +11,26 @@ function f = plot(obj, ind, f)
% Create axes if they don't already exist
f = firstPlotSetup(f);
+ normalized = obj.values ./ sum(obj.values, "all");
+ cRange = [min(normalized, [], "all"), max(normalized, [], "all")];
+
% Plot gradient on the "floor" of the domain
if isnan(ind)
- hold(f.CurrentAxes, "on");
- o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+ ax = f.CurrentAxes;
+ hold(ax, "on");
+ o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
o.HitTest = "off";
o.PickableParts = "none";
- hold(f.CurrentAxes, "off");
+ clim(ax, cRange);
+ hold(ax, "off");
else
- hold(f.Children(1).Children(ind(1)), "on");
- o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ sum(obj.values, "all"), "EdgeColor", "none");
+ ax = f.Children(1).Children(ind(1));
+ hold(ax, "on");
+ o = surf(ax, obj.X, obj.Y, zeros(size(obj.X)), normalized, "EdgeColor", "none");
o.HitTest = "off";
o.PickableParts = "none";
- hold(f.Children(1).Children(ind(1)), "off");
+ clim(ax, cRange);
+ hold(ax, "off");
end
% Add to other perspectives
diff --git a/@sigmoidSensor/sigmoidSensor.m b/@sigmoidSensor/sigmoidSensor.m
index 8c7879b..a6fac33 100644
--- a/@sigmoidSensor/sigmoidSensor.m
+++ b/@sigmoidSensor/sigmoidSensor.m
@@ -5,8 +5,9 @@ classdef sigmoidSensor
betaDist = NaN;
alphaTilt = NaN; % degrees
betaTilt = NaN;
-
- % pointing parameters
+ end
+ properties (Access = public)
+ % pointing states
tilt = 0;
azimuth = 0;
end
diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m
index 25f34df..757d0d5 100644
--- a/aerpaw/guidance_step.m
+++ b/aerpaw/guidance_step.m
@@ -193,8 +193,8 @@ else
% 5. Unconstrained gradient-ascent step for each agent
for ii = 1:size(sim.agents, 1)
sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
- sim.timestepIndex, ii, sim.agents, ...
- sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep);
+ sim.timestepIndex, ii, ...
+ sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
end
% 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
diff --git a/aerpaw/results/controllerAnalysis.m b/aerpaw/results/controllerAnalysis.m
new file mode 100644
index 0000000..c4fd98c
--- /dev/null
+++ b/aerpaw/results/controllerAnalysis.m
@@ -0,0 +1,28 @@
+function controller = controllerAnalysis(resultsPath)
+ arguments (Input)
+ resultsPath (1, 1) string;
+ end
+ arguments (Output)
+ controller table;
+ end
+
+ % Measure intervals between issuing commands from the controller
+ % (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
+ r = dir(resultsPath);
+ controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
+ controllerPath = dir(controllerPath);
+ controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
+ controller = readControllerLogs(controllerPath);
+ rpIdx = startsWith(controller.message, "Iteration duration: ");
+ s = split(controller.message(rpIdx), "Iteration duration: ");
+ s = split(s(:, 2), ' s');
+ s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
+ s.Format = "mm:ss.SSS";
+ fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
+ fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
+ fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
+ fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
+ if seconds(min(s)) < 4
+ warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
+ end
+end
\ No newline at end of file
diff --git a/aerpaw/results/resultsAnalysis.m b/aerpaw/results/resultsAnalysis.m
index 2960bd1..a76820c 100644
--- a/aerpaw/results/resultsAnalysis.m
+++ b/aerpaw/results/resultsAnalysis.m
@@ -1,25 +1,8 @@
%% Plot AERPAW logs (trajectory, radio)
resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
-% Measure intervals between issuing commands from the controller
-% (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
-r = dir(resultsPath);
-controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(startsWith({r.name}, 'controller_')).name);
-controllerPath = dir(controllerPath);
-controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
-controller = readControllerLogs(controllerPath);
-rpIdx = startsWith(controller.message, "Iteration duration: ");
-s = split(controller.message(rpIdx), "Iteration duration: ");
-s = split(s(:, 2), ' s');
-s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
-s.Format = "mm:ss.SSS";
-fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
-fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
-fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
-fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
-if seconds(min(s)) < 4
- warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
-end
+% Check timeline in controller logs
+controller = controllerAnalysis(resultsPath);
% Plot GPS logged data and scenario information (domain, objective, obstacles)
seaToGroundLevel = 110; % measured approximately from USGS national map viewer
@@ -54,7 +37,7 @@ for ii = 1:size(agents, 1)
collisionGeometry = spherical;
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, sprintf("Agent %d", ii), plotCommsGeometry);
+ 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);
end
% Create obstacles
@@ -81,9 +64,12 @@ copyobj(sim.f.Children, comparison);
% Plot trajectories on top
for ii = 1:size(G, 1)
+ gpsTimes = G{ii}.Timestamp;
+ gpsTimes.TimeZone = '';
+ inRange = gpsTimes >= controller.timestamp(1) & gpsTimes <= controller.timestamp(end);
for jj = 1:size(sim.spatialPlotIndices, 2)
hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "on");
- plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East, G{ii}.North, G{ii}.Up + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
+ plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East(inRange), G{ii}.North(inRange), G{ii}.Up(inRange) + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "off");
end
end
\ No newline at end of file
diff --git a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkd.xml b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkd.xml
new file mode 100644
index 0000000..99772b4
--- /dev/null
+++ b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkd.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkp.xml b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkp.xml
new file mode 100644
index 0000000..ae0264a
--- /dev/null
+++ b/resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkp.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m
index 273030c..5db14a8 100644
--- a/test/parametricTestSuite.m
+++ b/test/parametricTestSuite.m
@@ -47,7 +47,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
collisionGeometry = spherical;
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, sprintf("Agent %d", ii), tc.plotCommsGeometry);
+ 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), tc.plotCommsGeometry);
end
% Create obstacles
@@ -106,7 +106,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
% Initialize agent
collisionGeometry = collisionGeometry.initialize(agentPos, params.collisionRadius(ii, 1), REGION_TYPE.COLLISION, "Agent 1 Collision Region");
- agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, 1), params.maxIter(ii), params.initialStepSize(ii), "Agent 1", tc.plotCommsGeometry);
+ agents{1} = agents{1}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, 1), params.maxIter(ii), params.initialStepSize(ii), 5.0, "Agent 1", tc.plotCommsGeometry);
% Set up remaining agents in random (valid) locations
for jj = 2:size(agents, 1)
@@ -148,7 +148,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
% Initialize agent
collisionGeometry = collisionGeometry.initialize(agentPos, params.collisionRadius(ii, jj), REGION_TYPE.COLLISION, sprintf("Agent %d Collision Region", jj));
- agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, jj), params.maxIter(ii), params.initialStepSize(ii), sprintf("Agent %d", jj), tc.plotCommsGeometry);
+ agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, jj), params.maxIter(ii), params.initialStepSize(ii), 5.0, sprintf("Agent %d", jj), tc.plotCommsGeometry);
end
% randomly shuffle agents to make the network more interesting (probably)
diff --git a/test/test_miSim.m b/test/test_miSim.m
index 22bd52b..e36d117 100644
--- a/test/test_miSim.m
+++ b/test/test_miSim.m
@@ -31,6 +31,7 @@ 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 = 5; % angular step size (degrees) for tilt/azimuth gradient ascent per timestep.
minAgents = 3; % Minimum number of agents to be randomly generated
maxAgents = 4; % Maximum number of agents to be randomly generated
useDoubleIntegrator = false;
@@ -55,6 +56,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Communications
useFixedTopology = false;
+ optimizeSensorPointing = false;
minCommsRange = 3; % Minimum randomly generated collision geometry size
maxCommsRange = 5; % Maximum randomly generated collision geometry size
commsRanges = NaN;
@@ -173,7 +175,7 @@ classdef test_miSim < matlab.unittest.TestCase
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
% Initialize candidate agent
- newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+ newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Make sure candidate agent doesn't collide with
% domain
@@ -227,7 +229,7 @@ classdef test_miSim < matlab.unittest.TestCase
end
% 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.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
function miSim_run(tc)
% randomly create obstacles
@@ -312,7 +314,7 @@ classdef test_miSim < matlab.unittest.TestCase
tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
% Initialize candidate agent
- newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+ newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Make sure candidate agent doesn't collide with
% domain
@@ -366,7 +368,7 @@ classdef test_miSim < matlab.unittest.TestCase
end
% 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.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);
% Write out initialization state
tc.testClass.writeInits();
@@ -392,15 +394,15 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.commsRanges = 3 * d * ones(size(tc.agents));
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
- tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.obstacles = cell(0, 1);
tc.makePlots = false;
tc.makeVideo = false;
- 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.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);
centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
@@ -419,13 +421,13 @@ classdef test_miSim < matlab.unittest.TestCase
tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
% Initialize agents
- tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.obstacles = cell(0, 1);
tc.makePlots = false;
tc.makeVideo = false;
- 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.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);
close(tc.testClass.fPerf);
tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
@@ -449,11 +451,11 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 75;
- tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.obstacles = cell(0, 1);
- 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.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();
@@ -476,11 +478,11 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 75;
- tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.obstacles = cell(0, 1);
- 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.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();
@@ -504,18 +506,86 @@ classdef test_miSim < matlab.unittest.TestCase
BW = 20e6; % Bandwidth (Hz)
f_c = 2e9; % Center frequency (Hz)
G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+ beamwidthExponent = 6;
+ lossExponent = 2;
tc.sensor = rfSensor;
- tc.sensor = tc.sensor.initialize(P_TX, BW, f_c, G_RX_dBi, 45, 45);
+ tc.sensor = tc.sensor.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 45, 45, lossExponent);
% Initialize agents
tc.maxIter = 75;
- tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.obstacles = cell(0, 1);
tc.minAlt = 0.5;
- 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.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
+ function test_single_agent_gradient_ascent_sensor_pointing(tc)
+ % make basic domain
+ 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([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, 1e-6, [7, 6]);
+
+ % Initialize agent collision geometry
+ tc.agents = {agent};
+ geometry1 = spherical;
+ geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+
+ % Initialize agent sensor model with fixed parameters
+ tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);
+
+ % Initialize agents
+ tc.maxIter = 75;
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+ % Initialize the simulation
+ tc.optimizeSensorPointing = true;
+ tc.obstacles = cell(0, 1);
+ 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
+ function test_single_agent_gradient_ascent_rf_sensor_pointing(tc)
+ % make basic domain
+ 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
+ minimumSINR = 50; % (dB)
+ tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, minimumSINR, [7, 6]);
+
+ % Initialize agent collision geometry
+ tc.agents = {agent};
+ geometry1 = spherical;
+ geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
+
+ % Initialize agent sensor model with fixed parameters
+ P_TX = 1e-3; % Transmit power (Watts)
+ BW = 20e6; % Bandwidth (Hz)
+ f_c = 2e9; % Center frequency (Hz)
+ G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
+ beamwidthExponent = 6;
+ lossExponent = 2;
+
+ tc.sensor = rfSensor;
+ tc.sensor = tc.sensor.initialize(P_TX, BW, f_c, G_RX_dBi, beamwidthExponent, 0, 0, lossExponent);
+
+ % Initialize agents
+ tc.maxIter = 75;
+ tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+
+ % Initialize the simulation
+ tc.optimizeSensorPointing = true;
+ tc.obstacles = cell(0, 1);
+ tc.minAlt = 0.5;
+ 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();
@@ -546,12 +616,12 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 25;
tc.commsRanges = 5 * ones(size(tc.agents));
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+ 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);
% Initialize the simulation
tc.obstacles = cell(0, 1);
- 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.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.run();
@@ -593,11 +663,11 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.commsRanges = (2 * tc.collisionRanges(1) + obstacleLength) * 0.9 * ones(size(tc.agents)); % defined such that they cannot go around the obstacle on both sides
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% 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.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.run();
@@ -633,11 +703,11 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 50;
tc.commsRanges = 4 * ones(size(tc.agents)); % defined such that they cannot reach their objective without breaking connectivity
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+ 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);
% 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.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();
@@ -668,8 +738,8 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 125;
tc.commsRanges = 5 * ones(size(tc.agents));
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize obstacles
obstacleLength = 1.5;
@@ -680,7 +750,7 @@ classdef test_miSim < matlab.unittest.TestCase
tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
- 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.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);
% Communications link should be established
tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
@@ -715,17 +785,17 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 125;
tc.commsRanges = ones(size(tc.agents));
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
- tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
- tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
- tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], 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, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
- 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.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);
% Constraint adjacency matrix defined by LNA should be as follows
tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -767,19 +837,19 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agents
tc.maxIter = 125;
tc.commsRanges = d * ones(size(tc.agents));
- tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize);
- tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize);
- tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize);
- tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize);
- tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize);
- tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize);
- tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize);
+ tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
+ tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Initialize the simulation
tc.minAlt = 0;
tc.makePlots = false;
tc.makeVideo = false;
- 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.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);
% Constraint adjacency matrix defined by LNA should be as follows
tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -859,7 +929,7 @@ classdef test_miSim < matlab.unittest.TestCase
tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), ...
tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
newAgent = agent;
- newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize);
+ newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
% Domain / obstacle / agent collision checks
violation = false;
@@ -894,7 +964,7 @@ classdef test_miSim < matlab.unittest.TestCase
sim1 = miSim;
sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ...
tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ...
- tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology);
+ tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
% Write inits and build file path
sim1.writeInits();