added debug visualization for agent GA

@agent/agent.m

@@ -31,11 +31,13 @@ classdef agent
 
         % Plotting
         scatterPoints;
+        debug = false;
+        debugFig;
     end
 
     methods (Access = public)
         [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning);
+        [obj] = run(obj, domain, partitioning, t);
         [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, guidanceModel, comRange, index, label)
+function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label, debug)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         pos (1, 3) double;
@@ -11,6 +11,7 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
         comRange (1, 1) double = NaN;
         index (1, 1) double = NaN;
         label (1, 1) string = "";
+        debug (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
@@ -26,6 +27,32 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     obj.comRange = comRange;
     obj.index = index;
     obj.label = label;
+    obj.debug = debug;
+
+    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 View");
+        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 View");
+        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 View");
+        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 View");
+    end
 
     % Initialize FOV cone
     obj.fovGeometry = cone;

@agent/run.m

@@ -1,8 +1,9 @@
-function obj = run(obj, domain, partitioning)
+function obj = run(obj, domain, partitioning, t)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
+        t (1, 1) double;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
@@ -17,16 +18,16 @@ function obj = run(obj, domain, partitioning)
     maskedY = domain.objective.Y(partitionMask);
     sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
 
-    % Find agent's performance
-    obj.performance = [obj.performance; sum(objectiveValues .* sensorValues, 'all')];
-
-    %%
     % Put the values back into the form of the partition
     F = NaN(size(partitionMask));
     F(partitionMask) = objectiveValues;
     S = NaN(size(partitionMask));
     S(partitionMask) = sensorValues;
 
+    % Find agent's performance
+    C = S.* F;
+    obj.performance = [obj.performance  sum(C(~isnan(C)))];
+
     % Compute gradient on agent's performance
     [gradSensorPerformanceX, gradSensorPerformanceY] = gradient(S, domain.objective.discretizationStep); % grad S_n
     [gradObjectiveX, gradObjectiveY] = gradient(F, domain.objective.discretizationStep); % grad f
@@ -34,15 +35,31 @@ function obj = run(obj, domain, partitioning)
     gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
     gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
 
+    if obj.debug
+        hold(obj.debugFig.Children(1).Children(4), "on");
+        imagesc(obj.debugFig.Children(1).Children(4), F);
+        hold(obj.debugFig.Children(1).Children(4), "off");
+        hold(obj.debugFig.Children(1).Children(3), "on");
+        imagesc(obj.debugFig.Children(1).Children(3), S);
+        hold(obj.debugFig.Children(1).Children(3), "off");
+        hold(obj.debugFig.Children(1).Children(2), "on");
+        imagesc(obj.debugFig.Children(1).Children(2), gradF./max(gradF, [], 'all'));
+        hold(obj.debugFig.Children(1).Children(2), "off");
+        hold(obj.debugFig.Children(1).Children(1), "on");
+        imagesc(obj.debugFig.Children(1).Children(1), abs(gradS)./max(gradS, [], 'all'));
+        hold(obj.debugFig.Children(1).Children(1), "off");
+    end
+
     % grad(s*f) = grad(f) * s + f * grad(s) - product rule (f scalar field, s vector field)
-    gradC = S .* gradF + F .* gradS; % second term provides altitude
+    gradC = S .* gradF + F .* abs(gradS); % second term provides altitude
     % normalize in x3 dimension and find the direction which maximizes ascent
     nGradC = vecnorm(gradC, 2, 3);
     [xNextIdx, yNextIdx] = find(nGradC == max(nGradC, [], 'all')); % find direction of steepest increase
     pNext = [floor(mean(unique(domain.objective.X(:, xNextIdx)))), floor(mean(unique(domain.objective.Y(yNextIdx, :)))), obj.pos(3)]; % have to do some unfortunate rounding here soemtimes
 
     vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
-    nextPos = obj.pos + vDir * 0.2;    
+    rate = 0.1 - 0.004 * t;
+    nextPos = obj.pos + vDir * rate;    
 
     % Move to next position
     % (dynamics not modeled at this time)

@miSim/run.m

@@ -25,7 +25,7 @@ function [obj] = run(obj)
 
         % Iterate over agents to simulate their motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
         end
 
         % Update adjacency matrix

test/test_miSim.m

@@ -412,6 +412,38 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
             close(tc.testClass.fPerf);
         end
+        function test_single_partition_basic_GA(tc)
+            % make basic domain
+            l = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2) + rand(1, 2) * 6 - 3), tc.domain, tc.discretizationStep, tc.protectedRange);
+        
+            % Initialize agent collision geometry
+            geometry1 = rectangularPrism;
+            geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            
+            % Initialize agent sensor model
+            sensor = sigmoidSensor;
+            % Homogeneous sensor model parameters
+            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
+            alphaDist = l/2; % half of domain length/width
+            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
+
+            % Plot sensor parameters (optional)
+            f = sensor.plotParameters();
+
+            % Initialize agents
+            tc.agents = {agent};
+            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, @gradientAscent, 3, 1, sprintf("Agent %d", 1), true);
+
+            % Initialize the simulation
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+            
+            % Run the simulation
+            tc.testClass.run();
+        end
     end
 
     methods