Add minimum distance to obstacles in swarm performance

examples/examples_swarm/example_olfati_saber.m

@@ -193,13 +193,13 @@
     %% Analyse performance
 
     % Compute swarm performance
-    [safety, order, union, alg_conn, safety_obs] = ...
+    [safety, order, union, alg_conn, safety_obs, min_d_obs] = ...
         compute_swarm_performance(pos_ned_history, vel_ned_history, ...
         p_swarm, results_dirname);
 
     % Plot performance
     [perf_handle] = plot_swarm_performance(time_history', safety, order, ...
-        union, alg_conn, safety_obs, p_swarm, fontsize, results_dirname);
+        union, alg_conn, safety_obs, min_d_obs, p_swarm, fontsize, results_dirname);
 
 
 end

examples/examples_swarm/example_vasarhelyi.m

@@ -193,13 +193,13 @@
     %% Analyse performance
 
     % Compute swarm performance
-    [safety, order, union, alg_conn, safety_obs] = ...
+    [safety, order, union, alg_conn, safety_obs, min_d_obs] = ...
         compute_swarm_performance(pos_ned_history, vel_ned_history, ...
         p_swarm, results_dirname);
 
     % Plot performance
     [perf_handle] = plot_swarm_performance(time_history', safety, order, ...
-        union, alg_conn, safety_obs, p_swarm, fontsize, results_dirname);
+        union, alg_conn, safety_obs, min_d_obs, p_swarm, fontsize, results_dirname);
 
 
 end

graphics/graphics_swarm/plot_swarm_performance.m

@@ -1,5 +1,5 @@
 function [fig_handle] = plot_swarm_performance(time_history, safety, ...
-    order, union, alg_conn, safety_obs, S, fontsize, dirname)
+    order, union, alg_conn, safety_obs, min_d_obs, p_swarm, fontsize, dirname)
 % Plot swarm performance - plot the performance functions defined for the
 % swarm, namely: the safety, order, union and connectivity.
 
@@ -18,6 +18,12 @@
 ylabel('Performance', 'fontsize', fontsize);
 legend('safety','order','union','connectivity', 'safety obsacles');
 
+figure;
+plot(time_history, min(min_d_obs,[],2), 'LineWidth', 1.5);
+yline(0,'LineWidth', 1.5);
+xlabel('Time [s]', 'fontsize', fontsize);
+ylabel('Distance to obstacles', 'fontsize', fontsize);
+
 % Save only if 'dirname' is different from '[]'
 if ~isempty(dirname)
     file_path = strcat(dirname,'/performance');

math_tools/compute_swarm_performance.m

@@ -1,4 +1,4 @@
-function [safety, order, union, alg_conn, safety_obs ] = ...
+function [safety, order, union, alg_conn, safety_obs, min_d_obs ] = ...
     compute_swarm_performance(pos_history, vel_history, ...
     p_swarm, dirname)
 % Compute swarm performance - This function allows to compute the 
@@ -20,6 +20,7 @@
 %   alg_conn: algebraic connectivity metric.
 %   safety_obs: safety againt obstacles metric. reflects the number of 
 %               agent-obstacle collisions
+%   min_d_obs: minimum distance agent-obstacle
 %
 
 %% Init variables
@@ -36,6 +37,7 @@
 union = zeros(t_steps,1);
 alg_conn = zeros(t_steps,1);
 safety_obs = zeros(t_steps,1);
+min_d_obs = zeros(t_steps,nb_agents);
 
 %% Loop over time
 
@@ -100,6 +102,7 @@
             r_spheres = p_swarm.spheres(4,:);
 
             D_spheres = pdist2(pos_k',c_spheres');
+            min_d_obs(k,:) = min(pdist2(pos_k(1:2,:)',c_spheres') - repmat(r_spheres, nb_agents, 1),[],2); 
             nb_obs_coll(k) = nb_obs_coll(k) + sum(sum(D_spheres < repmat(r_spheres, nb_agents, 1)));
             nb_possible_coll = nb_possible_coll + nb_agents*length(r_spheres);
         end
@@ -108,6 +111,7 @@
             r_cyl = p_swarm.cylinders(3,:);
 
             D_cyl = pdist2(pos_k(1:2,:)',c_cyl');
+            min_d_obs(k,:) = min(pdist2(pos_k(1:2,:)',c_cyl') - repmat(r_cyl, nb_agents, 1),[],2); 
             nb_obs_coll(k) = nb_obs_coll(k) + sum(sum(D_cyl < repmat(r_cyl, nb_agents, 1)));
             nb_possible_coll = nb_possible_coll + nb_agents*length(r_cyl);
         end