From 1e0db2a46c66a1f427145e6f5a7388aa7cc5f617 Mon Sep 17 00:00:00 2001
From: Kevin D <krdee1@gmail.com>
Date: Tue, 25 Nov 2025 13:09:33 -0800
Subject: [PATCH 1/6] removed early exit from main loop

---
 @miSim/miSim.m |  3 +--
 @miSim/run.m   | 15 +--------------
 2 files changed, 2 insertions(+), 16 deletions(-)

diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index cbb3d9b..0724041 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -14,7 +14,6 @@ classdef miSim
         sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
         performance = NaN; % current cumulative sensor performance
-        oldMeanTotalPerf = 0;
 
         fPerf; % performance plot figure
     end
@@ -55,4 +54,4 @@ classdef miSim
     methods (Access = private)
         [v] = setupVideoWriter(obj);
     end
-end
\ No newline at end of file
+end
diff --git a/@miSim/run.m b/@miSim/run.m
index 437693e..dd233bc 100644
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -19,19 +19,6 @@ function [obj] = run(obj)
         % Check if it's time for new partitions
         updatePartitions = false;
         if ismember(obj.t, obj.partitioningTimes)
-            % Check if it's time to end the sim (performance has settled)
-            if obj.t >= obj.partitioningTimes(5)
-                idx = find(obj.t == obj.partitioningTimes);
-                newMeanTotalPerf = mean(obj.perf(end, ((idx - 5 + 1):idx)));
-                if (obj.oldMeanTotalPerf * 0.95 <= newMeanTotalPerf) && (newMeanTotalPerf <= max(1e-6, obj.oldMeanTotalPerf * 1.05))
-                    steady = steady + 1;
-                    if steady >= 3
-                        fprintf("Performance is stable, terminating early at %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
-                        break; % performance is not improving further, exit main sim loop
-                    end
-                end
-                obj.oldMeanTotalPerf = newMeanTotalPerf;
-            end
             updatePartitions = true;
             obj = obj.partition();
         end
@@ -54,4 +41,4 @@ function [obj] = run(obj)
 
     % Close video file
     v.close();
-end
\ No newline at end of file
+end

From 6d16dfe9742bb1f955fd2a48cc17d55e6dcec857 Mon Sep 17 00:00:00 2001
From: krdee1 <krdee1@gmail.com>
Date: Sun, 30 Nov 2025 09:52:17 -0800
Subject: [PATCH 2/6] flawed GA implementation

---
 @agent/agent.m                 |  4 +++-
 @agent/run.m                   | 41 +++++++++++++++++++++++++++++-----
 @miSim/run.m                   |  2 +-
 @sensingObjective/initialize.m |  6 +++--
 4 files changed, 43 insertions(+), 10 deletions(-)

diff --git a/@agent/agent.m b/@agent/agent.m
index 51af2db..bd5f83a 100644
--- a/@agent/agent.m
+++ b/@agent/agent.m
@@ -27,13 +27,15 @@ classdef agent
         % Communication
         comRange = NaN;
 
+        performance = 0;
+
         % Plotting
         scatterPoints;
     end
 
     methods (Access = public)
         [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, sensingObjective, domain, partitioning);
+        [obj] = run(obj, domain, partitioning);
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end
diff --git a/@agent/run.m b/@agent/run.m
index fb901b3..5132e40 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -1,7 +1,6 @@
-function obj = run(obj, sensingObjective, domain, partitioning)
+function obj = run(obj, domain, partitioning)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
-        sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
     end
@@ -9,11 +8,41 @@ function obj = run(obj, sensingObjective, domain, partitioning)
         obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
-    % Do sensing
-    [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
+    % Collect objective function values across partition
+    partitionMask = partitioning == obj.index;
+    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-    % Determine next planned position
-    nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
+    % Compute sensor performance across partition
+    maskedX = domain.objective.X(partitionMask);
+    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;
+
+    % 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
+
+    gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
+    gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
+
+    % 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
+    % 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;    
 
     % Move to next position
     % (dynamics not modeled at this time)
diff --git a/@miSim/run.m b/@miSim/run.m
index dd233bc..7ef4487 100644
--- a/@miSim/run.m
+++ b/@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.objective, obj.domain, obj.partitioning);
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning);
         end
 
         % Update adjacency matrix
diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m
index 8b67a89..84752e0 100644
--- a/@sensingObjective/initialize.m
+++ b/@sensingObjective/initialize.m
@@ -10,6 +10,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
+    obj.discretizationStep = discretizationStep;
+
     obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 
@@ -19,8 +21,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     yMin = min(domain.footprint(:, 2));
     yMax = max(domain.footprint(:, 2));
 
-    xGrid = unique([xMin:discretizationStep:xMax, xMax]);
-    yGrid = unique([yMin:discretizationStep:yMax, yMax]);
+    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
     
     % Store grid points for plotting later
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);

From c92ef143d1eac0cc970427d61fe5a556cdcec833 Mon Sep 17 00:00:00 2001
From: krdee1 <krdee1@gmail.com>
Date: Sun, 30 Nov 2025 11:00:39 -0800
Subject: [PATCH 3/6] added debug visualization for agent GA

---
 @agent/agent.m      |  4 +++-
 @agent/initialize.m | 29 ++++++++++++++++++++++++++++-
 @agent/run.m        | 31 ++++++++++++++++++++++++-------
 @miSim/run.m        |  2 +-
 test/test_miSim.m   | 32 ++++++++++++++++++++++++++++++++
 5 files changed, 88 insertions(+), 10 deletions(-)

diff --git a/@agent/agent.m b/@agent/agent.m
index bd5f83a..b14c679 100644
--- a/@agent/agent.m
+++ b/@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
diff --git a/@agent/initialize.m b/@agent/initialize.m
index 208014c..ab99f8e 100644
--- a/@agent/initialize.m
+++ b/@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;
diff --git a/@agent/run.m b/@agent/run.m
index 5132e40..b366169 100644
--- a/@agent/run.m
+++ b/@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)
diff --git a/@miSim/run.m b/@miSim/run.m
index 7ef4487..2d49312 100644
--- a/@miSim/run.m
+++ b/@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
diff --git a/test/test_miSim.m b/test/test_miSim.m
index 33de5b9..88cfc20 100644
--- a/test/test_miSim.m
+++ b/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

From 28a6bfe3de55653f6076a8c63f55ad28b31401bc Mon Sep 17 00:00:00 2001
From: krdee1 <krdee1@gmail.com>
Date: Sun, 30 Nov 2025 19:08:15 -0800
Subject: [PATCH 4/6] gradient ascent works now?

---
 @agent/initialize.m | 28 ++++++++++++---
 @agent/run.m        | 85 +++++++++++++++++++++++++++++++++++----------
 test/test_miSim.m   |  2 +-
 3 files changed, 91 insertions(+), 24 deletions(-)

diff --git a/@agent/initialize.m b/@agent/initialize.m
index ab99f8e..77f4bde 100644
--- a/@agent/initialize.m
+++ b/@agent/initialize.m
@@ -36,22 +36,42 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
         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");
+        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 View");
+        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 View");
+        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 View");
+        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
diff --git a/@agent/run.m b/@agent/run.m
index b366169..ade4055 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -18,14 +18,14 @@ function obj = run(obj, domain, partitioning, t)
     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
 
-    % Put the values back into the form of the partition
+    % Put the values back into the form of the partition to enable basic operations on this data
     F = NaN(size(partitionMask));
     F(partitionMask) = objectiveValues;
     S = NaN(size(partitionMask));
     S(partitionMask) = sensorValues;
 
     % Find agent's performance
-    C = S.* F;
+    C = S.* F; % try gradient on this directly
     obj.performance = [obj.performance  sum(C(~isnan(C)))];
 
     % Compute gradient on agent's performance
@@ -35,31 +35,78 @@ function obj = run(obj, domain, partitioning, t)
     gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
     gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
 
+    [gradCX, gradCY] = gradient(C, domain.objective.discretizationStep); % grad C;
+    gradC = cat(3, gradCX, gradCY, zeros(size(gradCX))); % temp zeros for gradCZ
+    nGradC = vecnorm(gradC, 2, 3);
+
     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");
+        ii = 8;
+        hold(obj.debugFig.Children(1).Children(ii), "on");
+        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");
+        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");
+        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");
+        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");
+        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");
+        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;
+        hold(obj.debugFig.Children(1).Children(ii), "on");
+        imagesc(obj.debugFig.Children(1).Children(ii), C./max(C, [], 'all'));
+        hold(obj.debugFig.Children(1).Children(ii), "off");
+        ii = ii - 1;
+        hold(obj.debugFig.Children(1).Children(ii), "on");
+        imagesc(obj.debugFig.Children(1).Children(ii), nGradC./max(nGradC, [], 'all'));
+        hold(obj.debugFig.Children(1).Children(ii), "off");
+        [x, y] = find(nGradC == max(nGradC, [], "all"));
+
+        % just pick one
+        r = randi([1, size(x, 1)]);
+        x = x(r); y = y(r);
+        
+        % 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
     end
 
     % grad(s*f) = grad(f) * s + f * grad(s) - product rule (f scalar field, s vector field)
-    gradC = S .* gradF + F .* abs(gradS); % second term provides altitude
+    % gradC = S .* abs(gradF) + F .* abs(gradS); % second term provides altitude
     % normalize in x3 dimension and find the direction which maximizes ascent
-    nGradC = vecnorm(gradC, 2, 3);
+    % 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
+    roundingScale = 10^-log10(domain.objective.discretizationStep);
+    pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3)]; % have to do some unfortunate rounding here soemtimes
 
     vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
-    rate = 0.1 - 0.004 * t;
-    nextPos = obj.pos + vDir * rate;    
+    rate = 0.2 - 0.004 * t;
+    nextPos = obj.pos + vDir * rate;
 
     % Move to next position
     % (dynamics not modeled at this time)
diff --git a/test/test_miSim.m b/test/test_miSim.m
index 88cfc20..5fe400c 100644
--- a/test/test_miSim.m
+++ b/test/test_miSim.m
@@ -418,7 +418,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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);
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
             geometry1 = rectangularPrism;

From bdd018e566c3620ba333bbb133c2c2a100dc55ef Mon Sep 17 00:00:00 2001
From: krdee1 <krdee1@gmail.com>
Date: Sun, 30 Nov 2025 22:32:17 -0800
Subject: [PATCH 5/6] refactored performance plot data storage

---
 @agent/run.m             |  4 ++--
 @miSim/miSim.m           |  2 +-
 @miSim/partition.m       | 14 --------------
 @miSim/plotPerformance.m |  2 ++
 @miSim/run.m             |  3 +++
 @miSim/updatePlots.m     | 18 +++++++-----------
 6 files changed, 15 insertions(+), 28 deletions(-)

diff --git a/@agent/run.m b/@agent/run.m
index ade4055..3fc1a24 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -25,8 +25,8 @@ function obj = run(obj, domain, partitioning, t)
     S(partitionMask) = sensorValues;
 
     % Find agent's performance
-    C = S.* F; % try gradient on this directly
-    obj.performance = [obj.performance  sum(C(~isnan(C)))];
+    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
diff --git a/@miSim/miSim.m b/@miSim/miSim.m
index 0724041..c69bac1 100644
--- a/@miSim/miSim.m
+++ b/@miSim/miSim.m
@@ -13,7 +13,7 @@ classdef miSim
         adjacency = NaN; % Adjacency matrix representing communications network graph
         sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
-        performance = NaN; % current cumulative sensor performance
+        performance = 0; % cumulative sensor performance
 
         fPerf; % performance plot figure
     end
diff --git a/@miSim/partition.m b/@miSim/partition.m
index d9e9115..c292264 100644
--- a/@miSim/partition.m
+++ b/@miSim/partition.m
@@ -24,18 +24,4 @@ function obj = partition(obj)
     [m, n, ~] = size(agentInds);
     [jj, kk] = ndgrid(1:m, 1:n);
     obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-
-    % Get individual agent sensor performance
-    nowIdx = [0; obj.partitioningTimes] == obj.t;
-    if isnan(obj.t)
-        nowIdx = 1;
-    end
-    for ii = 1:size(obj.agents, 1)
-        idx = obj.partitioning == ii;
-        agentPerformance = squeeze(agentPerformances(:, :, ii));
-        obj.perf(ii, nowIdx) = sum(agentPerformance(idx) .* obj.objective.values(idx));
-    end
-
-    % Current total performance
-    obj.perf(end, nowIdx) = sum(obj.perf(1:(end - 1), nowIdx));
 end
\ No newline at end of file
diff --git a/@miSim/plotPerformance.m b/@miSim/plotPerformance.m
index 5a9c635..5dff115 100644
--- a/@miSim/plotPerformance.m
+++ b/@miSim/plotPerformance.m
@@ -15,12 +15,14 @@ function obj = plotPerformance(obj)
     % Plot current cumulative performance
     hold(obj.fPerf.Children(1), 'on');
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
+    o.XData = NaN(size(o.XData)); % correct time will be set at runtime
     hold(obj.fPerf.Children(1), 'off');
 
     % Plot current agent performance
     for ii = 1:(size(obj.perf, 1) - 1)
         hold(obj.fPerf.Children(1), 'on');
         o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
+        o(end).XData = NaN(size(o(end).XData)); % correct time will be set at runtime
         hold(obj.fPerf.Children(1), 'off');
     end
 
diff --git a/@miSim/run.m b/@miSim/run.m
index 2d49312..2474b36 100644
--- a/@miSim/run.m
+++ b/@miSim/run.m
@@ -28,6 +28,9 @@ function [obj] = run(obj)
             obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
         end
 
+        % Update total performance
+        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];
+
         % Update adjacency matrix
         obj = obj.updateAdjacency();
 
diff --git a/@miSim/updatePlots.m b/@miSim/updatePlots.m
index 0c64610..5c07f32 100644
--- a/@miSim/updatePlots.m
+++ b/@miSim/updatePlots.m
@@ -39,16 +39,12 @@ function [obj] = updatePlots(obj, updatePartitions)
     drawnow;
     
     % Update performance plot
-    if updatePartitions
-        % find index corresponding to the current time
-        nowIdx = [0; obj.partitioningTimes] == obj.t;
-        nowIdx = find(nowIdx);
-
-        % Re-normalize performance plot
-        normalizingFactor = 1/max(obj.perf(end, 1:nowIdx));
-        obj.performancePlot(1).YData(1:nowIdx) = obj.perf(end, 1:nowIdx) * normalizingFactor;
-        for ii = 2:size(obj.performancePlot, 1)
-            obj.performancePlot(ii).YData(1:nowIdx) = obj.perf(ii - 1, 1:nowIdx) * normalizingFactor;
-        end
+    % Re-normalize performance plot
+    normalizingFactor = 1/max(obj.performance(end));
+    obj.performancePlot(1).YData(1:length(obj.performance)) = obj.performance * normalizingFactor;
+    obj.performancePlot(1).XData(find(isnan(obj.performancePlot(1).XData), 1, 'first')) = obj.t;
+    for ii = 2:(size(obj.agents, 1) + 1)
+        obj.performancePlot(ii).YData(1:length(obj.performance)) = obj.agents{ii - 1}.performance * normalizingFactor;
+        obj.performancePlot(ii).XData(find(isnan(obj.performancePlot(ii).XData), 1, 'first')) = obj.t;
     end
 end
\ No newline at end of file

From d30fd9ccaa004439b66a505c4fe540b1076b6d7c Mon Sep 17 00:00:00 2001
From: Kevin D <krdee1@gmail.com>
Date: Mon, 1 Dec 2025 22:58:38 -0800
Subject: [PATCH 6/6] fixed performance plot after 50th timestep

---
 @agent/run.m             | 25 ++++++++++---------------
 @miSim/plotPerformance.m |  7 +++++--
 2 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/@agent/run.m b/@agent/run.m
index 3fc1a24..16566c1 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -29,17 +29,17 @@ function obj = run(obj, domain, partitioning, t)
     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
-
-    gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
-    gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
-
     [gradCX, gradCY] = gradient(C, domain.objective.discretizationStep); % grad C;
     gradC = cat(3, gradCX, gradCY, zeros(size(gradCX))); % temp zeros for gradCZ
     nGradC = vecnorm(gradC, 2, 3);
 
     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");
         imagesc(obj.debugFig.Children(1).Children(ii), F./max(F, [], 'all'));
@@ -96,26 +96,21 @@ function obj = run(obj, domain, partitioning, t)
         end
     end
 
-    % grad(s*f) = grad(f) * s + f * grad(s) - product rule (f scalar field, s vector field)
-    % gradC = S .* abs(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
+    % Use largest grad(C) value to find the direction of the next position
+    [xNextIdx, yNextIdx] = find(nGradC == max(nGradC, [], 'all'));
     roundingScale = 10^-log10(domain.objective.discretizationStep);
     pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3)]; % have to do some unfortunate rounding here soemtimes
 
+    % Determine next position
     vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
     rate = 0.2 - 0.004 * t;
     nextPos = obj.pos + vDir * rate;
 
     % Move to next position
-    % (dynamics not modeled at this time)
     obj.lastPos = obj.pos;
     obj.pos = nextPos;
 
-    % Calculate movement
-    d = obj.pos - obj.collisionGeometry.center;
-
     % Reinitialize collision geometry in the new position
+    d = obj.pos - obj.collisionGeometry.center;
     obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
 end
\ No newline at end of file
diff --git a/@miSim/plotPerformance.m b/@miSim/plotPerformance.m
index 5dff115..27b6305 100644
--- a/@miSim/plotPerformance.m
+++ b/@miSim/plotPerformance.m
@@ -15,14 +15,17 @@ function obj = plotPerformance(obj)
     % Plot current cumulative performance
     hold(obj.fPerf.Children(1), 'on');
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    o.XData = NaN(size(o.XData)); % correct time will be set at runtime
+    warning('off', 'MATLAB:gui:array:InvalidArrayShape'); % suppress this warning to avoid polluting output
+    o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
+    o.YData = [0, NaN(1, obj.maxIter - 1)];
     hold(obj.fPerf.Children(1), 'off');
 
     % Plot current agent performance
     for ii = 1:(size(obj.perf, 1) - 1)
         hold(obj.fPerf.Children(1), 'on');
         o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
-        o(end).XData = NaN(size(o(end).XData)); % correct time will be set at runtime
+        o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
+        o(end).YData = [0, NaN(1, obj.maxIter - 1)];
         hold(obj.fPerf.Children(1), 'off');
     end