diff --git a/agent.m b/agent.m index bf3967e..4ededba 100644 --- a/agent.m +++ b/agent.m @@ -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) diff --git a/miSim.m b/miSim.m index a887a75..d7206be 100644 --- a/miSim.m +++ b/miSim.m @@ -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) diff --git a/sensingModels/fixedCardinalSensor.m b/sensingModels/fixedCardinalSensor.m index 954674e..96da85c 100644 --- a/sensingModels/fixedCardinalSensor.m +++ b/sensingModels/fixedCardinalSensor.m @@ -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 \ No newline at end of file diff --git a/sensingModels/sigmoidSensor.m b/sensingModels/sigmoidSensor.m index 5295855..8eed9ee 100644 --- a/sensingModels/sigmoidSensor.m +++ b/sensingModels/sigmoidSensor.m @@ -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; diff --git a/test_miSim.m b/test_miSim.m index 45423e4..ff451b0 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -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};