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/agent.m
+2
View File
@@ -32,7 +32,9 @@ classdef agent
properties (SetAccess = private, GetAccess = public)
initialStepSize = NaN;
initialMaxAngleStepSize = NaN;
stepDecayRate = NaN;
angleStepDecayRate = NaN;
end
methods (Access = public)
@agent/initialize.m
+4 -1
View File
@@ -1,4 +1,4 @@
function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, initialMaxAngleStepSize, label, plotCommsGeometry)
arguments (Input)
obj (1, 1) {mustBeA(obj, "agent")};
pos (1, 3) double;
@@ -7,6 +7,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
comRange (1, 1) double;
maxIter (1, 1) double;
initialStepSize (1, 1) double = 0.2;
initialMaxAngleStepSize (1, 1) double = 5.0;
label (1, 1) string = "";
plotCommsGeometry (1, 1) logical = false;
end
@@ -23,7 +24,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
obj.label = label;
obj.plotCommsGeometry = plotCommsGeometry;
obj.initialStepSize = initialStepSize;
obj.initialMaxAngleStepSize = initialMaxAngleStepSize;
obj.stepDecayRate = obj.initialStepSize / maxIter;
obj.angleStepDecayRate = obj.initialMaxAngleStepSize / maxIter;
% Initialize performance vector
if coder.target('MATLAB')
@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")
@agent/updatePlots.m
+51 -28
View File
@@ -7,45 +7,68 @@ function updatePlots(obj)
% Find change in agent position since last timestep
deltaPos = obj.pos - obj.lastPos;
if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
% Agent did not move, so nothing has to move on the plots
posChanged = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
orientChanged = obj.sensorModel.tilt ~= obj.fovGeometry.tilt || ...
obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
if ~posChanged && ~orientChanged
return;
end
% Scatterplot point positions
for ii = 1:size(obj.scatterPoints, 1)
obj.scatterPoints(ii).XData = obj.pos(1);
obj.scatterPoints(ii).YData = obj.pos(2);
obj.scatterPoints(ii).ZData = obj.pos(3);
end
% Collision geometry edges
for jj = 1:size(obj.collisionGeometry.lines, 2)
% Update plotting
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
if posChanged
% Scatterplot point positions
for ii = 1:size(obj.scatterPoints, 1)
obj.scatterPoints(ii).XData = obj.pos(1);
obj.scatterPoints(ii).YData = obj.pos(2);
obj.scatterPoints(ii).ZData = obj.pos(3);
end
end
% Communications geometry edges
if obj.plotCommsGeometry
for jj = 1:size(obj.commsGeometry.lines, 2)
% Collision geometry edges
for jj = 1:size(obj.collisionGeometry.lines, 2)
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
% Communications geometry edges
if obj.plotCommsGeometry
for jj = 1:size(obj.commsGeometry.lines, 2)
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
end
end
% Update FOV geometry surfaces
for jj = 1:size(obj.fovGeometry.surface, 2)
% Update each plot
% obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
% FOV cone: recompute full mesh whenever position or orientation changes
if ~isempty(obj.fovGeometry.surface)
% Sync fovGeometry state to current agent position and sensor orientation
obj.fovGeometry = obj.fovGeometry.initialize( ...
obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
obj.fovGeometry.tag, obj.fovGeometry.label, ...
obj.sensorModel.tilt, obj.sensorModel.azimuth);
% Recompute cone mesh (mirrors cone.plot logic)
maxAlt = obj.fovGeometry.surface(1).Parent.ZLim(2);
scalingFactor = maxAlt / obj.fovGeometry.height;
[X, Y, Z] = cylinder([scalingFactor * obj.fovGeometry.radius, 0], obj.fovGeometry.n);
Z = Z * maxAlt;
Ry = [cosd(obj.fovGeometry.tilt), 0, -sind(obj.fovGeometry.tilt); 0, 1, 0; sind(obj.fovGeometry.tilt), 0, cosd(obj.fovGeometry.tilt)];
Rz = [sind(obj.fovGeometry.azimuth), -cosd(obj.fovGeometry.azimuth), 0; cosd(obj.fovGeometry.azimuth), sind(obj.fovGeometry.azimuth), 0; 0, 0, 1];
R = Rz * Ry;
pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
X = reshape(pts(1, :), size(X)) + obj.pos(1);
Y = reshape(pts(2, :), size(Y)) + obj.pos(2);
Z = reshape(pts(3, :) + maxAlt, size(Z)) + obj.pos(3) - maxAlt;
for jj = 1:size(obj.fovGeometry.surface, 2)
obj.fovGeometry.surface(jj).XData = X;
obj.fovGeometry.surface(jj).YData = Y;
obj.fovGeometry.surface(jj).ZData = Z;
end
end
end
end