From 28a6bfe3de55653f6076a8c63f55ad28b31401bc Mon Sep 17 00:00:00 2001 From: krdee1 Date: Sun, 30 Nov 2025 19:08:15 -0800 Subject: [PATCH] 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;