better sigmoid sensor unit testing

@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/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

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,7 +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) = 2  + rand * 2; % place agents at decent altitudes for sensing
+                            candidatePos(3) = 2  + rand * 1.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));

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
             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