removed early exit from main loop

@miSim/miSim.m

@@ -14,6 +14,8 @@ 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
+
+        fPerf; % performance plot figure
     end
 
     properties (Access = private)
@@ -29,7 +31,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
@@ -53,4 +54,4 @@ classdef miSim
     methods (Access = private)
         [v] = setupVideoWriter(obj);
     end
-end
+end

@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);
@@ -40,4 +41,4 @@ function [obj] = run(obj)
 
     % Close video file
     v.close();
-end
+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)));
+        nowIdx = 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
 
+        % 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

sensorModels/@sigmoidSensor/sensorPerformance.m

@@ -10,8 +10,11 @@ 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)
+
+    % compute tilt angle
     tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))) - agentTilt; % degrees
 
     % Membership functions

test/test_miSim.m

@@ -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) = 2  + rand * 2; % 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
@@ -359,10 +361,12 @@ classdef test_miSim < matlab.unittest.TestCase
             sensor = sigmoidSensor;
             % Homogeneous sensor model parameters
             sensor = sensor.initialize(2.75, 9, NaN, NaN, 22.5, 9);
-            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));
 
+            % Plot sensor parameters (optional)
+            % f = sensor.plotParameters();
+
             % Initialize agents
             tc.agents = {agent; agent};
             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));
@@ -376,6 +380,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
         function test_single_partition(tc)
             % make basic domain
@@ -383,7 +388,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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;
@@ -395,6 +400,8 @@ classdef test_miSim < matlab.unittest.TestCase
             % 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
@@ -403,7 +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

@@ -33,20 +33,29 @@ classdef test_sigmoidSensor < matlab.unittest.TestCase
             betaDist = 3;
             alphaTilt = 15; % degrees
             betaTilt = 3;
+            h = 1e-6;
             tc.testClass = tc.testClass.initialize(alphaDist, betaDist, NaN, NaN, alphaTilt, betaTilt);
             
-            % Plot
+            % Plot (optional)
-            tc.testClass.plotParameters();
+            % tc.testClass.plotParameters();
 
-            % Performance at current position should be maximized (1)
+            % Anticipate perfect performance for a point directly below and
-            % some wiggle room is needed for certain parameter conditions,
+            % extremely close
-            % e.g. small alphaDist and betaDist produce mu_d slightly < 1
+            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [0, 0, 0]), 1, 'RelTol', 1e-3); 
-            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
+            % 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, 'AbsTol', 1e-3);
+            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