Code covered by the BSD License

Kaushik Das (view profile)

This is a code of "Broadcast Control Mechanisms for three agent".

```clear all ; clc ; close all ;

%%

% Defining the Initial position of the Agents
% -------------------------------------------------------------------------
% initial position of agent 1

p11(:,1) = 2.25 %1 ;
p12(:,1) = 3.25 %1 ;
th1(1) = 60.92+50 ;

% initial position of agent 2

p21(:,1) = 2.75 %5 ;
p22(:,1) = 5.25 %4 ;
th2(1) = 150.92 + 100 %135;

% initial position of agent 3

p31(:,1) = 4.25 %2 ;
p32(:,1) = 4.75 %6 ;
th3(1) = -29.08+70 %-45 ;

%   mov = avifile('example23.avi') ;
% Perturbation in Angle
% -------------------------------------------------------------------------

pertb_ang = 120 ;

k = 1
Iter = 0
U = 0        %  Path defined
n=1

% starts the loop from here

for i2 = 1:1

%  angle_convert

c1 = cosd(th1(:,k)) ;
s1 = sind(th1(:,k)) ;

c2 = cosd(th2(:,k)) ;
s2 = sind(th2(:,k)) ;

c3 = cosd(th3(:,k)) ;
s3 = sind(th3(:,k)) ;

% Defining the A matrix
% -------------------------------------------------------------------------
% Variable  1    2    3    4    5    6    7    8    9

A =     [  -1    1    0    0    c1  -c1  -s1   s1  -1
1   -1    0    0   -c1   c1   s1  -s1  -1
0    0   -1    1    s1  -s1   c1  -c1  -1
0    0    1   -1   -s1   s1  -c1   c1  -1

-1    1    0    0    c2  -c2  -s2   s2  -1
1   -1    0    0   -c2   c2   s2  -s2  -1
0    0   -1    1    s2  -s2   c2  -c2  -1
0    0    1   -1   -s2   s2  -c2   c2  -1

-1    1    0    0    c3  -c3  -s3   s3  -1
1   -1    0    0   -c3   c3   s3  -s3  -1
0    0   -1    1    s3  -s3   c3  -c3  -1
0    0    1   -1   -s3   s3  -c3   c3  -1

-1    0    0    0     0    0    0    0   0
0   -1    0    0     0    0    0    0   0
0    0   -1    0     0    0    0    0   0
0    0    0   -1     0    0    0    0   0
0    0    0    0    -1    0    0    0   0
0    0    0    0     0   -1    0    0   0
0    0    0    0     0    0   -1    0   0
0    0    0    0     0    0    0   -1   0
0    0    0    0     0    0    0    0  -1 ] ;

% Defining the Constraint Matrix B
% ------------------------------------------------------------------------

B = [ -p11(:,k)
p11(:,k)
-p12(:,k)
p12(:,k)
-p21(:,k)
p21(:,k)
-p22(:,k)
p22(:,k)
-p31(:,k)
p31(:,k)
-p32(:,k)
p32(:,k)

0
0
0
0
0
0
0
0
0 ] ;

lb = zeros(9,1);

% Defining The Objective Function Matrix C
% ------------------------------------------------------------------------

C = [ 0 0 0 0 0 0 0 0 1 ] ;

% Computing the Linear Program
% ------------------------------------------------------------------------
%  tic
[X,lambda,exitflag,output] =  linprog(C,A,B);%,[],[],lb) ;
%  t=toc
%  pause

x1(:,k) = X(1,:) ;
x2(:,k) = X(2,:) ;
x3(:,k) = X(3,:) ;
x4(:,k) = X(4,:) ;
x5(:,k) = X(5,:) ;
x6(:,k) = X(6,:) ;
x7(:,k) = X(7,:) ;
x8(:,k) = X(8,:) ;
x9(:,k) = X(9,:) ;

z1(:,k) = x1(:,k) - x2(:,k) ;
z2(:,k) = x3(:,k) - x4(:,k) ;
u1(:,k) = x5(:,k) - x6(:,k) ;
u2(:,k) = x7(:,k) - x8(:,k) ;

% display('The Value of Performance Radius = ')

r = x9(:,k) ;

% rectangle('Position',[z1(:,k),z2(:,k),r,r])
% axis equal
% hold on

% Square Vertex
% x_vert_u(:,k) = z1(:,k) + x9(:,k) ;
% x_vert_l(:,k) = z1(:,k) - x9(:,k) ;
%
% y_vert_u(:,k) = z2(:,k) + x9(:,k) ;
% y_vert_l(:,k) = z2(:,k) - x9(:,k) ;

% Direction of u
% -------------------------------------------------------------------------

if  (u2(:,k)<0 && u1(:,k)<0)

ang_u(:,k) = 180+(atand(u2(:,k)/u1(:,k))) ;

elseif  (u2(:,k)<0 && u1(:,k)>0)

ang_u(:,k) = 360 + atand(u2(:,k)/u1(:,k)) ;

elseif   (u2(:,k)>0 && u1(:,k)<0)

ang_u(:,k) = 180 - abs(atand(u2(:,k)/u1(:,k))) ;

else
ang_u(:,k) = atand(u2(:,k)/u1(:,k)) ;

end

% Random Number Decleration
% -------------------------------------------------------------------------
Ran_1(:,k) = randn ;
Ran_2(:,k) = randn ;
Ran_3(:,k) = randn ;

% agent 1
% -------------------------------------------------------------------------

Pertb_ang1(:,k) = Ran_1(:,k)*pertb_ang ;
th1(:,k+1) = th1(:,k) + ang_u(:,k) %+ Pertb_ang1(:,k);  % good

if (th1(:,k+1) >= 360) && (th1(:,k+1) < 720)
th1(:,k+1) = th1(:,k+1) - 360 ;
elseif (th1(:,k+1) >=720) && (th1(:,k+1) < 1080)
th1(:,k+1) = th1(:,k+1) - 720 ;
elseif (th1(:,k+1) >=1080) && (th1(:,k+1) < 1440)
th1(:,k+1) = th1(:,k+1) - 1080 ;
elseif (th1(:,k+1) >= 1440) && ( th1(:,k+1) < 1800 )
th1(:,k+1) = th1(:,k+1) -1440 ;
elseif ( th1(:,k+1) < -360 ) && ( th1(:,k+1) >= -720)
th1(:,k+1) = th1(:,k+1) + 360 ;
elseif ( th1(:,k+1) < -720 ) && (th1(:,k+1) >= -1080)
th1(:,k+1) = th1(:,k+1)+ 720 ;
else
th1(:,k+1) = th1(:,k+1) ;
end

% agent 2
% -------------------------------------------------------------------------

Pertb_ang2(:,k) = Ran_2(:,k)*pertb_ang ;
th2(:,k+1) = th2(:,k) + ang_u(:,k)%+ Pertb_ang2(:,k);

if (th2(:,k+1) >= 360) && (th2(:,k+1) < 720)
th2(:,k+1) = th2(:,k+1) - 360 ;
elseif (th2(:,k+1) >=720) && (th2(:,k+1) < 1080)
th2(:,k+1) = th2(:,k+1) - 720 ;
elseif (th2(:,k+1) >=1080) && (th2(:,k+1) < 1440)
th2(:,k+1) = th2(:,k+1) - 1080 ;
elseif (th2(:,k+1) >= 1440) && ( th2(:,k+1) < 1800 )
th2(:,k+1) = th2(:,k+1) -1440 ;
elseif ( th2(:,k+1) < -360 ) && ( th2(:,k+1) >= -720)
th2(:,k+1) = th2(:,k+1) + 360 ;
elseif ( th2(:,k+1) < -720 ) && (th2(:,k+1) >= -1080)
th2(:,k+1) = th2(:,k+1)+ 720 ;
else
th2(:,k+1) = th2(:,k+1) ;
end

% agent 3
% -------------------------------------------------------------------------

Pertb_ang3(:,k) = Ran_3(:,k)*pertb_ang ;
th3(:,k+1) = th3(:,k) + ang_u(:,k) %+ Pertb_ang3(:,k);

if (th3(:,k+1) >= 360) && (th3(:,k+1) < 720)
th3(:,k+1) = th3(:,k+1) - 360 ;
elseif (th3(:,k+1) >=720) && (th3(:,k+1) < 1080)
th3(:,k+1) = th3(:,k+1) - 720 ;
elseif (th3(:,k+1) >=1080) && (th3(:,k+1) < 1440)
th3(:,k+1) = th3(:,k+1) - 1080 ;
elseif (th3(:,k+1) >= 1440) && ( th3(:,k+1) < 1800 )
th3(:,k+1) = th3(:,k+1) -1440 ;
elseif ( th3(:,k+1) < -360 ) && ( th3(:,k+1) >= -720)
th3(:,k+1) = th3(:,k+1) + 360 ;
elseif ( th3(:,k+1) < -720 ) && (th3(:,k+1) >= -1080)
th3(:,k+1) = th3(:,k+1)+ 720 ;
else
th3(:,k+1) = th3(:,k+1) ;
end

p11(:,k+1) = p11(:,k) + u1(:,k)*c1 - u2(:,k)*s1 ;
p12(:,k+1) = p12(:,k) + u1(:,k)*s1 + u2(:,k)*c1 ;

p21(:,k+1) = p21(:,k) + u1(:,k)*c2 - u2(:,k)*s2 ;
p22(:,k+1) = p22(:,k) + u1(:,k)*s2 + u2(:,k)*c2 ;

p31(:,k+1) = p31(:,k) + u1(:,k)*c3 - u2(:,k)*s3 ;
p32(:,k+1) = p32(:,k) + u1(:,k)*s3 + u2(:,k)*c3 ;

% For LP angle validation
% -------------------------------------------------------------------------
% ang1(:,k) = atand(q12/q11);
% ang2(:,k) = atand(q22/q21);
% ang3(:,k) = atand(q32/q31);

% Total Path Covered
% -------------------------------------------------------------------------

U1(k) = sqrt(u1(:,k)*u1(:,k)+u2(:,k)*u2(:,k));
U = U + U1(k) ;

d1=U1(k)/50;

p111(1)=p11(:,k);
p112(1)=p12(:,k);

p211(1)=p21(:,k);
p212(1)=p22(:,k);

p311(1)=p31(:,k);
p312(1)=p32(:,k);

for i = 1:25

p111(i+1)=p111(i)+d1*cosd(ang_u(:,k)+th1(:,k));
p112(i+1)=p112(i)+d1*sind(ang_u(:,k)+th1(:,k));

p211(i+1)=p211(i)+d1*cosd(ang_u(:,k)+th2(:,k));
p212(i+1)=p212(i)+d1*sind(ang_u(:,k)+th2(:,k));

p311(i+1)=p311(i)+d1*cosd(ang_u(:,k)+th3(:,k));
p312(i+1)=p312(i)+d1*sind(ang_u(:,k)+th3(:,k));

%     plot([p111(i) p111(i+1)],[p112(i) p112(i+1)],'ro','LineWidth',1,...
%                 'MarkerEdgeColor','r',...
%                 'MarkerFaceColor','g',...
%                 'MarkerSize',2);
% %      axis([0 10 0 10])
%     axis equal
%
%         xlabel('X-Axis');
%
%             ylabel('Y-Axis');
%
% %               axis([0,2*pi,-1,1])
% %
% %           S1=sprintf('r = %.2f' ,t);
% %
% %             text(3,4,S1)
%      title('Trajectory of the Agents');
%     hold on
%
%     plot([p211(i) p211(i+1)],[p212(i) p212(i+1)],'bo','LineWidth',1,...
%                 'MarkerEdgeColor','b',...
%                 'MarkerFaceColor','g',...
%                 'MarkerSize',2);
%     hold on
%
%     plot([p311(i) p311(i+1)],[p312(i) p312(i+1)],'ko','LineWidth',1,...
%                 'MarkerEdgeColor','k',...
%                 'MarkerFaceColor','g',...
%                 'MarkerSize',2);
%     hold on
%
%     F=getframe;
% %     n = n+1;

end

%           S1=sprintf('r = %.2f' ,r);
%
%             text(3,4,S1)

% For Plotting
% -------------------------------------------------------------------------
% trajectory_agent;

% % plot([p11(i) p11(i+1)],[p12(i) p12(i+1)],'r')
% plot([p11(i) p11(i+1)],[p12(i) p12(i+1)],'r','LineWidth',1,...
%                 'MarkerEdgeColor','k',...
%                 'MarkerFaceColor','g',...
%                 'MarkerSize',2)
%
%
% axis([0 10 0 10])
% xlabel('X-axis')
% ylabel('Y-axis')
% % axis equal
% grid on
% drawnow
% % pause(1)
% hold on
% plot([p21(i) p21(i+1)],[p22(i) p22(i+1)],'b-.')
% % plot([p21(i) p21(i+1)],[p22(i) p22(i+1)],'bo','LineWidth',1.5,...
% %                 'MarkerEdgeColor','k',...
% %                 'MarkerFaceColor','g',...
% %                 'MarkerSize',4)
%
%
%
%
% % axis([0 7 0 7])
% drawnow
% % pause(1)
% hold on
% plot([p31(i) p31(i+1)],[p32(i) p32(i+1)],'g')
% % plot([p31(i) p31(i+1)],[p32(i) p32(i+1)],'bo','LineWidth',1.5,...
% %                 'MarkerEdgeColor','k',...
% %                 'MarkerFaceColor','g',...
% %                 'MarkerSize',4)
%
%
% % axis([0 7 0 7])
% drawnow
% % pause(1)
% % F(i)=getframe;
%
% clc

% -------------------------------------------------------------------------

if ( r < 0.002)
break
else
k = k+1 ;
end

end

% max_min_avg

plotresults
%

```