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