function jointPDFminDistanceAlignment( T ) posX=T(:,4); posY=T(:,5); angle=T(:,6); velX=T(:,7); velY=T(:,8); angle=wrapToPi(angle); noOfParticles=T(end,2)+1; tEnd=T(end,1); minDistance=ones(length(posX),1)*0; minDAngle=ones(length(posX),1)*0; relVel=ones(length(posX),1)*0; bP=0.5666; epsilon=2.5; aP=epsilon*bP; for tt=1:tEnd for pp=1:noOfParticles p=(tt-1)*noOfParticles+pp; for qq=1:noOfParticles q=(tt-1)*noOfParticles+qq; temp=sqrt((posX(p)-posX(q))^2+(posY(p)-posY(q))^2); if (temp < minDistance(p) || minDistance(p)==0) && p ~= q minDistance(p)=temp; minDAngle(p)=abs(angle(p)-angle(q)); relVel(p)=norm([velX(p)-velX(q);velY(p)-velY(q)]); if minDAngle(p)>pi minDAngle(p)=minDAngle(p)-pi; end end end end end Xedges = [0.6:0.01:1.6]; Yedges = [0:0.03:3.1415]; h = histogram2(minDistance,minDAngle,Xedges,Yedges,'DisplayStyle','tile','ShowEmptyBins','on','normalization','pdf'); tmap = [0.00 0.00 0.00 0.69 0.00 0.00 1.00 0.69 0.00 1.00 0.98 0.78 1.00 1.00 1.00]; xmap = [0 63.75 127.5 191.25 255]; map = interp1(xmap/255,tmap,linspace(0,1,255)); colormap(map) k=h.Values; l=smoothdata(k,'gaussian',10); s=surf(Xedges(1:end-1),Yedges(1:end-1),l'); s.EdgeColor='none'; shading interp view(2); xlabel('minDist') ylabel('alignment') title(max(max(l))); colorbar end