├── Main.m ├── README.md ├── agent.m ├── func_U.m ├── func_phi.m ├── func_rho.m ├── generate_region.m ├── vec_Phi.m └── vec__phi.m /Main.m: -------------------------------------------------------------------------------- 1 | clc,clear,close all; 2 | %% Generate the Scalar Field 3 | m=25; % The number of radial basis function used to generate the field is m 4 | 5 | reg_x=[-5,5]; % These are x and y limite for the region 6 | reg_y=[-5,5]; 7 | 8 | 9 | % Kernels=[(reg_x(2)-reg_x(1))*rand(m,1)+reg_x(1) (reg_y(2)-reg_y(1))*rand(m,1)+reg_y(1)]; % This contains the positions of the RBFs 10 | Kernels=[kron(linspace(reg_x(1),reg_x(2),5)/1.1,[1 1 1 1 1])',repmat(linspace(reg_y(1),reg_y(2),5)/1.1,1,5)']; 11 | % Gamma=0.1*abs(randn(m,1)); % Gamma in the RBFs 12 | Gamma=0.1*ones(m,1); 13 | % sigma=2*abs(randn(m,1)); % sigma in the RBFs 14 | sigma=1*ones(m,1); 15 | % Theta=0.1*abs(randn(m,1)); % scaling factors of RBFs 16 | Theta=zeros(m,1); 17 | Theta(19)=5; % This set a peak for the field 18 | Theta(7)=5; 19 | % This returns the true scalar value of the whole field 20 | mesh_value = generate_region(reg_x,reg_y,m,Gamma,sigma,Kernels,Theta); 21 | 22 | M=size(mesh_value,1); 23 | [mesh_x,mesh_y]=meshgrid(linspace(reg_x(1),reg_x(2),M),flip(linspace(reg_y(1),reg_y(2),M))); 24 | image(linspace(reg_x(1),reg_x(2),M),linspace(reg_y(2),reg_y(1),M),mesh_value,'CDataMapping','scaled'); 25 | % 26 | title("Initialization of agents and fields") 27 | % view(2); 28 | % plot3(mesh_x,mesh_y,255.*mesh_value./max(max(mesh_value))) 29 | % hold on; 30 | % surf(mesh_x,mesh_y,mesh_value,'edgecolor', 'none'); 31 | % hold off; 32 | %% Global parameters definition 33 | N=25; % Number of agents 34 | d=0.6; % a parameter 35 | d0=1.5*d; % a parameter 36 | d1=3.5*d; % a parameter 37 | r=4*d; % The communication range of an agent 38 | s=1; % Sampling rate of agents 39 | sigma_w=1; % variance of the sampling noise 40 | %% Agents initialization 41 | t=0; 42 | InitialPosi=randn(N,2); % Initialize the agent positions 43 | InitialPosi(InitialPosi>5)=4; % correct positions outside the border 44 | InitialPosi(InitialPosi<-5)=-4; 45 | Agents=agent.empty(N,0); 46 | for n=1:N 47 | Agents(n).Code=n; 48 | Agents(n).Position=InitialPosi(n,:); 49 | Agents(n).CommuDist=r; 50 | Agents(n).Speed=0; 51 | Agents(n).d=d; 52 | Agents(n).d0=d0; 53 | Agents(n).d1=d1; 54 | Agents(n).Theta_est=zeros(m,1); 55 | Agents(n).P=eye(m); 56 | Agents(n).Gamma=Gamma; 57 | Agents(n).sigma=sigma; 58 | Agents(n).Kernels=Kernels; 59 | Agents(n).v=[0;0]; 60 | Agents(n).gamma=0.2; 61 | Agents(n).k3=0.1; 62 | Agents(n).k4=0.1; 63 | Agents(n).delta_t=1; 64 | Agents(n).BorderLimitX=reg_x; 65 | Agents(n).BorderLimitY=reg_y; 66 | Agents(n).PastPositions=[]; 67 | end 68 | 69 | hold on; 70 | scatter(InitialPosi(:,1),InitialPosi(:,2),'g*'); 71 | scatter(Kernels(:,1),Kernels(:,2),'ro'); 72 | Agents=Agents.UpdateNeighbour; 73 | 74 | %% Distributed Learning 75 | IterationFlag=1; 76 | IterNumMax=500; 77 | IterCount=0; 78 | figure(2), 79 | errorRecord=[]; 80 | while IterationFlag 81 | IterCount=IterCount+1; 82 | 83 | Agents=Agents.Measure(Theta,Kernels,Gamma,sigma,sigma_w); % The agents measure their local scalar field values 84 | Agents=Agents.ReceiveNeighbourMeasurements; % The agents exchange their measurement with neighbours 85 | Agents=Agents.Learn; % The agents estimate their local field with the measurement 86 | Agents=Agents.GenerateControl; % The control signal is generated, which will be used to move the agents 87 | Agents=Agents.Move; % The agents move to new positions according to the control signal 88 | Agents=Agents.UpdateNeighbour; % agents update their new list of neighbours 89 | 90 | for i=1:N % Hard decision that restrict the agents stay inside the region M of interest 91 | Agents(i).Position(Agents(i).Position>=5)=4.99; 92 | Agents(i).Position(Agents(i).Position<=-5)=-4.99; 93 | Agents(i).PastPositions=[Agents(i).PastPositions;Agents(i).Position]; 94 | end 95 | 96 | Posi=reshape([Agents.Position],[2,N])'; 97 | 98 | % plot the current agent positions and the estimated field of agent 1 99 | if ~mod(IterCount,50) 100 | figure(2), 101 | mesh_value_est_1 = generate_region(reg_x,reg_y,m,Gamma,sigma,Kernels,Agents(1).Theta_est); 102 | image(linspace(reg_x(1),reg_x(2),M),linspace(reg_y(2),reg_y(1),M),mesh_value_est_1,'CDataMapping','scaled'); 103 | hold on; 104 | for i=1:N 105 | temp=Agents(i).PastPositions; 106 | plot(temp(:,1),temp(:,2),'r'); 107 | end 108 | scatter(Posi(:,1),Posi(:,2),'g*'); 109 | scatter(Agents(1).Position(1),Agents(1).Position(2),'r*'); 110 | scatter(Kernels(:,1),Kernels(:,2),'ro'); 111 | hold off; 112 | end 113 | 114 | pause(0.001) 115 | if IterCount>IterNumMax 116 | IterationFlag=0; 117 | end 118 | errorRecord=[errorRecord ReportMSEError(Agents)]; 119 | end 120 | 121 | figure(2), 122 | mesh_value_est_1 = generate_region(reg_x,reg_y,m,Gamma,sigma,Kernels,Agents(1).Theta_est); 123 | image(linspace(reg_x(1),reg_x(2),M),linspace(reg_y(2),reg_y(1),M),mesh_value_est_1,'CDataMapping','scaled'); 124 | hold on; 125 | for i=1:N 126 | temp=Agents(i).PastPositions; 127 | plot(temp(:,1),temp(:,2),'r'); 128 | end 129 | scatter(Kernels(:,1),Kernels(:,2),'ro'); 130 | scatter(Posi(:,1),Posi(:,2),'g*'); 131 | scatter(Agents(1).Position(1),Agents(1).Position(2),'r*'); 132 | title("Final positions of agents and estimated field of agent-1") 133 | hold off; 134 | 135 | figure,semilogy(errorRecord); 136 | title("MSE - Iteration") 137 | 138 | 139 | 140 | 141 | 142 | 143 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This project contains codes based on paper 2 | 3 | Jongeun Choi, Songhwai Oh, Roberto Horowitz, *Distributed learning and cooperative control for multi-agent systems*, Automatica, Volume 45, Issue 12, 2009 4 | 5 | -------------------------------------------------------------------------------- /agent.m: -------------------------------------------------------------------------------- 1 | classdef agent 2 | 3 | properties 4 | Code % agent serial number 5 | BorderLimitX % X limit of the whole region 6 | BorderLimitY % Y limit of the whole region 7 | CommuDist % The farthest distance of communication of between two agents 8 | Position % The current position of an agent 9 | Neighbour % record the neighbors of an agents (not including itself, agent code from small to large) 10 | Speed % This variable is useless, replaced by variable v below 11 | SampleNumber % number of samples from self and neighbors 12 | RealLocalValue % The real value of local scalar field 13 | Measurement_self % The measured value of an agent itself 14 | Measurement_around % The collected measurements from the neighbor of an agent 15 | Ys % The measured values from itself and its neighbors 16 | K % a variable in the distributed learning stage (field estimation) 17 | P % a variable in the distributed learning stage (field estimation) 18 | d % a parameter related to the inner potential of the agent flock 19 | d0 % a parameter related to the inner potential of the agent flock 20 | d1 % a parameter related to the inner potential of the agent flock 21 | Phi_star % a variable used in the distributed learning stage (field estimation) 22 | Theta_est % the estimated parameters of the field 23 | Mu_est % the estimated value of current agent position 24 | Gamma % scalar values of the radial basis functions 25 | sigma % variance of the radial basis functions 26 | Kernels % kernel positions of the radial basis functions 27 | v % the control of the agent, can be regarded as its speed 28 | gamma % a standard adaptive gain sequence, reduces as iteration number goes up 29 | delta_t % the time between two update of estimation and movement 30 | k_di % a scalar of the importance of speed feedback in control stage 31 | k3 % a scalar of the importance of flock speed in control stage 32 | k4 % a scalar of the importance of field gradient in control stage 33 | PastPositions 34 | end 35 | 36 | methods 37 | function obj = agent(CommuDist,Position) 38 | if nargin>0 39 | obj.CommuDist=CommuDist; 40 | obj.Position=Position; 41 | end 42 | end 43 | 44 | % function MEASURE gives the current measurement of the agent 45 | function obj=Measure(obj,theta,Kernel,Gamma,sigma,sigma_w) 46 | N=length(obj); 47 | for i=1:N 48 | % RealValue=0; 49 | Phi_star=vec_Phi(obj(i).Position,Kernel,Gamma,sigma); 50 | RealValue=Phi_star*theta; 51 | obj(i).RealLocalValue=RealValue; 52 | MeasureValue=RealValue+sigma_w*randn; 53 | obj(i).Measurement_self=MeasureValue; 54 | end 55 | end 56 | 57 | % function REPORTMSEERROR calculate the current MSE of the agents 58 | % estimation of the scalar field 59 | function error=ReportMSEError(obj) 60 | N=length(obj); 61 | error=0; 62 | for i=1:N 63 | error=error+(obj(i).Mu_est-obj(i).RealLocalValue)^2; 64 | end 65 | error =error/N; 66 | end 67 | 68 | % function UPDATENEIGHBOUR update the neighbours of agents 69 | function obj=UpdateNeighbour(obj) 70 | N=length(obj); 71 | for i=1:N 72 | obj(i).Neighbour=[]; 73 | end 74 | for i=1:N-1 75 | for j=i+1:N 76 | Dist=norm(obj(i).Position-obj(j).Position); 77 | if obj(i).CommuDist>Dist 78 | obj(i).Neighbour=[obj(i).Neighbour j]; 79 | obj(j).Neighbour=[obj(j).Neighbour i]; 80 | end 81 | end 82 | end 83 | for i=1:N 84 | obj(i).SampleNumber=1+length(obj(i).Neighbour); 85 | end 86 | end 87 | 88 | % function RECEIVENEIGHBOURMEASUREMENTS let agents collect 89 | % measurement values from their neighbours 90 | function obj=ReceiveNeighbourMeasurements(obj) 91 | N=length(obj); 92 | for i=1:N 93 | ThisAgent=obj(i); 94 | L=length(ThisAgent.Neighbour); 95 | s=L+1; 96 | ThisAgent.Measurement_around=[]; 97 | for j=ThisAgent.Neighbour 98 | ThisAgent.Measurement_around=[ThisAgent.Measurement_around obj(j).Measurement_self]; 99 | end 100 | ThisAgent.Ys=[ThisAgent.Measurement_self ThisAgent.Measurement_around]; 101 | B=sortrows([[ThisAgent.Code;ThisAgent.Neighbour'],ThisAgent.Ys']); 102 | ThisAgent.Ys=B(:,2); 103 | obj(i)=ThisAgent; 104 | end 105 | end 106 | 107 | % function LEARN is the recursive update the estimation of the 108 | % scalar field 109 | function obj=Learn(obj) 110 | N=length(obj); 111 | for i=1:N 112 | Y_star=obj(i).Ys; 113 | kernel=obj(i).Kernels; 114 | G=obj(i).Gamma; 115 | s=obj(i).sigma; 116 | m=size(kernel,1); 117 | Positions=obj(i).Position; 118 | for j=obj(i).Neighbour 119 | Positions=[Positions;obj(j).Position]; 120 | end 121 | B=sortrows([[obj(i).Code;obj(i).Neighbour'],Positions]); 122 | Positions=B(:,2:3); 123 | Phi_star=vec_Phi(Positions,kernel,G,s); 124 | % Update 125 | obj(i).K=obj(i).P*Phi_star'*inv(eye(obj(i).SampleNumber)+Phi_star*obj(i).P*Phi_star'); 126 | obj(i).P=(eye(m)-obj(i).K*Phi_star)*obj(i).P; 127 | obj(i).Theta_est=obj(i).Theta_est+obj(i).K*(Y_star-Phi_star*obj(i).Theta_est); 128 | obj(i).Mu_est=vec_Phi(obj(i).Position,kernel,G,s)*obj(i).Theta_est; 129 | end 130 | 131 | end 132 | 133 | % function CALCDIFFPOTENTIALU calculate the potential U1 used in 134 | % control stage 135 | function DiffPotential=CalcDiffPotentialU(obj,obj_self) 136 | % obj=obj_all; 137 | alpha=0.10; 138 | beta=1; 139 | DiffPotential=[0,0]; 140 | k1=9; 141 | k2=0; 142 | k1=k1/(k1+k2); 143 | k2=1-k1; 144 | for j=obj_self.Neighbour 145 | r_ij=norm(obj_self.Position-obj(j).Position)^2; 146 | if r_ij<(obj_self.d0^2) 147 | DiffPotential=DiffPotential+(r_ij-obj_self.d^2)*(obj_self.Position-obj(j).Position)/(alpha+r_ij)^2; 148 | else 149 | DiffPotential=DiffPotential+func_rho((sqrt(r_ij)-obj_self.d0)/(abs(obj_self.d1-obj_self.d0)))*norm(obj_self.d0^2-obj_self.d^2)/(alpha+obj_self.d0^2)^2*(obj_self.Position-obj(j).Position); 150 | end 151 | end 152 | PotentialInner=DiffPotential; 153 | order=2; 154 | PotentialBorder1=-beta/(obj_self.Position(1)-obj_self.BorderLimitX(1))^order+beta/(obj_self.Position(1)-obj_self.BorderLimitX(2))^order; 155 | PotentialBorder2=-beta/(obj_self.Position(2)-obj_self.BorderLimitY(1))^order+beta/(obj_self.Position(2)-obj_self.BorderLimitY(2))^order; 156 | PotentialBorder=[PotentialBorder1,PotentialBorder2]; 157 | PotentialBorder(abs(PotentialBorder)>50)=PotentialBorder(abs(PotentialBorder)>50)*0.1; 158 | DiffPotential=k1*PotentialInner+k2*PotentialBorder; 159 | end 160 | 161 | % function GENRATECONTROL generate the control signal v for the 162 | % agents 163 | function obj=GenerateControl(obj) 164 | N=length(obj); 165 | k_di=1.5*eye(2); 166 | delta_t=obj(1).delta_t; 167 | for i=1:N 168 | DiffPotential=1*obj.CalcDiffPotentialU(obj(i)); 169 | u_i=-0.1*DiffPotential'-k_di*(delta_t/obj(i).gamma)*(obj(i).v); 170 | for j=obj(i).Neighbour 171 | u_i=u_i+obj(i).k3*(delta_t*(obj(j).v-obj(i).v)/obj(i).gamma); 172 | end 173 | u_i=u_i+obj(i).k4*(vec__phi(obj(i).Position,obj(i).Kernels,obj(i).Gamma,obj(i).sigma)./obj(i).sigma.^2.*(-obj(i).Position+obj(i).Kernels))'*obj(i).Theta_est; 174 | gamma_old=obj(i).gamma; 175 | obj(i).gamma=obj(i).gamma*1/1.005; 176 | obj(i).v=obj(i).gamma/delta_t*(delta_t/gamma_old*obj(i).v+gamma_old*u_i); 177 | end 178 | end 179 | 180 | % function MOVE let agents move to a new position according to 181 | % control signal v 182 | function obj=Move(obj) 183 | N=length(obj); 184 | for i=1:N 185 | obj(i).Position=obj(i).Position+obj(i).delta_t*obj(i).v'; 186 | end 187 | end 188 | 189 | end 190 | end 191 | 192 | -------------------------------------------------------------------------------- /func_U.m: -------------------------------------------------------------------------------- 1 | function [outputArg1,outputArg2] = func_U(inputArg1,inputArg2) 2 | %FUNC_U Summary of this function goes here 3 | % Detailed explanation goes here 4 | outputArg1 = inputArg1; 5 | outputArg2 = inputArg2; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /func_phi.m: -------------------------------------------------------------------------------- 1 | function value = func_phi(v,k,Gamma,sigma) 2 | %FUNC_PHI Summary of this function goes here 3 | % Detailed explanation goes here 4 | v_k=v-k; 5 | v_k2=vecnorm(v_k,2,3).^2; 6 | value=1./Gamma.*exp(-v_k2./(sigma.^2)/2); 7 | end 8 | 9 | -------------------------------------------------------------------------------- /func_rho.m: -------------------------------------------------------------------------------- 1 | function rho = func_rho(z) 2 | 3 | h=0.2; 4 | 5 | rho=zeros(size(z,1),size(z,2)); 6 | rho(z>=0&z=0&z=h&z<=1)=0.5.*(1+cos(pi.*(z(z>=h&z<=1)-h)./(1-h))); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /generate_region.m: -------------------------------------------------------------------------------- 1 | function mesh_value = generate_region(reg_x,reg_y,m,Gamma,sigma,Kernels,theta) 2 | x_min=reg_x(1); 3 | x_max=reg_x(2); 4 | y_min=reg_y(1); 5 | y_max=reg_y(2); 6 | M=512; 7 | x=linspace(x_min,x_max,M); 8 | y=linspace(y_min,y_max,M); 9 | y=flip(y); 10 | [mesh_x,mesh_y]=meshgrid(x,y); 11 | Mesh=zeros(M,M,2); 12 | Mesh(:,:,1)=mesh_x; 13 | Mesh(:,:,2)=mesh_y; 14 | mesh_value=zeros(M,M); 15 | for k=1:m 16 | K=ones(M,M,2); 17 | K(:,:,1)=Kernels(k,1)*K(:,:,1); 18 | K(:,:,2)=Kernels(k,2)*K(:,:,2); 19 | mesh_value = mesh_value+theta(k).*func_phi(Mesh,K,Gamma(k),sigma(k)); 20 | end 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /vec_Phi.m: -------------------------------------------------------------------------------- 1 | function phi = vec_Phi(v,k,Gamma,sigma) 2 | %VEC_PHI Summary of this function goes here 3 | % Detailed explanation goes here 4 | m=size(k,1); 5 | s=size(v,1); 6 | Phi_star=zeros(s,m); 7 | for i=1:s 8 | Phi_star(i,:)=vec__phi(v(i,:),k,Gamma,sigma)'; 9 | end 10 | phi=Phi_star; 11 | end 12 | 13 | -------------------------------------------------------------------------------- /vec__phi.m: -------------------------------------------------------------------------------- 1 | function phi = vec__phi(v,k,Gamma,sigma) 2 | %VEC__PHI Summary of this function goes here 3 | % Detailed explanation goes here 4 | v_k=v-k; 5 | v_k2=vecnorm(v_k').^2; 6 | phi=1./Gamma.*exp(-v_k2'./(sigma.^2)/2); 7 | end 8 | 9 | --------------------------------------------------------------------------------