reimplemented gradient ascent as central finite differences method

@agent/agent.m

@@ -31,14 +31,13 @@ classdef agent
 
         % Plotting
         scatterPoints;
-        debug = false;
-        debugFig;
         plotCommsGeometry = true;
     end
 
     methods (Access = public)
         [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index);
+        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [partitioning] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, comRange, label, debug, plotCommsGeometry)
+function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, comRange, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         pos (1, 3) double;
@@ -9,7 +9,6 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
         sensorModel (1, 1) {mustBeSensor};
         comRange (1, 1) double;
         label (1, 1) string = "";
-        debug (1, 1) logical = false;
         plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
@@ -23,57 +22,11 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
     obj.label = label;
-    obj.debug = debug;
     obj.plotCommsGeometry = plotCommsGeometry;
 
     % Add spherical geometry based on com range
     obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
-    if obj.debug
-        obj.debugFig = figure;
-        tiledlayout(obj.debugFig, "TileSpacing", "tight", "Padding", "compact");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance x Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance x Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Agent Performance (C)");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Agent Performance (del C)");
-    end
-
     % Initialize FOV cone
     obj.fovGeometry = cone;
     obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));

@agent/partition.m

@@ -0,0 +1,35 @@
+function [partitioning] = partition(obj, agents, objective)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        agents (:, 1) {mustBeA(agents, 'cell')};
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+    end
+    arguments (Output)
+        partitioning (:, :) double;
+    end
+    
+    % Assess sensing performance of each agent at each sample point
+    % in the domain
+    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
+    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    agentPerformances = cat(3, agentPerformances{:});
+
+    % Get highest performance value at each point
+    [~, idx] = max(agentPerformances, [], 3);
+
+    % Collect agent indices in the same way as performance
+    indices = 1:size(agents, 1);
+    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+    if size(agentInds, 1) ~= size(agents, 1)
+        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
+    end
+    agentInds = num2cell(agentInds, 2:3);
+    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
+    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
+    agentInds = cat(3, agentInds{:});
+
+    % Use highest performing agent's index to form partitions
+    [m, n, ~] = size(agentInds);
+    [jj, kk] = ndgrid(1:m, 1:n);
+    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+end

@agent/run.m

@@ -1,10 +1,11 @@
-function obj = run(obj, domain, partitioning, t, index)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
-        t (1, 1) double;
+        timestepIndex (1, 1) double;
         index (1, 1) double;
+        agents (:, 1) {mustBeA(agents, 'cell')};
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
@@ -14,134 +15,63 @@ function obj = run(obj, domain, partitioning, t, index)
     partitionMask = partitioning == index;
     objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-    % Compute sensor performance across partition
+    % Compute sensor performance on partition
     maskedX = domain.objective.X(partitionMask);
     maskedY = domain.objective.Y(partitionMask);
-    zFactor = 1;
-    sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-    sensorValuesLower = obj.sensorModel.sensorPerformance(obj.pos - [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
-    sensorValuesHigher = obj.sensorModel.sensorPerformance(obj.pos + [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
 
-    % Put the values back into the form of the partition to enable basic operations on this data
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z
-    F = NaN(size(partitionMask));
+    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
-    F(partitionMask) = objectiveValues;
+    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
-    S = NaN(size(partitionMask));
+    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
-    Slower = S;
+    for ii = 1:7
-    Shigher = S;
+        % Apply delta to position
-    S(partitionMask) = sensorValues;
+        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
-    Slower(partitionMask) = sensorValuesLower;
-    Shigher(partitionMask) = sensorValuesHigher;
-
-    % Find agent's performance
-    C = S .* F;
-    obj.performance = [obj.performance, sum(C(~isnan(C)))]; % at current Z only
-    C = cat(3, Shigher, S, Slower) .* F;
-
-    % Compute gradient on agent's performance
-    [gradCX, gradCY, gradCZ] = gradient(C, domain.objective.discretizationStep); % grad C
-    gradC = cat(4, gradCX, gradCY, gradCZ);
-    nGradC = vecnorm(gradC, 2, 4);
-
-    if obj.debug
-        % Compute additional component-level values for diagnosing issues
-        [gradSensorPerformanceX, gradSensorPerformanceY] = gradient(S, domain.objective.discretizationStep); % grad S_n
-        [gradObjectiveX, gradObjectiveY] = gradient(F, domain.objective.discretizationStep); % grad f
-        gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
-        gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
-
-        ii = 8;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F./max(F, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S./max(S, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S .* vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all')./(max(F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'))));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
         
-        ii = ii - 1;
+        % Compute performance values on partition
-        hold(obj.debugFig.Children(1).Children(ii), "on");
+        if ii < 5
-        cla(obj.debugFig.Children(1).Children(ii));
+            % Compute sensing performance
-        imagesc(obj.debugFig.Children(1).Children(ii), C./max(C, [], 'all'));
+            sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-        hold(obj.debugFig.Children(1).Children(ii), "off");
+            % Objective performance does not change for 0, +/- X, Y steps.
-        ii = ii - 1;
+            % Those values are computed once before the loop and are only
-        hold(obj.debugFig.Children(1).Children(ii), "on");
+            % recomputed when +/- Z steps are applied
-        cla(obj.debugFig.Children(1).Children(ii));
+        else
-        imagesc(obj.debugFig.Children(1).Children(ii), nGradC./max(nGradC, [], 'all'));
+            % Redo partitioning for Z stepping only
-        hold(obj.debugFig.Children(1).Children(ii), "off");
+            partitioning = obj.partition(agents, domain.objective);
-        [x, y] = find(nGradC == max(nGradC, [], "all"));
 
-        % just pick one
+            % Recompute partiton-derived performance values for objective
-        r = randi([1, size(x, 1)]);
+            partitionMask = partitioning == index;
-        x = x(r); y = y(r);
+            objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-        % switch them
+            % Recompute partiton-derived performance values for sensing
-        temp = x;
+            maskedX = domain.objective.X(partitionMask);
-        x = y;
+            maskedY = domain.objective.Y(partitionMask);
-        y = temp;
+            sensorValues = obj.sensorModel.sensorPerformance(pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-        
-        % find objective location in discrete domain
-        [~, xIdx] = find(domain.objective.groundPos(1) == domain.objective.X);
-        xIdx = unique(xIdx);
-        [yIdx, ~] = find(domain.objective.groundPos(2) == domain.objective.Y);
-        yIdx = unique(yIdx);
-        for ii = 8:-1:1
-            hold(obj.debugFig.Children(1).Children(ii), "on");
-            % plot GA selection
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'go');
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'g+');
-            % plot objective center
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'ro');
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'r+');
-            hold(obj.debugFig.Children(1).Children(ii), "off");
         end
+
+        % Rearrange data into image arrays
+        F = NaN(size(partitionMask));
+        F(partitionMask) = objectiveValues;
+        S = NaN(size(partitionMask));
+        S(partitionMask) = sensorValues;
+
+        % Compute agent performance
+        C = S .* F;
+        C_delta(ii) = sum(C(~isnan(C)));
     end
 
-    % return now if there is no data to work with, and do not move
+    % Compute gradient by finite central differences
-    if all(isnan(nGradC), 'all')
+    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)];
-        return;
-    end
 
-    % Use largest grad(C) value to find the direction of the next position
+    % Compute scaling factor
-    [xNextIdx, yNextIdx, zNextIdx] = ind2sub(size(nGradC), find(nGradC == max(nGradC, [], 'all')));
+    targetRate = 0.2 - 0.0008 * timestepIndex; % slow down as you get closer
-    % switch them
+    rateFactor = targetRate / norm(gradC); 
-    temp = xNextIdx;
-    xNextIdx = yNextIdx;
-    yNextIdx = temp;
 
-    roundingScale = 10^-log10(domain.objective.discretizationStep);
+    % Compute unconstrained next position
-    zKey = zFactor * [1; 0; -1];
+    pNext = obj.pos + rateFactor * gradC;
-    pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3) + zKey(zNextIdx)]; % have to do some unfortunate rounding here sometimes
-
-    % Determine next position
-    vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
-    rate = 0.1 - 0.0004 * t; % slow down as you get closer, coming to a stop by the end
-    nextPos = obj.pos + vDir * rate;
 
     % Move to next position
     obj.lastPos = obj.pos;
-    obj.pos = nextPos;
+    obj.pos = pNext;
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;

@miSim/miSim.m

@@ -13,7 +13,6 @@ classdef miSim
         agents = cell(0, 1); % agents that move within the domain
         adjacency = NaN; % Adjacency matrix representing communications network graph
         constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
-        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
         perf; % sensor performance timeseries array
         performance = 0; % simulation performance timeseries vector

@miSim/partition.m

@@ -9,7 +9,7 @@ function obj = partition(obj)
     % Assess sensing performance of each agent at each sample point
     % in the domain
     agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    agentPerformances{end + 1} = obj.domain.objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
     agentPerformances = cat(3, agentPerformances{:});
     
     % Get highest performance value at each point

@miSim/run.m

@@ -33,7 +33,7 @@ function [obj] = run(obj)
 
         % 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.t, jj);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
         end
 
         % Adjust motion determined by unconstrained gradient ascent using

@sensingObjective/initialize.m

@@ -1,10 +1,11 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
@@ -12,6 +13,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
 
     obj.discretizationStep = discretizationStep;
 
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+
     obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 

@sensingObjective/sensingObjective.m

@@ -10,10 +10,11 @@ classdef sensingObjective
         Y = [];
         values = [];
         protectedRange = 1; % keep obstacles from crowding objective
+        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange);
+        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
         [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end

resources/project/hjJloWDVwJVRCwGO0y5r9iO7IDk/PR5LYpNCHNsKl-th1ypVJR7QPBAd.xml

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

resources/project/hjJloWDVwJVRCwGO0y5r9iO7IDk/PR5LYpNCHNsKl-th1ypVJR7QPBAp.xml

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

test/test_miSim.m

@@ -456,13 +456,10 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, "", false);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), true, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1));
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
-            if isgraphics(tc.testClass.agents{1}.debugFig)
-                close(tc.testClass.agents{1}.debugFig);
-            end
 
             % tc.verifyGreaterThan(tc.testClass.performance(end)/max(tc.testClass.performance), 0.99); % ends up very near a relative maximum
         end