kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

gradient ascent works now?

Browse Source
This commit is contained in:
Kevin Dee
2025-11-30 19:08:15 -08:00
committed by Kevin D
parent c92ef143d1
commit 28a6bfe3de
3 changed files with 91 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
@agent/initialize.m
View File

@@ -36,22 +36,42 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
axes(obj.debugFig.Children(1).Children(1)); axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image"); axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y"); 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; nexttile;
axes(obj.debugFig.Children(1).Children(1)); axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image"); axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y"); 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; nexttile;
axes(obj.debugFig.Children(1).Children(1)); axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image"); axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y"); 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; nexttile;
axes(obj.debugFig.Children(1).Children(1)); axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image"); axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y"); 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 end
% Initialize FOV cone % Initialize FOV cone

83
@agent/run.m
View File

@@ -18,14 +18,14 @@ function obj = run(obj, domain, partitioning, t)
maskedY = domain.objective.Y(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 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 = NaN(size(partitionMask));
F(partitionMask) = objectiveValues; F(partitionMask) = objectiveValues;
S = NaN(size(partitionMask)); S = NaN(size(partitionMask));
S(partitionMask) = sensorValues; S(partitionMask) = sensorValues;
% Find agent's performance % Find agent's performance
C = S.* F; C = S.* F; % try gradient on this directly
obj.performance = [obj.performance sum(C(~isnan(C)))]; obj.performance = [obj.performance sum(C(~isnan(C)))];
% Compute gradient on agent's performance % Compute gradient on agent's performance
@@ -35,30 +35,77 @@ function obj = run(obj, domain, partitioning, t)
gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f 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 if obj.debug
hold(obj.debugFig.Children(1).Children(4), "on"); ii = 8;
imagesc(obj.debugFig.Children(1).Children(4), F); hold(obj.debugFig.Children(1).Children(ii), "on");
hold(obj.debugFig.Children(1).Children(4), "off"); imagesc(obj.debugFig.Children(1).Children(ii), F./max(F, [], 'all'));
hold(obj.debugFig.Children(1).Children(3), "on"); hold(obj.debugFig.Children(1).Children(ii), "off");
imagesc(obj.debugFig.Children(1).Children(3), S); ii = ii - 1;
hold(obj.debugFig.Children(1).Children(3), "off"); hold(obj.debugFig.Children(1).Children(ii), "on");
hold(obj.debugFig.Children(1).Children(2), "on"); imagesc(obj.debugFig.Children(1).Children(ii), S./max(S, [], 'all'));
imagesc(obj.debugFig.Children(1).Children(2), gradF./max(gradF, [], 'all')); hold(obj.debugFig.Children(1).Children(ii), "off");
hold(obj.debugFig.Children(1).Children(2), "off"); ii = ii - 1;
hold(obj.debugFig.Children(1).Children(1), "on"); hold(obj.debugFig.Children(1).Children(ii), "on");
imagesc(obj.debugFig.Children(1).Children(1), abs(gradS)./max(gradS, [], 'all')); imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
hold(obj.debugFig.Children(1).Children(1), "off"); 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 end
% grad(s*f) = grad(f) * s + f * grad(s) - product rule (f scalar field, s vector field) % 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 % 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 [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); vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
rate = 0.1 - 0.004 * t; rate = 0.2 - 0.004 * t;
nextPos = obj.pos + vDir * rate; nextPos = obj.pos + vDir * rate;
% Move to next position % Move to next position

2
test/test_miSim.m
View File

@@ -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"); tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective % 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 % Initialize agent collision geometry
geometry1 = rectangularPrism; geometry1 = rectangularPrism;