%      LinEqn.M
%
%   Solve linear equations as a consensus problem
%
% J. Lunze
% 28.8.2018
%
% Version of 21.12.2018
% 
close all
clear all

%%  Parameters of the linear equation
%
N1=2;   % number of equations
A=[1 2; 1 -2];    % (N1,N1)-matrix
%A=[1 3; 3 -1];    % solution is obtained in 1 step
%A=[1 2; 2 1];     % many iterations necessary
b=[2; 0];
AGraph=[0 1; 1 0];
%  degree matrix
D=diag(AGraph*ones(N1,1));
epsilon=0.00001;   % precision of the result
kmax=2000;       % maximum number of iterations
q=0.5;          % gain
%   determination of the precise solution
Xtrue=inv(A)*b;
%  Set-up the consensus problem
figure(1)
P=[];
for k1=1:N1
    P(:,:,k1)=eye(N1,N1)...
           -A(k1,:)'*inv(A(k1,:)*A(k1,:)')*A(k1,:);
end
%  initialisation
X0=zeros(N1,N1);      % rows should include the initial states of the nodes
x1=0.25;
X0(1,:)=[x1 b(1)/A(1,2)-A(1,1)/A(1,2)*x1];
x1=0.1;
X0(2,:)=[x1 b(2)/A(2,2)-A(2,1)/A(2,2)*x1];
Xold=X0;
X=[];
X(:,:,1)=X0;
%    Iterate
Xnew=zeros(N1,N1);       % rows should represent the node state at time k+1
precision=norm(X0);
k3=1;
while precision>epsilon && k3<kmax
    k3=k3+1;
    Xs=inv(D)*AGraph*Xold;   % rows include xsi(k)
    for k1=1:N1
        Xnew(k1,:)=Xold(k1,:)+q*(Xs(k1,:)-Xold(k1,:))*P(:,:,k1)';
    end
        X(:,:,k3)=Xnew;
        precision=norm(Xnew-Xold);
        Xold=Xnew;
end
%   plot the first element of the state vectors of the nodes
Xplot=[];
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,1,k1);  % first component of the state vector
    end
end
%  k3 is the number of iterations used
subplot(2,1,1)
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i1}$$');
rotateY;
xleer;
yticks([0 0.5 1]);
axis([0 50 0 1.1]);
hold on;
plot(k3-1, Xtrue(1), 'bo');
latextitle('$${\tt LinEqn:}$$ Solve a linear equation as a consensus problem');
hold off;
%    plot the second element
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,2,k1);  % second component of the state vector
    end
end
subplot(2,1,2)
stairs([0:k3-1] ,Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i2}$$');
rotateY;
latexxlabel('$$k$$');
yticks([0 0.5 1]);
xticks([0 10 20 30 40 50]);
axis([0 50 0 1.1]);
hold on;
plot(k3-1, Xtrue(2), 'bo');
hold off;

%%    Illustration of the consensus dynamics
%
figure(2)
q=1;
plot(X0(1,1), X0(1,2), 'bo', X0(2,1), X0(2,2), 'bo');
hold on
% draw the lines  A'_1 x=b_1 and A'_2 x=b_2
X1=0:0.1:2;
X2=b(1)/A(1,2)-A(1,1)/A(1,2)*X1;
plot(X1, X2, 'k');
X2=b(2)/A(2,2)-A(2,1)/A(2,2)*X1;
plot(X1, X2, 'k');   
Xold=X0;
%    Iterate
for k1=1:5
    plot([Xold(1,1) Xold(2,1)], [Xold(1,2), Xold(2,2)], 'b');
    Xs=[Xold(2,:); Xold(1,:)];
    Xnew(1,:)=Xold(1,:)+q*(Xs(1,:)-Xold(1,:))*P(:,:,1)';
    Xnew(2,:)=Xold(2,:)+q*(Xs(2,:)-Xold(2,:))*P(:,:,2)';
    plot([Xold(2,1), Xnew(1,1)], [Xold(2,2), Xnew(1,2)], 'b--');
    plot([Xold(1,1), Xnew(2,1)], [Xold(1,2), Xnew(2,2)], 'b--');
    plot(Xnew(1,1), Xnew(1,2), 'bo');
    plot(Xnew(2,1), Xnew(2,2), 'bo');
    Xold=Xnew;
end
plot(X0(1,1), X0(1,2), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
plot(X0(2,1), X0(2,2), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
latextitle('$${\tt LinEqn:}$$ Illustration of the solution process');
latexxlabel('$$x_1$$');
latexylabel('$$x_2$$');
rotateY;
xticks([0 0.5 1]);
yticks([0 0.5 1]);
axis([0 1.1 0 1.1]);
axis('square');
hold off



%%  New parameters of the linear equation
%
figure(3)
N1=4;   % number of equations
A=[3 1 3 1; 2 2 1 0; 0 1 3 2; 1 0 0 2];    % (N1,N1)-matrix
b=[3.2; 2.5; 3; 1.8];
AGraph=[0 1 0 0; 1 0 1 1; 0 1 0 0; 0 1 0 0];
%  degree matrix
D=diag(AGraph*ones(N1,1));
epsilon=0.001;   % precision of the result
kmax=2000;       % maximum number of iterations
q=0.3;          % gain
%   determination of the precise solution
Xtrue=inv(A)*b;
%  Set-up the consensus problem
P=[];
for k1=1:N1
    P(:,:,k1)=eye(N1,N1)...
           -A(k1,:)'*inv(A(k1,:)*A(k1,:)')*A(k1,:);
end
%  initialisation
X0=zeros(N1,N1);      % rowss should include the initial states of the nodes
for k1=1:N1
   k2=0;
   x0NotFound=1;
   if A(k1,k1)~=0
       X0(k1,k1)=b(k1)/A(k1,k1);
       x0NotFound=0;
   end
   while x0NotFound
       k2=k2+1;
       if A(k1,k2)~=0
           X0(k1,k2)=b(k1)/A(k1,k2);
           x0NotFound=0;
       end
   end
end
Xold=X0;
X=[];
X(:,:,1)=X0;
%    Iterate
Xnew=zeros(N1,N1);       % rows should represent the node state at time k+1
precision=norm(X0);
k3=1;
while precision>epsilon && k3<kmax
    k3=k3+1;
    Xs=inv(D)*AGraph*Xold;   % rows include xsi(k)
    for k1=1:N1
        Xnew(k1,:)=Xold(k1,:)+q*(Xs(k1,:)-Xold(k1,:))*P(:,:,k1)';
    end
        X(:,:,k3)=Xnew;
        precision=norm(Xnew-Xold);
        Xold=Xnew;
end
%   k3 is the number of iterations used
%   plot the first element of the state vectors of the nodes
kplot=25;    % number of iterations plotted
subplot(8,1,[1 2])
Xplot=[];
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,1,k1);  % first component of the state vector
    end
end
 stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i1}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(1), 'bo');
latextitle('$${\tt LinEqn:}$$ Solve a linear equation as a consensus problem');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0 1]);
%    plot the second element
subplot(8,1,[3 4])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,2,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i2}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(2), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0  1]);
%    plot the third element
subplot(8,1,[5 6])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,3,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i3}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(3), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0 1]);
%    plot the fourth element
subplot(8,1,[7 8])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,4,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i4}$$');
rotateY;
latexxlabel('$$k$$');
hold on;
plot(kplot, Xtrue(4), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0  1]);
xticks([0 5 10 15 20 25 30]);



%%  The same equation, but different communication structure
%
figure(4)
AGraph=[0 0 0 1; 1 0 0 0; 0 1 0 0; 0 0 1 0];
%  degree matrix
D=diag(AGraph*ones(N1,1));
epsilon=0.001;   % precision of the result
kmax=2000;       % maximum number of iterations
q=0.3;          % gain
%   determination of the precise solution
Xtrue=inv(A)*b;
%  Set-up the consensus problem
P=[];
for k1=1:N1
    P(:,:,k1)=eye(N1,N1)...
           -A(k1,:)'*inv(A(k1,:)*A(k1,:)')*A(k1,:);
end
%  initialisation
X0=zeros(N1,N1);      % rowss should include the initial states of the nodes
for k1=1:N1
   k2=0;
   x0NotFound=1;
   if A(k1,k1)~=0
       X0(k1,k1)=b(k1)/A(k1,k1);
       x0NotFound=0;
   end
   while x0NotFound
       k2=k2+1;
       if A(k1,k2)~=0
           X0(k1,k2)=b(k1)/A(k1,k2);
           x0NotFound=0;
       end
   end
end
Xold=X0;
X=[];
X(:,:,1)=X0;
%    Iterate
Xnew=zeros(N1,N1);       % rows should represent the node state at time k+1
precision=norm(X0);
k3=1;
while precision>epsilon && k3<kmax
    k3=k3+1;
    Xs=inv(D)*AGraph*Xold;   % rows include xsi(k)
    for k1=1:N1
        Xnew(k1,:)=Xold(k1,:)+q*(Xs(k1,:)-Xold(k1,:))*P(:,:,k1)';
    end
        X(:,:,k3)=Xnew;
        precision=norm(Xnew-Xold);
        Xold=Xnew;
end
%   k3 is the number of iterations used
%   plot the first element of the state vectors of the nodes
kplot=25;    % number of iterations plotted
subplot(8,1,[1 2])
Xplot=[];
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,1,k1);  % first component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i1}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(1), 'bo');
latextitle('$${\tt LinEqn:}$$ Solve a linear equation as a consensus problem');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0 1]);
%    plot the second element
subplot(8,1,[3 4])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,2,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i2}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(2), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0 1]);
%    plot the third element
subplot(8,1,[5 6])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,3,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i3}$$');
rotateY;
xleer;
hold on;
plot(kplot, Xtrue(3), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0 1]);
%    plot the fourth element
subplot(8,1,[7 8])
for k1=1:k3          %  number of time points
    for k2=1:N1      %  number of agents
        Xplot(k1,k2)=X(k2,4,k1);  % second component of the state vector
    end
end
stairs([0:k3-1], Xplot,'Color','b','LineWidth',1.5);
latexylabel('$$x_{i4}$$');
rotateY;
latexxlabel('$$k$$');
hold on;
plot(kplot, Xtrue(4), 'bo');
hold off;
axis([0 kplot -0.1 1.2]);
yticks([0  1]);
xticks([0 5 10 15 20 25 30]);


%%  Illustration of the convergence rate in dependence upon the matrix A
%
figure(5)
subplot(1,2,1)
N1=2;
A=[1 3; 3 -1];    % solution is obtained in 1 step
%A=[1 2; 2 1];     % many iterations necessary
b=[3; 0];
AGraph=[0 1; 1 0];
%  degree matrix
D=diag(AGraph*ones(N1,1));
epsilon=0.00001;   % precision of the result
kmax=2000;       % maximum number of iterations
q=1;          % gain
%   initial states
X0=zeros(N1,N1);      % rows should include the initial states of the nodes
x1=0.9;
X0(1,:)=[x1 b(1)/A(1,2)-A(1,1)/A(1,2)*x1];
x1=0.1;
X0(2,:)=[x1 b(2)/A(2,2)-A(2,1)/A(2,2)*x1];
Xold=X0;
plot(X0(1,1), X0(1,2), 'bo', X0(2,1), X0(2,2), 'bo');
hold on
% draw the lines  A'_1 x=b_1 and A'_2 x=b_2
X1=0:0.1:2;
X2=b(1)/A(1,2)-A(1,1)/A(1,2)*X1;
plot(X1, X2, 'k');
X2=b(2)/A(2,1)-A(2,1)/A(2,2)*X1;
plot(X1, X2, 'k');   
Xold=X0;
Xnew=[];
%  set-up the consensus problem
P=[];
for k1=1:N1
    P(:,:,k1)=eye(N1,N1)...
           -A(k1,:)'*inv(A(k1,:)*A(k1,:)')*A(k1,:);
end
%    Iterate
for k1=1:5
    plot([Xold(1,1) Xold(2,1)], [Xold(1,2), Xold(2,2)], 'b');
    Xs=[Xold(2,:); Xold(1,:)];
    Xnew(1,:)=Xold(1,:)+q*(Xs(1,:)-Xold(1,:))*P(:,:,1)';
    Xnew(2,:)=Xold(2,:)+q*(Xs(2,:)-Xold(2,:))*P(:,:,2)';
    plot([Xold(2,1), Xnew(1,1)], [Xold(2,2), Xnew(1,2)], 'b--');
    plot([Xold(1,1), Xnew(2,1)], [Xold(1,2), Xnew(2,2)], 'b--');
    plot(Xnew(1,1), Xnew(1,2), 'bo');
    plot(Xnew(2,1), Xnew(2,2), 'bo');
    Xold=Xnew;
end
latextitle('$${\tt LinEqn:}$$ Illustration of the solution process');
latexxlabel('$$x_1$$');
latexylabel('$$x_2$$');
rotateY;
xticks([0 0.5 1]);
yticks([0 0.5 1]);
axis([0 1.1 0 1.1]);
axis('square');
hold off
%
%   Second example
subplot(1,2,2)
A=[1 2; 2 2];     % many iterations necessary
b=[2; 2.2];
%   initial states
X0=zeros(N1,N1);      % rows should include the initial states of the nodes
x1=1;
X0(1,:)=[x1 b(1)/A(1,2)-A(1,1)/A(1,2)*x1];
x1=0.8;
X0(2,:)=[x1 b(2)/A(2,2)-A(2,1)/A(2,2)*x1];
plot(X0(1,1), X0(1,2), 'bo', X0(2,1), X0(2,2), 'bo');
hold on
% draw the lines  A'_1 x=b_1 and A'_2 x=b_2
X1=0:0.1:2;
X2=b(1)/A(1,2)-A(1,1)/A(1,2)*X1;
plot(X1, X2, 'k');
X2=b(2)/A(2,2)-A(2,1)/A(2,2)*X1;
plot(X1, X2, 'k');   
Xold=X0;
Xnew=[];
%  set-up the consensus problem
P=[];
for k1=1:N1
    P(:,:,k1)=eye(N1,N1)...
           -A(k1,:)'*inv(A(k1,:)*A(k1,:)')*A(k1,:);
end
%    Iterate
for k1=1:22
    plot([Xold(1,1) Xold(2,1)], [Xold(1,2), Xold(2,2)], 'b');
    Xs=[Xold(2,:); Xold(1,:)];
    Xnew(1,:)=Xold(1,:)+q*(Xs(1,:)-Xold(1,:))*P(:,:,1)';
    Xnew(2,:)=Xold(2,:)+q*(Xs(2,:)-Xold(2,:))*P(:,:,2)';
    plot([Xold(2,1), Xnew(1,1)], [Xold(2,2), Xnew(1,2)], 'b--');
    plot([Xold(1,1), Xnew(2,1)], [Xold(1,2), Xnew(2,2)], 'b--');
    plot(Xnew(1,1), Xnew(1,2), 'bo');
    plot(Xnew(2,1), Xnew(2,2), 'bo');
    Xold=Xnew;
end
latexxlabel('$$x_1$$');
%latexylabel('$$x_2$$');
%rotateY;
yleer;
xticks([0 0.5 1]);
yticks([0 0.5 1]);
axis([0 1.1 0 1.1]);
axis('square');
hold off

%%
%
epsfigc15('LinEqn');

