I am new to multi-agent robotic systems and I am currently studying consensus and flocking algorithm. Recently I implemented a simple integrator consensus algorithm that leads a group of N robots to a single point. The dynamics of robots are defined as $\dot{x}_i=u_i$ where $x_i$ is the position of robot $i$ and $u_i$ is its control law. I want to add a repulsive force between the robots ( between robot $i$ and robot $i-1$ for example) to my consensus law to study the behavior of the robots but I didn't figure out how to add it. My consensus law is defined as :
X_dot=-L*X.' ;
U(:,i)= X_dot(i,:).'
% update the position of robot i using its dynamics
X(:,i)=X(:,i)+f(X(:,i),U(:,i))*dt;
Where L is the laplacian matrix. The consensus law works fine, I just don't know how to add the repulsive force..