added early exit from main loop for semistable final states

.gitignore

@@ -45,3 +45,6 @@ sandbox/*
 # Videos
 *.mp4
 *.avi
+
+# Figures
+*.fig

@agent/initialize.m

@@ -29,5 +29,5 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    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));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@miSim/miSim.m

@@ -14,6 +14,9 @@ classdef miSim
         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
+        oldMeanTotalPerf = 0;
+
+        fPerf; % performance plot figure
     end
 
     properties (Access = private)
@@ -29,7 +32,6 @@ classdef miSim
         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

@miSim/partition.m

@@ -20,15 +20,20 @@ function obj = partition(obj)
     agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
     agentInds = cat(3, agentInds{:});
 
-    % Get highest performing agent's index
+    % Use highest performing agent's index to form partitions
     [m, n, ~] = size(agentInds);
     [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;
+    if isnan(obj.t)
+        nowIdx = 1;
+    end
     for ii = 1:size(obj.agents, 1)
-        obj.perf(ii, nowIdx) = sum(agentPerformances(sub2ind(size(agentInds), jj, kk, ii)), 'all');
+        idx = obj.partitioning == ii;
+        agentPerformance = squeeze(agentPerformances(:, :, ii));
+        obj.perf(ii, nowIdx) = sum(agentPerformance(idx) .* obj.objective.values(idx));
     end
 
     % Current total performance

@miSim/plotPerformance.m

@@ -24,5 +24,13 @@ function obj = plotPerformance(obj)
         hold(obj.fPerf.Children(1), 'off');
     end
 
+    % Add legend
+    agentStrings = repmat("Agent %d", size(obj.perf, 1) - 1, 1);
+    for ii = 1:size(agentStrings, 1)
+        agentStrings(ii) = sprintf(agentStrings(ii), ii);
+    end
+    agentStrings = ["Total"; agentStrings];
+    legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
+
     obj.performancePlot = o;
 end

@miSim/run.m

@@ -10,6 +10,7 @@ function [obj] = run(obj)
     v = obj.setupVideoWriter();
     v.open();
 
+    steady = 0;
     for ii = 1:size(obj.times, 1)
         % Display current sim time
         obj.t = obj.times(ii);
@@ -18,6 +19,19 @@ function [obj] = run(obj)
         % Check if it's time for new partitions
         updatePartitions = false;
         if ismember(obj.t, obj.partitioningTimes)
+            % Check if it's time to end the sim (performance has settled)
+            if obj.t >= obj.partitioningTimes(5)
+                idx = find(obj.t == obj.partitioningTimes);
+                newMeanTotalPerf = mean(obj.perf(end, ((idx - 5 + 1):idx)));
+                if (obj.oldMeanTotalPerf * 0.95 <= newMeanTotalPerf) && (newMeanTotalPerf <= max(1e-6, obj.oldMeanTotalPerf * 1.05))
+                    steady = steady + 1;
+                    if steady >= 3
+                        fprintf("Performance is stable, terminating early at %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
+                        break; % performance is not improving further, exit main sim loop
+                    end
+                end
+                obj.oldMeanTotalPerf = newMeanTotalPerf;
+            end
             updatePartitions = true;
             obj = obj.partition();
         end

@miSim/updatePlots.m

@@ -40,13 +40,15 @@ function [obj] = updatePlots(obj, updatePartitions)
     
     % Update performance plot
     if updatePartitions
+        % find index corresponding to the current time
         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
+        nowIdx = find(nowIdx);
 
+        % Re-normalize performance plot
+        normalizingFactor = 1/max(obj.perf(end, 1:nowIdx));
+        obj.performancePlot(1).YData(1:nowIdx) = obj.perf(end, 1:nowIdx) * normalizingFactor;
+        for ii = 2:size(obj.performancePlot, 1)
+            obj.performancePlot(ii).YData(1:nowIdx) = obj.perf(ii - 1, 1:nowIdx) * normalizingFactor;
+        end
+    end
 end

@sensingObjective/initialize.m

@@ -29,8 +29,11 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     obj.objectiveFunction = objectiveFunction;
     obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
     
+    % Normalize
+    obj.values = obj.values ./ max(obj.values, [], "all");
+
     % store ground position
-    idx = obj.values == max(obj.values, [], "all");
+    idx = obj.values == 1;
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
 
     assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info Ref="sensingModels" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="420d04e4-3880-4a45-8609-11cb30d87302" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Ref="sensorModels" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="d143c27d-6824-4569-9093-8150b60976cb" type="Reference"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/9ucwbDs2BkA9ZqIHQI_XVIGSH5Ap.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml

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

sensorModels/@sigmoidSensor/plotParameters.m

@@ -12,7 +12,7 @@ function f = plotParameters(obj)
 
     % Sample membership functions
     d_x = obj.distanceMembership(d);
-    t_x = obj.tiltMembership(deg2rad(t));
+    t_x = obj.tiltMembership(t);
 
     % Plot resultant sigmoid curves
     f = figure;

sensorModels/@sigmoidSensor/sensorPerformance.m

@@ -10,9 +10,12 @@ function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos
         value (:, 1) double;
     end
 
+    % compute direct distance and distance projected onto the ground
     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;
+
+    % compute tilt angle
+    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))) - agentTilt; % degrees
 
     % Membership functions
     mu_d = obj.distanceMembership(d);

sensorModels/@sigmoidSensor/sigmoidSensor.m

@@ -5,7 +5,7 @@ classdef sigmoidSensor
         betaDist = NaN;
         alphaPan = NaN;
         betaPan = NaN;
-        alphaTilt = NaN;
+        alphaTilt = NaN; % degrees
         betaTilt = NaN;
     end
 

sensorModels/@sigmoidSensor/tiltMembership.m

@@ -1,7 +1,7 @@
 function x = tiltMembership(obj, t)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-        t (:, 1) double;
+        t (:, 1) double; % degrees
     end
     arguments (Output)
         x (:, 1) double;

test/test_miSim.m

@@ -25,8 +25,8 @@ classdef test_miSim < matlab.unittest.TestCase
         objective = sensingObjective;
 
         % Agents
-        minAgents = 3; % Minimum number of agents to be randomly generated
-        maxAgents = 6; % Maximum number of agents to be randomly generated
+        minAgents = 2; % Minimum number of agents to be randomly generated
+        maxAgents = 4; % Maximum number of agents to be randomly generated
         sensingLength = 0.05; % length parameter used by sensing function
         agents = cell(0, 1);
         
@@ -42,11 +42,11 @@ classdef test_miSim < matlab.unittest.TestCase
         betaTiltMax = 15;
         alphaDistMin = 2.5;
         alphaDistMax = 3;
-        alphaTiltMin = deg2rad(15);
-        alphaTiltMax = deg2rad(30);
+        alphaTiltMin = 15; % degrees
+        alphaTiltMax = 30; % degrees
 
         % Communications
-        comRange = 5; % Maximum range between agents that forms a communications link
+        comRange = 8; % Maximum range between agents that forms a communications link
     end
 
     % Setup for each test
@@ -104,10 +104,11 @@ 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
+                            candidatePos(3) = 1  + rand * 3; % 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));
+                        candidatePos(3) = 1  + rand * 3; % place agents at decent altitudes for sensing
                     end
 
                     % Make sure that the candidate position is within the
@@ -239,6 +240,7 @@ classdef test_miSim < matlab.unittest.TestCase
                         end
                     else
                         candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
+                        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
 
                     % Make sure that the candidate position is within the
@@ -349,39 +351,44 @@ classdef test_miSim < matlab.unittest.TestCase
             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
+            dh = [0,0,-1]; % bias agent altitude from domain center
             geometry1 = rectangularPrism;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize([tc.domain.center + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
-            geometry2 = geometry2.initialize([tc.domain.center - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2));
+            geometry1 = geometry1.initialize([tc.domain.center + dh + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            geometry2 = geometry2.initialize([tc.domain.center + dh - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2));
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
             % Homogeneous sensor model parameters
-            sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
-            f = sensor.plotParameters();
+            sensor = sensor.initialize(2.75, 9, NaN, NaN, 22.5, 9);
             % 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));
 
+            % Plot sensor parameters (optional)
+            % f = sensor.plotParameters();
+
             % Initialize agents
             tc.agents = {agent; agent};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
-            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));
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [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));
+            geometry3 = geometry3.initialize([tc.domain.center + dh - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [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));
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, @gradientAscent, 3*d, 3, sprintf("Agent %d", 3));
 
             % Initialize the simulation
             tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+            close(tc.testClass.fPerf);
         end
-        function test_annular_partition(tc)
+        function test_single_partition(tc)
             % make basic domain
-            tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+            l = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3); l * 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);
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2) + rand(1, 2) * 6 - 3), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
             geometry1 = rectangularPrism;
@@ -390,7 +397,11 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agent sensor model
             sensor = sigmoidSensor;
             % Homogeneous sensor model parameters
-            sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 0.3641, 13);
+            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
+            alphaDist = l/2; % half of domain length/width
+            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
+
+            % Plot sensor parameters (optional)
             f = sensor.plotParameters();
 
             % Initialize agents
@@ -399,6 +410,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize the simulation
             tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+            close(tc.testClass.fPerf);
         end
     end
 

test/test_sigmoidSensor.m

@@ -13,8 +13,8 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
         betaTiltMax = 15;
         alphaDistMin = 2.5;
         alphaDistMax = 3;
-        alphaTiltMin = deg2rad(15);
-        alphaTiltMax = deg2rad(30);
+        alphaTiltMin = 15; % degrees
+        alphaTiltMax = 30; % degrees
     end
 
     methods (TestMethodSetup)
@@ -31,22 +31,31 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
             tc.testClass = sigmoidSensor;
             alphaDist = 2.5;
             betaDist = 3;
-            alphaTilt = deg2rad(15);
+            alphaTilt = 15; % degrees
             betaTilt = 3;
+            h = 1e-6;
             tc.testClass = tc.testClass.initialize(alphaDist, betaDist, NaN, NaN, alphaTilt, betaTilt);
             
-            % Plot
-            tc.testClass.plotParameters();
+            % Plot (optional)
+            % 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); 
+            % Anticipate perfect performance for a point directly below and
+            % extremely close
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [0, 0, 0]), 1, 'RelTol', 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);
+            % Performance at nadir point, distance alphaDist should be 1/2 exactly
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], NaN, 0, [0, 0, 0]), 1/2);
+
+            % Performance at (almost) 0 distance, alphaTilt should be 1/2
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [tand(alphaTilt)*h, 0, 0]), 1/2, 'RelTol', 1e-3);
+
+            % Performance at great distance should be 0
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], NaN, 0, [0, 0, 0]), 0, 'AbsTol', 1e-9);
+
+            % Performance at great tilt should be 0
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [5, 5, 0]), 0, 'AbsTol', 1e-9);
         end
     end