better random agent placement with respect to sensor capabilities

.gitignore

@@ -41,3 +41,7 @@ codegen/
 
 # Sandbox contents
 sandbox/*
+
+# Videos
+*.mp4
+*.avi

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
+function obj = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
@@ -11,12 +11,11 @@ function [obj, f] = initialize(obj, domain, objective, agents, timestep, partito
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Define simulation time parameters
     obj.timestep = timestep;
-    obj.maxIter = maxIter;
+    obj.maxIter = maxIter - 1;
 
     % Define domain
     obj.domain = domain;
@@ -34,9 +33,17 @@ function [obj, f] = initialize(obj, domain, objective, agents, timestep, partito
     % Compute adjacency matrix
     obj = obj.updateAdjacency();
 
+    % Set up times to iterate over
+    obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
+    obj.partitioningTimes = obj.times(obj.partitioningFreq:obj.partitioningFreq:size(obj.times, 1));
+
+    % Prepare performance data store (at t = 0, all have 0 performance)
+    obj.fPerf = figure;
+    obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
+
     % Create initial partitioning
     obj = obj.partition();
 
     % Set up plots showing initialized state
-    [obj, f] = obj.plot();
+    obj = obj.plot();
 end

@miSim/miSim.m

@@ -13,14 +13,25 @@ classdef miSim
         adjacency = NaN; % Adjacency matrix representing communications network graph
         sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
+        performance = NaN; % current cumulative sensor performance
     end
 
     properties (Access = private)
+        % Sim
+        t = NaN; % current sim time
+        perf; % sensor performance timeseries array
+        times;
+        partitioningTimes;
+
         % Plot objects
+        f = firstPlotSetup(); % main plotting tiled layout figure
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
         partitionPlot; % objects for partition plot
 
+        fPerf; % performance plot figure
+        performancePlot; % objects for sensor performance plot
+
         % Indicies for various plot types in the main tiled layout figure
         spatialPlotIndices = [6, 4, 3, 2];
         objectivePlotIndices = [6, 4];
@@ -29,15 +40,15 @@ classdef miSim
     end
 
     methods (Access = public)
-        [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
-        [obj, f] = run(obj, f);
+        [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
+        [obj] = run(obj);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
-        [obj, f] = plot(obj);
-        [obj, f] = plotConnections(obj, ind, f);
-        [obj, f] = plotPartitions(obj, ind, f);
-        [obj, f] = plotGraph(obj, ind, f);
-        [obj, f] = updatePlots(obj, f, updatePartitions);
+        [obj] = plot(obj);
+        [obj] = plotConnections(obj);
+        [obj] = plotPartitions(obj);
+        [obj] = plotGraph(obj);
+        [obj] = updatePlots(obj, updatePartitions);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/partition.m

@@ -15,13 +15,22 @@ function obj = partition(obj)
     % Get highest performance value at each point
     [~, idx] = max(agentPerformances, [], 3);
 
-    % Collect agent indices in the same way
+    % Collect agent indices in the same way as performance
     agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
     agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
     agentInds = cat(3, agentInds{:});
 
     % Get highest performing agent's index
     [m,n,~] = size(agentInds);
-    [i,j] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
+    [jj,kk] = ndgrid(1:m, 1:n);
+    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+
+    % Get individual agent sensor performance
+    nowIdx = [0; obj.partitioningTimes] == obj.t;
+    for ii = 1:size(obj.agents, 1)
+        obj.perf(ii, nowIdx) = sum(agentPerformances(sub2ind(size(agentInds), jj, kk, ii)), 'all');
+    end
+
+    % Current total performance
+    obj.perf(end, nowIdx) = sum(obj.perf(1:(end - 1), nowIdx));
 end

@miSim/plot.m

@@ -1,41 +1,43 @@
-function [obj, f] = plot(obj)
+function obj = plot(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Plot domain
-    [obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
+    [obj.domain, obj.f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
 
     % Plot obstacles
     for ii = 1:size(obj.obstacles, 1)
-        [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
+        [obj.obstacles{ii}, obj.f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, obj.f);
     end
 
     % Plot objective gradient
-    f = obj.domain.objective.plot(obj.objectivePlotIndices, f);
+    obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
 
     % Plot agents and their collision geometries
     for ii = 1:size(obj.agents, 1)
-        [obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
+        [obj.agents{ii}, obj.f] = obj.agents{ii}.plot(obj.spatialPlotIndices, obj.f);
     end
 
     % Plot communication links
-    [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
+    obj = obj.plotConnections();
 
     % Plot abstract network graph
-    [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
+    obj = obj.plotGraph();
 
     % Plot domain partitioning
-    [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
+    obj = obj.plotPartitions();
 
     % 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)]);
+        xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+        ylim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
+        zlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
     end
+
+    % Plot performance
+    obj = obj.plotPerformance();
 end

@miSim/plotConnections.m

@@ -1,12 +1,9 @@
-function [obj, f] = plotConnections(obj, ind, f)
+function obj = plotConnections(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Iterate over lower triangle off-diagonal region of the 
@@ -24,20 +21,20 @@ function [obj, f] = plotConnections(obj, ind, f)
     X = X'; Y = Y'; Z = Z';
 
     % Plot the connections
-    if isnan(ind)
-        hold(f.CurrentAxes, "on");
-        o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-        hold(f.CurrentAxes, "off");
+    if isnan(obj.spatialPlotIndices)
+        hold(obj.f.CurrentAxes, "on");
+        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(obj.f.CurrentAxes, "off");
     else
-        hold(f.Children(1).Children(ind(1)), "on");
-        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-        hold(f.Children(1).Children(ind(1)), "off");
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
+        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
     end
 
     % Copy to other plots
-    if size(ind, 2) > 1
-        for ii = 2:size(ind, 2)
-            o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
+    if size(obj.spatialPlotIndices, 2) > 1
+        for ii = 2:size(obj.spatialPlotIndices, 2)
+            o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
         end 
     end
 

@miSim/plotGraph.m

@@ -1,29 +1,26 @@
-function [obj, f] = plotGraph(obj, ind, f)
+function obj = plotGraph(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Form graph from adjacency matrix
     G = graph(obj.adjacency, 'omitselfloops');
 
     % Plot graph object
-    if isnan(ind)
-        hold(f.CurrentAxes, 'on');
-        o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(f.CurrentAxes, 'off');
+    if isnan(obj.networkGraphIndex)
+        hold(obj.f.CurrentAxes, 'on');
+        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.CurrentAxes, 'off');
     else
-        hold(f.Children(1).Children(ind(1)), 'on');
-        o = plot(f.Children(1).Children(ind(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(f.Children(1).Children(ind(1)), 'off');
-        if size(ind, 2) > 1
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
+        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
+        if size(obj.networkGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
-                o = [o; copyobj(o(1), f.Children(1).Children(ind(ii)))];
+                o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
             end
         end
     end

@miSim/plotPartitions.m

@@ -1,25 +1,22 @@
-function [obj, f] = plotPartitions(obj, ind, f)
+function obj = plotPartitions(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
-    if isnan(ind)
-        hold(f.CurrentAxes, 'on');
-        o = imagesc(f.CurrentAxes, obj.partitioning);
-        hold(f.CurrentAxes, 'off');
+    if isnan(obj.partitionGraphIndex)
+        hold(obj.f.CurrentAxes, 'on');
+        o = imagesc(obj.f.CurrentAxes, obj.partitioning);
+        hold(obj.f.CurrentAxes, 'off');
     else
-        hold(f.Children(1).Children(ind(1)), 'on');
-        o = imagesc(f.Children(1).Children(ind(1)), obj.partitioning);
-        hold(f.Children(1).Children(ind(1)), 'on');
-        if size(ind, 2) > 1
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        if size(obj.partitionGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(1), f.Children(1).Children(ind(ii)))];
+                o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];
             end
         end
     end

@miSim/plotPerformance.m

@@ -0,0 +1,28 @@
+function obj = plotPerformance(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    axes(obj.fPerf);
+    title(obj.fPerf.Children(1), "Sensor Performance");
+    xlabel(obj.fPerf.Children(1), 'Time (s)');
+    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
+    grid(obj.fPerf.Children(1), 'on');
+
+    % Plot current cumulative performance
+    hold(obj.fPerf.Children(1), 'on');
+    o = plot(obj.fPerf.Children(1), obj.perf(end, :));
+    hold(obj.fPerf.Children(1), 'off');
+
+    % Plot current agent performance
+    for ii = 1:(size(obj.perf, 1) - 1)
+        hold(obj.fPerf.Children(1), 'on');
+        o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
+        hold(obj.fPerf.Children(1), 'off');
+    end
+
+    obj.performancePlot = o;
+end

@miSim/run.m

@@ -1,32 +1,23 @@
-function [obj, f] = run(obj, f)
+function [obj] = run(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
-    % Create axes if they don't already exist
-    f = firstPlotSetup(f);
-
-    % Set up times to iterate over
-    times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
-    partitioningTimes = times(obj.partitioningFreq:obj.partitioningFreq:size(times, 1));
-
     % Start video writer
     v = obj.setupVideoWriter();
     v.open();
 
-    for ii = 1:size(times, 1)
+    for ii = 1:size(obj.times, 1)
         % Display current sim time
-        t = times(ii);
-        fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter)
+        obj.t = obj.times(ii);
+        fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
         % Check if it's time for new partitions
         updatePartitions = false;
-        if ismember(t, partitioningTimes)
+        if ismember(obj.t, obj.partitioningTimes)
             updatePartitions = true;
             obj = obj.partition();
         end
@@ -37,13 +28,13 @@ function [obj, f] = run(obj, f)
         end
 
         % Update adjacency matrix
-        obj = obj.updateAdjacency;
+        obj = obj.updateAdjacency();
 
         % Update plots
-        [obj, f] = obj.updatePlots(f, updatePartitions);
+        obj = obj.updatePlots(updatePartitions);
 
         % Write frame in to video
-        I = getframe(f);
+        I = getframe(obj.f);
         v.writeVideo(I);
     end
 

@miSim/setupVideoWriter.m

@@ -9,7 +9,7 @@ function v = setupVideoWriter(obj)
     if ispc || ismac
         v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'MPEG-4');
     elseif isunix
-        v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
+        v = VideoWriter(fullfile('.', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
     end
     
     v.FrameRate = 1 / obj.timestep;

@miSim/updatePlots.m

@@ -1,12 +1,10 @@
-function [obj, f] = updatePlots(obj, f, updatePartitions)
+function [obj] = updatePlots(obj, updatePartitions)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
         updatePartitions (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Update agent positions, collision geometries
@@ -20,24 +18,35 @@ function [obj, f] = updatePlots(obj, f, updatePartitions)
 
     % Update agent connections plot
     delete(obj.connectionsPlot);
-    [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
+    obj = obj.plotConnections();
 
     % Update network graph plot
     delete(obj.graphPlot);
-    [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
+    obj = obj.plotGraph();
 
     % Update partitioning plot
     if updatePartitions
         delete(obj.partitionPlot);
-        [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
+        obj = obj.plotPartitions();
     end
 
     % reset plot limits to fit domain
     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)]);
+        xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+        ylim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
+        zlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
+    end
+    drawnow;
+    
+    % Update performance plot
+    if updatePartitions
+        nowIdx = [0; obj.partitioningTimes] == obj.t;
+        % set(obj.performancePlot(1), 'YData', obj.perf(end, 1:find(nowIdx)));
+        obj.performancePlot(1).YData(nowIdx) = obj.perf(end, nowIdx);
+        for ii = 2:size(obj.performancePlot, 1)
+            obj.performancePlot(ii).YData(nowIdx) = obj.perf(ii, nowIdx);
+        end
+        drawnow;
     end
 
-    drawnow;
 end

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/AmJAKvt15XQGz6JdpTsYHCBwsj0d.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="test"/>
+    </Category>
+</Info>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/AmJAKvt15XQGz6JdpTsYHCBwsj0p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="test_sigmoidSensor.m" type="File"/>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/Xa8LYoHW6o1icagNgoJY5eYksJ0d.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/Xa8LYoHW6o1icagNgoJY5eYksJ0p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plotParameters.m" type="File"/>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/ryNm0kOvTMRy3DefgdmGp1GqV-Yd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/ryNm0kOvTMRy3DefgdmGp1GqV-Yp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="tiltMembership.m" type="File"/>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/wiELvAwZYF03DX0eRNI7i1vkqXkd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/wiELvAwZYF03DX0eRNI7i1vkqXkp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="distanceMembership.m" type="File"/>

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/vQEWllkHYzJ8MP7NOsUYZ2vAZCAd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/vQEWllkHYzJ8MP7NOsUYZ2vAZCAp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="plotPerformance.m" type="File"/>

sensingModels/@sigmoidSensor/distanceMembership.m

@@ -0,0 +1,10 @@
+function x = distanceMembership(obj, d)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        d (:, 1) double;
+    end
+    arguments (Output)
+        x (:, 1) double;
+    end
+    x = 1 - (1 ./ (1 + exp(-obj.betaDist .* (abs(d) - obj.alphaDist))));
+end

sensingModels/@sigmoidSensor/plotParameters.m

@@ -0,0 +1,42 @@
+function f = plotParameters(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Distance and tilt sample points
+    d = 0:(obj.alphaDist / 100):(2*obj.alphaDist);
+    t = -90:1:90;
+
+    % Sample membership functions
+    d_x = obj.distanceMembership(d);
+    t_x = obj.tiltMembership(deg2rad(t));
+
+    % Plot resultant sigmoid curves
+    f = figure;
+    tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
+
+    % Distance
+    nexttile(1, [1,  1]);
+    grid("on");
+    title("Distance Membership Sigmoid");
+    xlabel("Distance (m)");
+    ylabel("Membership");
+    hold('on');
+    plot(d, d_x, 'LineWidth', 2);
+    hold('off');
+    ylim([0, 1]);
+
+    % Tilt
+    nexttile(2, [1,  1]);
+    grid("on");
+    title("Tilt Membership Sigmoid");
+    xlabel("Tilt (deg)");
+    ylabel("Membership");
+    hold('on');
+    plot(t, t_x, 'LineWidth', 2);
+    hold('off');
+    ylim([0, 1]);
+end

sensingModels/@sigmoidSensor/sensorPerformance.m

@@ -15,9 +15,8 @@ function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos
     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.betaTilt .* (tiltAngle + obj.alphaTilt)))) - (1 ./ (1 + exp(-obj.betaTilt .* (tiltAngle - obj.alphaTilt)))); % tilt
+    mu_d = obj.distanceMembership(d);
+    mu_t = obj.tiltMembership(tiltAngle);
 
-    value = mu_d .* mu_p .* mu_t;
+    value = mu_d .* mu_t; % assume pan membership is always 1
 end

sensingModels/@sigmoidSensor/sigmoidSensor.m

@@ -13,5 +13,10 @@ classdef sigmoidSensor
         [obj]               = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt);
         [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning);
         [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
+        [f] = plotParameters(obj);
+    end
+    methods (Access = private)
+        x = distanceMembership(obj, d);
+        x = tiltMembership(obj, t);
     end
 end

sensingModels/@sigmoidSensor/tiltMembership.m

@@ -0,0 +1,10 @@
+function x = tiltMembership(obj, t)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        t (:, 1) double;
+    end
+    arguments (Output)
+        x (:, 1) double;
+    end
+    x = (1 ./ (1 + exp(-obj.betaTilt .* (t + obj.alphaTilt)))) - (1 ./ (1 + exp(-obj.betaTilt .* (t - obj.alphaTilt))));
+end

test/test_miSim.m

@@ -1,5 +1,6 @@
 classdef test_miSim < matlab.unittest.TestCase
     properties (Access = private)
+        % System under test
         testClass = miSim;
 
         % Sim
@@ -34,6 +35,16 @@ classdef test_miSim < matlab.unittest.TestCase
         maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
         collisionRanges = NaN;
 
+        % Sensing
+        betaDistMin = 3;
+        betaDistMax = 15;
+        betaTiltMin = 3;
+        betaTiltMax = 15;
+        alphaDistMin = 2.5;
+        alphaDistMax = 3;
+        alphaTiltMin = deg2rad(15);
+        alphaTiltMax = deg2rad(30);
+
         % Communications
         comRange = 5; % Maximum range between agents that forms a communications link
     end
@@ -93,6 +104,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     if ii == 1
                         while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                             candidatePos = tc.domain.random();
+                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, 0.5 + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                         end
                     else
                         candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
@@ -139,7 +151,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
                     % Initialize candidate agent sensor model
                     sensor = sigmoidSensor;
-                    sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
+                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
 
                     % Initialize candidate agent
                     newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii)); 
@@ -190,7 +202,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -223,6 +235,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     if ii == 1
                         while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                             candidatePos = tc.domain.random();
+                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, 0.5 + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                         end
                     else
                         candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
@@ -269,7 +282,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     
                     % Initialize candidate agent sensor model
                     sensor = sigmoidSensor;
-                    sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
+                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
 
                     % Initialize candidate agent
                     newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
@@ -320,10 +333,10 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
 
             % Run simulation loop
-            [tc.testClass, f] = tc.testClass.run(f);
+            tc.testClass = tc.testClass.run();
         end
         function test_basic_partitioning(tc)
             % place agents a fixed distance +/- X from the domain's center
@@ -343,7 +356,11 @@ classdef test_miSim < matlab.unittest.TestCase
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
+            % Homogeneous sensor model parameters
             sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
+            f = sensor.plotParameters();
+            % Heterogeneous sensor model parameters
+            % sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
 
             % Initialize agents
             tc.agents = {agent; agent};
@@ -351,13 +368,37 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2));
 
             % Optional third agent along the +Y axis
-            % geometry3 = rectangularPrism;
-            % geometry3 = geometry3.initialize([tc.domain.center - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 3));
-            % tc.agents{3} = agent;
-            % tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, @gradientAscent, 3*d, 3, sprintf("Agent %d", 3));
+            geometry3 = rectangularPrism;
+            geometry3 = geometry3.initialize([tc.domain.center - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 3));
+            tc.agents{3} = agent;
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, @gradientAscent, 3*d, 3, sprintf("Agent %d", 3));
 
             % Initialize the simulation
-            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+        end
+        function test_annular_partition(tc)
+            % make basic domain
+            tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
+        
+            % Initialize agent collision geometry
+            geometry1 = rectangularPrism;
+            geometry1 = geometry1.initialize([[tc.domain.center(1:2), 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2), 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            
+            % Initialize agent sensor model
+            sensor = sigmoidSensor;
+            % Homogeneous sensor model parameters
+            sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 0.3641, 13);
+            f = sensor.plotParameters();
+
+            % Initialize agents
+            tc.agents = {agent};
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1));
+
+            % Initialize the simulation
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
         end
     end
 

test/test_sigmoidSensor.m

@@ -0,0 +1,53 @@
+classdef test_sigmoidSensor < matlab.unittest.TestCase
+    properties (Access = private)
+        % System under test
+        testClass = sigmoidSensor;
+
+        % Domain
+        domain = rectangularPrism;
+
+        % Sensor parameter ranges
+        betaDistMin = 3;
+        betaDistMax = 15;
+        betaTiltMin = 3;
+        betaTiltMax = 15;
+        alphaDistMin = 2.5;
+        alphaDistMax = 3;
+        alphaTiltMin = deg2rad(15);
+        alphaTiltMax = deg2rad(30);
+    end
+
+    methods (TestMethodSetup)
+        function tc = setup(tc)
+            % Reinitialize sensor with random parameters
+            tc.testClass = sigmoidSensor;
+            tc.testClass = tc.testClass.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+        end
+    end
+
+    methods (Test)
+        % Test methods
+        function test_sensorPerformance(tc)
+            tc.testClass = sigmoidSensor;
+            alphaDist = 2.5;
+            betaDist = 3;
+            alphaTilt = deg2rad(15);
+            betaTilt = 3;
+            tc.testClass = tc.testClass.initialize(alphaDist, betaDist, NaN, NaN, alphaTilt, betaTilt);
+            
+            % Plot
+            tc.testClass.plotParameters();
+
+            % Performance at current position should be maximized (1)
+            % some wiggle room is needed for certain parameter conditions,
+            % e.g. small alphaDist and betaDist produce mu_d slightly < 1
+            tc.verifyEqual(tc.testClass.sensorPerformance(zeros(1, 3), NaN, 0, zeros(1, 3)), 1, 'AbsTol', 1e-3); 
+            % It looks like mu_t can max out at really low values like 0.37
+            % when alphaTilt and betaTilt are small, which seems wrong
+
+            % Performance at distance alphaDist should be 1/2
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], NaN, 0, [0, 0, 0]), 1/2, 'AbsTol', 1e-3);
+        end
+    end
+
+end