reorganized code into separate files
This commit is contained in:
13
sensingModels/@fixedCardinalSensor/fixedCardinalSensor.m
Normal file
13
sensingModels/@fixedCardinalSensor/fixedCardinalSensor.m
Normal file
@@ -0,0 +1,13 @@
|
||||
classdef fixedCardinalSensor
|
||||
% Senses in the +/-x, +/- y directions at some specified fixed length
|
||||
properties
|
||||
alphaTilt = NaN;
|
||||
r = 0.1; % fixed sensing length
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
[obj] = initialize(obj, r);
|
||||
[neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning);
|
||||
[value] = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
|
||||
end
|
||||
end
|
||||
10
sensingModels/@fixedCardinalSensor/initialize.m
Normal file
10
sensingModels/@fixedCardinalSensor/initialize.m
Normal file
@@ -0,0 +1,10 @@
|
||||
function obj = initialize(obj, r)
|
||||
arguments(Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
r (1, 1) double;
|
||||
end
|
||||
arguments(Output)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
end
|
||||
obj.r = r;
|
||||
end
|
||||
45
sensingModels/@fixedCardinalSensor/sense.m
Normal file
45
sensingModels/@fixedCardinalSensor/sense.m
Normal file
@@ -0,0 +1,45 @@
|
||||
function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
agent (1, 1) {mustBeA(agent, 'agent')};
|
||||
sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
partitioning (:, :) double = NaN;
|
||||
end
|
||||
arguments (Output)
|
||||
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);
|
||||
neighborPos = [currentPos(1) + obj.r, currentPos(2); ... % (+x)
|
||||
currentPos(1), currentPos(2) + obj.r; ... % (+y)
|
||||
currentPos(1) - obj.r, currentPos(2); ... % (-x)
|
||||
currentPos(1), currentPos(2) - obj.r; ... % (-y)
|
||||
];
|
||||
|
||||
% Check for neighbor positions that fall outside of the domain
|
||||
outOfBounds = false(size(neighborPos, 1), 1);
|
||||
for ii = 1:size(neighborPos, 1)
|
||||
if ~domain.contains([neighborPos(ii, :), 0])
|
||||
outOfBounds(ii) = true;
|
||||
end
|
||||
end
|
||||
|
||||
% Replace out of bounds positions with inoffensive in-bounds positions
|
||||
neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1);
|
||||
|
||||
% Sense values at selected positions
|
||||
neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
|
||||
sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
|
||||
sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
|
||||
sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
|
||||
];
|
||||
|
||||
% Prevent out of bounds locations from ever possibly being selected
|
||||
neighborValues(outOfBounds) = 0;
|
||||
end
|
||||
14
sensingModels/@fixedCardinalSensor/sensorPerformance.m
Normal file
14
sensingModels/@fixedCardinalSensor/sensorPerformance.m
Normal file
@@ -0,0 +1,14 @@
|
||||
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
agentPos (1, 3) double;
|
||||
agentPan (1, 1) double;
|
||||
agentTilt (1, 1) double;
|
||||
targetPos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
value (:, 1) double;
|
||||
end
|
||||
|
||||
value = 0.5 * ones(size(targetPos, 1), 1);
|
||||
end
|
||||
21
sensingModels/@sigmoidSensor/initialize.m
Normal file
21
sensingModels/@sigmoidSensor/initialize.m
Normal file
@@ -0,0 +1,21 @@
|
||||
function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
||||
alphaDist (1, 1) double;
|
||||
betaDist (1, 1) double;
|
||||
alphaPan (1, 1) double;
|
||||
betaPan (1, 1) double;
|
||||
alphaTilt (1, 1) double;
|
||||
betaTilt (1, 1) double;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
||||
end
|
||||
|
||||
obj.alphaDist = alphaDist;
|
||||
obj.betaDist = betaDist;
|
||||
obj.alphaPan = alphaPan;
|
||||
obj.betaPan = betaPan;
|
||||
obj.alphaTilt = alphaTilt;
|
||||
obj.betaTilt = betaTilt;
|
||||
end
|
||||
21
sensingModels/@sigmoidSensor/sense.m
Normal file
21
sensingModels/@sigmoidSensor/sense.m
Normal file
@@ -0,0 +1,21 @@
|
||||
function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
||||
agent (1, 1) {mustBeA(agent, 'agent')};
|
||||
sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
partitioning (:, :) double;
|
||||
end
|
||||
arguments (Output)
|
||||
values (:, 1) double;
|
||||
positions (:, 3) double;
|
||||
end
|
||||
|
||||
% Find positions for this agent's assigned partition in the domain
|
||||
idx = partitioning == agent.index;
|
||||
positions = [sensingObjective.X(idx), sensingObjective.Y(idx), zeros(size(sensingObjective.X(idx)))];
|
||||
|
||||
% Evaluate objective function at every point in this agent's
|
||||
% assigned partiton
|
||||
values = sensingObjective.values(idx);
|
||||
end
|
||||
23
sensingModels/@sigmoidSensor/sensorPerformance.m
Normal file
23
sensingModels/@sigmoidSensor/sensorPerformance.m
Normal file
@@ -0,0 +1,23 @@
|
||||
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
||||
agentPos (1, 3) double;
|
||||
agentPan (1, 1) double;
|
||||
agentTilt (1, 1) double;
|
||||
targetPos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
value (:, 1) double;
|
||||
end
|
||||
|
||||
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; % 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;
|
||||
end
|
||||
17
sensingModels/@sigmoidSensor/sigmoidSensor.m
Normal file
17
sensingModels/@sigmoidSensor/sigmoidSensor.m
Normal file
@@ -0,0 +1,17 @@
|
||||
classdef sigmoidSensor
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
% Sensor parameters
|
||||
alphaDist = NaN;
|
||||
betaDist = NaN;
|
||||
alphaPan = NaN;
|
||||
betaPan = NaN;
|
||||
alphaTilt = NaN;
|
||||
betaTilt = NaN;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
[obj] = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt);
|
||||
[values, positions] = sense(obj, agent, sensingObjective, domain, partitioning);
|
||||
[value] = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
|
||||
end
|
||||
end
|
||||
@@ -1,79 +0,0 @@
|
||||
classdef fixedCardinalSensor
|
||||
% Senses in the +/-x, +/- y directions at some specified fixed length
|
||||
properties
|
||||
alphaTilt = NaN;
|
||||
r = 0.1; % fixed sensing length
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, r)
|
||||
arguments(Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
r (1, 1) double;
|
||||
end
|
||||
arguments(Output)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
end
|
||||
obj.r = r;
|
||||
end
|
||||
function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
agent (1, 1) {mustBeA(agent, 'agent')};
|
||||
sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
partitioning (:, :) double = NaN;
|
||||
end
|
||||
arguments (Output)
|
||||
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);
|
||||
neighborPos = [currentPos(1) + obj.r, currentPos(2); ... % (+x)
|
||||
currentPos(1), currentPos(2) + obj.r; ... % (+y)
|
||||
currentPos(1) - obj.r, currentPos(2); ... % (-x)
|
||||
currentPos(1), currentPos(2) - obj.r; ... % (-y)
|
||||
];
|
||||
|
||||
% Check for neighbor positions that fall outside of the domain
|
||||
outOfBounds = false(size(neighborPos, 1), 1);
|
||||
for ii = 1:size(neighborPos, 1)
|
||||
if ~domain.contains([neighborPos(ii, :), 0])
|
||||
outOfBounds(ii) = true;
|
||||
end
|
||||
end
|
||||
|
||||
% Replace out of bounds positions with inoffensive in-bounds positions
|
||||
neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1);
|
||||
|
||||
% Sense values at selected positions
|
||||
neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
|
||||
sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
|
||||
sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
|
||||
sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
|
||||
];
|
||||
|
||||
% Prevent out of bounds locations from ever possibly being selected
|
||||
neighborValues(outOfBounds) = 0;
|
||||
end
|
||||
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
|
||||
agentPos (1, 3) double;
|
||||
agentPan (1, 1) double;
|
||||
agentTilt (1, 1) double;
|
||||
targetPos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
value (:, 1) double;
|
||||
end
|
||||
|
||||
value = 0.5 * ones(size(targetPos, 1), 1);
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -1,79 +0,0 @@
|
||||
classdef sigmoidSensor
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
% Sensor parameters
|
||||
alphaDist = NaN;
|
||||
betaDist = NaN;
|
||||
alphaPan = NaN;
|
||||
betaPan = NaN;
|
||||
alphaTilt = NaN;
|
||||
betaTilt = NaN;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
||||
alphaDist (1, 1) double;
|
||||
betaDist (1, 1) double;
|
||||
alphaPan (1, 1) double;
|
||||
betaPan (1, 1) double;
|
||||
alphaTilt (1, 1) double;
|
||||
betaTilt (1, 1) double;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
|
||||
end
|
||||
|
||||
obj.alphaDist = alphaDist;
|
||||
obj.betaDist = betaDist;
|
||||
obj.alphaPan = alphaPan;
|
||||
obj.betaPan = betaPan;
|
||||
obj.alphaTilt = alphaTilt;
|
||||
obj.betaTilt = betaTilt;
|
||||
end
|
||||
function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
||||
agent (1, 1) {mustBeA(agent, 'agent')};
|
||||
sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
partitioning (:, :) double;
|
||||
end
|
||||
arguments (Output)
|
||||
values (:, 1) double;
|
||||
positions (:, 3) double;
|
||||
end
|
||||
|
||||
% Find positions for this agent's assigned partition in the domain
|
||||
idx = partitioning == agent.index;
|
||||
positions = [sensingObjective.X(idx), sensingObjective.Y(idx), zeros(size(sensingObjective.X(idx)))];
|
||||
|
||||
% Evaluate objective function at every point in this agent's
|
||||
% assigned partiton
|
||||
values = sensingObjective.values(idx);
|
||||
end
|
||||
function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
|
||||
agentPos (1, 3) double;
|
||||
agentPan (1, 1) double;
|
||||
agentTilt (1, 1) double;
|
||||
targetPos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
value (:, 1) double;
|
||||
end
|
||||
|
||||
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; % 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;
|
||||
end
|
||||
end
|
||||
end
|
||||
Reference in New Issue
Block a user