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

added sensor pointing by gradient ascent

Browse Source
This commit is contained in:
Kevin Dee
2026-05-07 09:04:52 -07:00
parent 740b42eba4
commit b44df40c7e
17 changed files with 296 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
@agent/run.m
+51 -36
View File
@@ -1,14 +1,14 @@
function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useDoubleIntegrator, dampingCoeff, dt)
function obj = run(obj, domain, partitioning, timestepIndex, index, useDoubleIntegrator, dampingCoeff, dt, optimizeSensorPointing)
arguments (Input)
obj (1, 1) {mustBeA(obj, "agent")};
domain (1, 1) {mustBeGeometry};
partitioning (:, :) double;
timestepIndex (1, 1) double;
index (1, 1) double;
agents (:, 1) {mustBeA(agents, "cell")};
useDoubleIntegrator (1, 1) logical = false;
dampingCoeff (1, 1) double = 2.0;
dt (1, 1) double = 1.0;
optimizeSensorPointing (1, 1) logical = false;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, "agent")};
@@ -33,34 +33,32 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
maskedX = domain.objective.X(partitionMask);
maskedY = domain.objective.Y(partitionMask);
% 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
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 + delta * deltaApplicator(ii, 1:3);
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 ii < 6
% Compute sensing performance
sensorValues = obj.sensorModel.sensorPerformance(pos, [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);
% Recompute partiton-derived performance values for objective
partitionMask = partitioning == index;
objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
% Recompute partiton-derived performance values for sensing
maskedX = domain.objective.X(partitionMask);
maskedY = domain.objective.Y(partitionMask);
sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
end
sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
% Rearrange data into image arrays
F = NaN(size(partitionMask));
@@ -73,37 +71,54 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
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*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
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
targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
gradNorm = norm(gradC);
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 gradNorm < 1e-100
a_gradient = zeros(1, 3);
if gradPosNorm < 1e-100
a_gradient = zeros(1, 5);
else
% Scale so steady-state step ≈ targetRate (matching SI behavior)
a_gradient = (targetRate * dampingCoeff / (gradNorm * dt)) * gradC;
a_gradient = (targetPosRate * dampingCoeff / (gradPosNorm * dt)) * gradC;
end
% Semi-implicit Euler: unconditionally stable for any dampingCoeff and dt
obj.vel = (obj.vel + a_gradient * dt) / (1 + dampingCoeff * 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 gradNorm >= 1e-100
obj.pos = obj.pos + (targetRate / gradNorm) * gradC;
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")