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

fixed cone radius and sigmoid sensor test parameters

Browse Source
This commit is contained in:
Kevin D
2025-11-14 13:33:32 -08:00
parent 18b690d9d8
commit e7127365bd
5 changed files with 18 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
agent.m
View File

@@ -63,7 +63,7 @@ classdef agent
% Initialize FOV cone
obj.fovGeometry = cone;
obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], obj.sensorModel.r, obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tan(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
end
function obj = run(obj, sensingObjective, domain, partitioning)
arguments (Input)

7
miSim.m
View File

@@ -93,6 +93,13 @@ classdef miSim
% Plot domain partitioning
[obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
% Enforce plot limits
for ii = 1:size(obj.spatialPlotIndices, 2)
xlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
ylim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
zlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
end
end
function [obj, f] = run(obj, f)
arguments (Input)

5
sensingModels/fixedCardinalSensor.m
View File

@@ -1,6 +1,7 @@
classdef fixedCardinalSensor
% Senses in the +/-x, +/- y directions at some specified fixed length
properties
alphaTilt = NaN;
r = 0.1; % fixed sensing length
end
@@ -27,6 +28,9 @@ classdef fixedCardinalSensor
neighborValues (4, 1) double;
neighborPos (4, 3) double;
end
% Set alphaTilt to produce an FOV cone with radius 'r' on the ground
obj.alphaTilt = atan2(obj.r, agent.pos(3));
% Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
currentPos = agent.pos(1:2);
@@ -70,7 +74,6 @@ classdef fixedCardinalSensor
end
value = 0.5 * ones(size(targetPos, 1), 1);
end
end
end

12
sensingModels/sigmoidSensor.m
View File

@@ -7,8 +7,6 @@ classdef sigmoidSensor
betaPan = NaN;
alphaTilt = NaN;
betaTilt = NaN;
r = NaN;
end
methods (Access = public)
@@ -32,8 +30,6 @@ classdef sigmoidSensor
obj.betaPan = betaPan;
obj.alphaTilt = alphaTilt;
obj.betaTilt = betaTilt;
obj.r = obj.alphaDist;
end
function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning)
arguments (Input)
@@ -68,13 +64,13 @@ classdef sigmoidSensor
value (:, 1) double;
end
d = vecnorm(agentPos - targetPos, 2, 2);
panAngle = atan2(targetPos(:, 2) - agentPos(2), targetPos(:, 1) - agentPos(1)) - agentPan;
tiltAngle = atan2(targetPos(:, 3) - agentPos(3), d) - agentTilt;
d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
tiltAngle = atan2(targetPos(:, 3) - agentPos(3), x) - agentTilt;
% Membership functions
mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance
mu_p = (1 ./ (1 + exp(-obj.betaPan .* (panAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (panAngle - obj.alphaPan)))); % pan
mu_p = 1; % pan
mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt
value = mu_d .* mu_p .* mu_t * 1e12;

4
test_miSim.m
View File

@@ -360,7 +360,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize candidate agent sensor model
sensor = sigmoidSensor;
sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
% Initialize candidate agent
newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
@@ -434,7 +434,7 @@ classdef test_miSim < matlab.unittest.TestCase
% Initialize agent sensor model
sensor = sigmoidSensor;
sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
% Initialize agents
tc.agents = {agent; agent};