added sensor pointing by gradient ascent

@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)

@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')

@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
+    if optimizeSensorPointing
-    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+        % Stash actual current sensor model tilt/azimuth before messing with it
-    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
+        % in these following hypotheticals
-    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+        tilt = obj.sensorModel.tilt;
-    for ii = 1:7
+        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
+        sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-            % 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
 
         % 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
+    targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
-    gradNorm = norm(gradC);
+    gradPosNorm = norm(gradC(1:3));
 
     % Compute unconstrained next state
     if useDoubleIntegrator
         % Double-integrator: gradient produces desired acceleration with damping
-        if gradNorm < 1e-100
+        if gradPosNorm < 1e-100
-            a_gradient = zeros(1, 3);
+            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
+        if gradPosNorm >= 1e-100
-            obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
+            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")

@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))
+    posChanged    = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
-        % Agent did not move, so nothing has to move on the plots
+    orientChanged = obj.sensorModel.tilt    ~= obj.fovGeometry.tilt || ...
+                    obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
+
+    if ~posChanged && ~orientChanged
         return;
     end
 
-    % Scatterplot point positions
+    if posChanged
-    for ii = 1:size(obj.scatterPoints, 1)
+        % Scatterplot point positions
-        obj.scatterPoints(ii).XData = obj.pos(1);
+        for ii = 1:size(obj.scatterPoints, 1)
-        obj.scatterPoints(ii).YData = obj.pos(2);
+            obj.scatterPoints(ii).XData = obj.pos(1);
-        obj.scatterPoints(ii).ZData = obj.pos(3);
+            obj.scatterPoints(ii).YData = obj.pos(2);
-    end
+            obj.scatterPoints(ii).ZData = obj.pos(3);
-
-    % 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);
         end
-    end
 
-    % Communications geometry edges
+        % Collision geometry edges
-    if obj.plotCommsGeometry
+        for jj = 1:size(obj.collisionGeometry.lines, 2)
-        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
+
+        % 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
+    % FOV cone: recompute full mesh whenever position or orientation changes
-    for jj = 1:size(obj.fovGeometry.surface, 2)
+    if ~isempty(obj.fovGeometry.surface)
-        % Update each plot
+        % Sync fovGeometry state to current agent position and sensor orientation
-        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
+        obj.fovGeometry = obj.fovGeometry.initialize( ...
-        obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
+            obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
-        obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
+            obj.fovGeometry.tag, obj.fovGeometry.label, ...
-        obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
+            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
+end

@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();

@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

@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

@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

@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");
+        ax = f.CurrentAxes;
-        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
+        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");
+        ax = f.Children(1).Children(ind(1));
-        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ sum(obj.values, "all"), "EdgeColor", "none");
+        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

@sigmoidSensor/sigmoidSensor.m

@@ -5,8 +5,9 @@ classdef sigmoidSensor
         betaDist = NaN;
         alphaTilt = NaN; % degrees
         betaTilt = NaN;
-
+    end
-        % pointing parameters
+    properties (Access = public)
+        % pointing states
         tilt = 0;
         azimuth = 0;
     end

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.timestepIndex, ii, ...
-                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep);
+                                             sim.useDoubleIntegrator, sim.dampingCoeff, sim.timestep, sim.optimizeSensorPointing);
     end
 
     % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)

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

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 
+% Check timeline in controller logs
-% (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
+controller = controllerAnalysis(resultsPath);
-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
 
 % 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

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/cCclYJTOop6jkdZsItlf7iNuov4/tmG0S9kw2cno3kVq1hMKT8-rXXkp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="controllerAnalysis.m" type="File"/>

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)

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{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.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.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{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.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{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.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{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.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{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.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{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.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.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.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.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{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.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.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.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.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.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.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();