
%Tracking of dynamic target

clear all;
nodes = 5; % 4 followers and 1 target
d=0.01;%distance between followers
r=200; % range of plot axis (xy)
iterations = 1000; 
s=[1 1 1 1 2 2 2 3 3 4 ];%graph definition
t=[5 2 3 4 5 3 4 5 4 5];
v1_pos=[100,100]; %initialize followers position
v2_pos=[-100,100];
v3_pos=[100,-100];
v4_pos=[-100,-100];
v5_init_pos=[100*rand, 100*rand]; %target position is random

G =graph(s,t)%generates  graph
L=full(laplacian(G)); %matlab function for Laplacian
degreeMax= max(max(L));%generate weight that control dynamics
alpha=0.08*1/degreeMax; %coefficient 0.08 found experimentaly
P=eye(nodes)- alpha* L; %Perron matrix

x_pos_vect(1,:)=[v1_pos(1) v2_pos(1) v3_pos(1) v4_pos(1) v5_init_pos(1)];
y_pos_vect(1,:)=[v1_pos(2) v2_pos(2) v3_pos(2) v4_pos(2) v5_init_pos(2)];

v5_pos_x(1)=v5_init_pos(1);
v5_pos_y(1)=v5_init_pos(2);


for i = 1:iterations
 x_pos_vect(i+1,:)=P* x_pos_vect(i,:)';
y_pos_vect(i+1,:)=P*y_pos_vect(i,:)';

if i<0.5*iterations %for first half of iterations the target moves
y_pos_vect(i+1,5)=y_pos_vect(i,5)+0.5*rand*(-1^(i));
 x_pos_vect(i+1,5)= x_pos_vect(i,5)+0.5*rand*(-1^(i));
end

% distance between followers
y_pos_vect(i+1,1)=y_pos_vect(i+1,1)+d;
 x_pos_vect(i+1,1)= x_pos_vect(i+1,1)+d;

y_pos_vect(i+1,2)=y_pos_vect(i+1,1)+d;
 x_pos_vect(i+1,2)= x_pos_vect(i+1,5)-d;

y_pos_vect(i+1,3)=y_pos_vect(i+1,3)-d;
 x_pos_vect(i+1,3)= x_pos_vect(i+1,3)+d;

y_pos_vect(i+1,4)=y_pos_vect(i+1,4)-d;
 x_pos_vect(i+1,4)= x_pos_vect(i+1,4)-d;

end

%% plot the animation of mooving point
figure('Position', [100, 100, 450,300])
for i=1:iterations
hold off;
plot(x_pos_vect(i,5),y_pos_vect(i,5),'ob','MarkerSize',8)
hold on;
plot(x_pos_vect(i,1),y_pos_vect(i,1),'or','MarkerFaceColor','r') 
hold on;
plot(x_pos_vect(i,2),y_pos_vect(i,2),'or','MarkerFaceColor','r') 
hold on;
plot(x_pos_vect(i,3),y_pos_vect(i,3),'or','MarkerFaceColor','r') 
hold on;
plot(x_pos_vect(i,4),y_pos_vect(i,4),'or','MarkerFaceColor','r') 
hold on;
grid;
 axis([-r r -r r])
 pause(0.001)
end