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

reimplemented gradient ascent as central finite differences method

Browse Source
This commit is contained in:
Kevin Dee
2026-01-11 12:42:48 -08:00
parent c47b7229ba
commit ec202d7790
12 changed files with 101 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
@agent/agent.m
View File

@@ -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

49
@agent/initialize.m
View File

@@ -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));

35
@agent/partition.m Normal file
View File

@@ -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

162
@agent/run.m
View File

@@ -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
F = NaN(size(partitionMask));
F(partitionMask) = objectiveValues;
S = NaN(size(partitionMask));
Slower = S;
Shigher = S;
S(partitionMask) = sensorValues;
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");
% Compute agent performance at the current position and each delta position +/- X, Y, Z
delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
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
C_delta = NaN(7, 1); % agent performance at delta steps in each direction
for ii = 1:7
% Apply delta to position
pos = obj.pos + delta * deltaApplicator(ii, 1:3);
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), C./max(C, [], '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), nGradC./max(nGradC, [], 'all'));
hold(obj.debugFig.Children(1).Children(ii), "off");
[x, y] = find(nGradC == max(nGradC, [], "all"));
% Compute performance values on partition
if ii < 5
% Compute sensing performance
sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
% Objective performance does not change for 0, +/- X, Y steps.
% Those values are computed once before the loop and are only
% recomputed when +/- Z steps are applied
else
% Redo partitioning for Z stepping only
partitioning = obj.partition(agents, domain.objective);
% just pick one
r = randi([1, size(x, 1)]);
x = x(r); y = y(r);
% Recompute partiton-derived performance values for objective
partitionMask = partitioning == index;
objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
% switch them
temp = x;
x = y;
y = temp;
% 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");
% Recompute partiton-derived performance values for sensing
maskedX = domain.objective.X(partitionMask);
maskedY = domain.objective.Y(partitionMask);
sensorValues = obj.sensorModel.sensorPerformance(pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
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
if all(isnan(nGradC), 'all')
return;
end
% Compute gradient by finite central differences
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)];
% Use largest grad(C) value to find the direction of the next position
[xNextIdx, yNextIdx, zNextIdx] = ind2sub(size(nGradC), find(nGradC == max(nGradC, [], 'all')));
% switch them
temp = xNextIdx;
xNextIdx = yNextIdx;
yNextIdx = temp;
% Compute scaling factor
targetRate = 0.2 - 0.0008 * timestepIndex; % slow down as you get closer
rateFactor = targetRate / norm(gradC);
roundingScale = 10^-log10(domain.objective.discretizationStep);
zKey = zFactor * [1; 0; -1];
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;
% Compute unconstrained next position
pNext = obj.pos + rateFactor * gradC;
% 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;

1
@miSim/miSim.m
View File

@@ -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

2
@miSim/partition.m
View File

@@ -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

2
@miSim/run.m
View File

@@ -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

5
@sensingObjective/initialize.m
View File

@@ -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;

3
@sensingObjective/sensingObjective.m
View File

@@ -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

6
resources/project/hjJloWDVwJVRCwGO0y5r9iO7IDk/PR5LYpNCHNsKl-th1ypVJR7QPBAd.xml Normal file
View File

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

2
resources/project/hjJloWDVwJVRCwGO0y5r9iO7IDk/PR5LYpNCHNsKl-th1ypVJR7QPBAp.xml Normal file
View File

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

5
test/test_miSim.m
View File

@@ -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