Upload New File

Matlab Code/trajectoryUtility.m

+function util = trajectoryUtility(verts, plcs, env, k, sig, plotArea)
+
+Tj = size(plcs,2);
+[vertags,vertplcs] = covertIdxForm(verts,Tj);
+agentplcs = zeros(length(verts),2);
+% get each vertex position
+for a = 1:length(verts)
+    agentplcs(a,:) = plcs{k,vertplcs(a)}(vertags(a),:);
+end
+
+util = 0;
+
+for i=1:length(env.x_area)
+    for j=1:length(env.y_area)
+        dists = sqrt((agentplcs(:,1) - env.x_area(i)).^2 + (agentplcs(:,2) - env.y_area(j)).^2);
+        if any(dists <= sig) % if anybody is close enough to get a measurement
+            util = util + env.Z(j, i);
+        end
+    end
+end
+
+% util = sum(gmPDF(agentplcs(:,1), agentplcs(:,2))); % util for this edge selection
+
+end
\ No newline at end of file