163 lines
7.5 KiB
Matlab
163 lines
7.5 KiB
Matlab
function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing, otherAgents)
|
|
arguments (Input)
|
|
obj (1, 1) {mustBeA(obj, "agent")};
|
|
domain (1, 1) {mustBeGeometry};
|
|
partitioning (:, :) double;
|
|
timestepIndex (1, 1) double;
|
|
index (1, 1) double;
|
|
useDoubleIntegrator (1, 1) logical = false;
|
|
dampingCoeff (1, 1) double = 2.0;
|
|
dt (1, 1) double = 1.0;
|
|
optimizeSensorPointing (1, 1) logical = false;
|
|
otherAgents (:, 1) cell = cell();
|
|
end
|
|
arguments (Output)
|
|
obj (1, 1) {mustBeA(obj, "agent")};
|
|
end
|
|
|
|
% Always update lastPos/lastVel so constrainMotion evaluates barriers at
|
|
% the correct (most recent) position, even when this agent has no partition.
|
|
obj.lastPos = obj.pos;
|
|
if useDoubleIntegrator
|
|
obj.lastVel = obj.vel;
|
|
end
|
|
|
|
% Collect objective function values across partition
|
|
partitionMask = partitioning == index;
|
|
if ~any(partitionMask(:))
|
|
% This agent has no partition, maintain current state
|
|
return;
|
|
end
|
|
objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
|
|
|
|
% Compute sensor performance on partition
|
|
maskedX = domain.objective.X(partitionMask);
|
|
maskedY = domain.objective.Y(partitionMask);
|
|
|
|
if isa(obj.sensorModel, "rfSensor")
|
|
% Extract other agents' sensor models and positions once, outside the delta loop.
|
|
% Mask the full-grid RSS caches (filled by partition()) down to this agent's
|
|
% partition subset so sensorPerformance can reuse them for all perturbations.
|
|
otherSensorsPos = cell2mat(cellfun(@(x) x.pos, otherAgents, "UniformOutput", false));
|
|
otherSensors = cellfun(@(x) x.sensorModel, otherAgents, "UniformOutput", false);
|
|
partitionIndices = find(partitionMask);
|
|
for kk = 1:numel(otherSensors)
|
|
if ~isempty(otherSensors{kk}.rssCache)
|
|
otherSensors{kk}.rssCache = otherSensors{kk}.rssCache(partitionIndices);
|
|
end
|
|
end
|
|
% Pre-mask this agent's own full-grid cache to the partition subset.
|
|
% Used for ii==1 (current position, no perturbation) to avoid recomputing.
|
|
baseSensorModel = obj.sensorModel;
|
|
if ~isempty(obj.sensorModel.rssCache)
|
|
baseSensorModel.rssCache = obj.sensorModel.rssCache(partitionIndices);
|
|
end
|
|
end
|
|
|
|
if optimizeSensorPointing
|
|
% Stash actual current sensor model tilt/azimuth before messing with it
|
|
% in these following hypotheticals
|
|
tilt = obj.sensorModel.tilt;
|
|
azimuth = obj.sensorModel.azimuth;
|
|
end
|
|
|
|
% Compute agent performance at the current position and each delta position +/- X, Y, Z, tilt, azimuth
|
|
deltaPos = domain.objective.discretizationStep; % smallest possible step size that gets different results
|
|
if optimizeSensorPointing
|
|
deltaAngle = atan2d(domain.objective.discretizationStep, obj.pos(3)); % smallest possible angle derived from smallest possible step size and current height
|
|
end
|
|
deltaApplicator = [0, 0, 0, 0, 0; 1, 0, 0, 0, 0; -1, 0, 0, 0, 0; 0, 1, 0, 0, 0; 0, -1, 0, 0, 0; 0, 0, 1, 0, 0; 0, 0, -1, 0, 0; 0, 0, 0, 1, 0; 0, 0, 0, -1, 0; 0, 0, 0, 0, 1; 0, 0, 0, 0, -1;]; % none, +X, -X, +Y, -Y, +Z, -Z, +tilt, -tilt, +azimuth, -azimuth
|
|
C_delta = NaN(size(deltaApplicator, 1), 1); % agent performance at delta steps in each direction
|
|
for ii = 1:size(deltaApplicator, 1)
|
|
if ~optimizeSensorPointing && ii > 7; break; end
|
|
% Apply delta to position
|
|
pos = obj.pos + deltaPos * deltaApplicator(ii, 1:3);
|
|
if optimizeSensorPointing
|
|
% Apply delta to tilt and azimuth
|
|
obj.sensorModel.tilt = tilt + deltaAngle * deltaApplicator(ii, 4);
|
|
obj.sensorModel.azimuth = azimuth + deltaAngle * deltaApplicator(ii, 5);
|
|
end
|
|
|
|
% Compute performance values on partition
|
|
if isa(obj.sensorModel, "sigmoidSensor")
|
|
sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
|
|
elseif isa(obj.sensorModel, "rfSensor")
|
|
if ii == 1
|
|
sensorModelForDelta = baseSensorModel; % reuse partition-step cache; no recompute needed
|
|
else
|
|
sensorModelForDelta = obj.sensorModel.clearRssCache;
|
|
end
|
|
[sensorValues, ~, ~, ~] = sensorModelForDelta.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))], otherSensorsPos, otherSensors);
|
|
else
|
|
error("?");
|
|
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
|
|
|
|
if optimizeSensorPointing
|
|
% Reset sensor model to actual tilt and azimuth angles
|
|
obj.sensorModel.tilt = tilt;
|
|
obj.sensorModel.azimuth = azimuth;
|
|
end
|
|
|
|
% Store agent performance at current time and place
|
|
if coder.target('MATLAB')
|
|
obj.performance(timestepIndex + 1) = C_delta(1);
|
|
end
|
|
|
|
% Compute gradient by finite central differences
|
|
gradC = [(C_delta(2)-C_delta(3))/(2*deltaPos), (C_delta(4)-C_delta(5))/(2*deltaPos), (C_delta(6)-C_delta(7))/(2*deltaPos)];
|
|
if optimizeSensorPointing
|
|
gradC(4) = (C_delta(8) -C_delta(9)) /(2*deltaAngle);
|
|
gradC(5) = (C_delta(10)-C_delta(11))/(2*deltaAngle);
|
|
end
|
|
|
|
% Compute scaling factor
|
|
targetPosRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
|
|
gradPosNorm = norm(gradC(1:3));
|
|
|
|
% Compute unconstrained next state
|
|
if useDoubleIntegrator
|
|
% Double-integrator: gradient produces desired acceleration with damping
|
|
if gradPosNorm < 1e-100
|
|
a_gradient = zeros(1, 5);
|
|
else
|
|
% Scale so steady-state step ≈ targetRate (matching SI behavior)
|
|
a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
|
|
end
|
|
% Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
|
|
obj.vel = (obj.vel + a_gradient(1:3) * dt) / (1 + dampingCoeff * dt);
|
|
obj.pos = obj.lastPos + obj.vel * dt;
|
|
else
|
|
% Single-integrator: gradient directly sets position step
|
|
if gradPosNorm >= 1e-100
|
|
obj.pos = obj.pos + (targetPosRate / gradPosNorm) * gradC(1:3);
|
|
end
|
|
end
|
|
|
|
% Update tilt and azimuth, saturating at the decaying maximum allowed step size
|
|
if optimizeSensorPointing
|
|
maxAngleStep = obj.initialMaxAngleStepSize - obj.angleStepDecayRate * timestepIndex;
|
|
obj.sensorModel.tilt = obj.sensorModel.tilt + sign(gradC(4)) * min(abs(gradC(4)), maxAngleStep);
|
|
obj.sensorModel.azimuth = obj.sensorModel.azimuth + sign(gradC(5)) * min(abs(gradC(5)), maxAngleStep);
|
|
end
|
|
|
|
% Reinitialize collision geometry in the new position
|
|
d = obj.pos - obj.collisionGeometry.center;
|
|
if isa(obj.collisionGeometry, "rectangularPrism")
|
|
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
|
elseif isa(obj.collisionGeometry, "spherical")
|
|
obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
|
else
|
|
error("?");
|
|
end
|
|
end |