%% Decentralized Orthogonal Iteration: Calculating k eigenvectors
clc;
clear all;
close all;
%% 먼저 stage에 agent를 뿌리고 neighor를 설정한다.
make_agent_formation; % <= 여기서 stage와 agent변수를 설정
%% 위에서 설정한 agent들로 Adjacency matrix를 만든다.
sig2d = 3; % 거리에 따른 weight의 decay를 결정하는 값.
A = make_adjacency_matrix(agents, stage, sig2d);
%% 1. Calculate eigenvalue and vectors from A
tic;
[e_vec_A e_val_A] = eig(A);
fprintf('\n Eigenvalue decomposition ');toc;
e_vec_A = fliplr(e_vec_A); % flip eval and evec to make ascending order
e_val_A = rot90(e_val_A, 2);
diag_sv = abs(diag(e_val_A));
[~, idx] = sort(diag_sv, 'ascend');
idx(idx') = linspace(stage.nr_agent, 1, stage.nr_agent);
%% 2. Orthogonal Iteration
k = 10; % the number of eigenvectors
Q = randn(stage.nr_agent, k);
maxiter = 1E3;
tic;
for iter = 1:maxiter
V = A*Q;
Q = qr_GS(V);
end
fprintf('\n Orthogonal ITeration '); toc;
% Orthogonal Iteration의 결과를 확인한다.
Qsorted(:, idx) = e_vec_A;
Qref = Qsorted(:, 1:k);
QorthogonalIter = Q;
fprintf('\n Qref: eigenvectors are sorted by its singular value \n');
Qref
QorthogonalIter
error_norm = norm(abs(Qref)-abs(QorthogonalIter));
error_max = max(max(abs(Qref)-abs(QorthogonalIter)));
fprintf('Error norm: %.5f max: %.5f \n', error_norm, error_max);
%% 3. Decentralized Orthogonal Iteration
% 1. 각 agent에 random k-dimensional vector를 설정한다.
for ai = 1:stage.nr_agent
agents{ai}.Q = randn(1, k);
end
% 2. Push-Sum을 위한 col-stochastic mtx B를 만든다.
B = A > 0;
B = B / diag(sum(B)); % <== Make Column-Stochastic
% 3. DOI를 수행한다.
doi.Iter = 1;
doi.maxIter = 5E2;
while 1
fprintf('[%d/%d] \n', doi.Iter, doi.maxIter);
% (DOI-3rd, 4th line)
for ai = 1:stage.nr_agent
% (3rd)
agents{ai}.V = zeros(1, k);
for nj = agents{ai}.nei
agents{ai}.V = agents{ai}.V + A(ai, nj)*agents{nj}.Q;
end
% (4th)
agents{ai}.K = agents{ai}.V'*agents{ai}.V;
end
% (DOI-5th line) Push-Sum
for ai = 1:stage.nr_agent
agents{ai}.S = agents{ai}.K;
agents{ai}.w = 0;
end
agents{1}.w = 1; % <== 하나만 1로 바꾼다.
ps.Iter = 1;
ps.maxIter = 5E2;
while 1
% fprintf('[%d/%d] \n', ps.Iter, ps.maxIter);
% (PS-4th, 5th line) Push-Sum Algorithm
for ai = 1:stage.nr_agent
agents{ai}.Stemp = zeros(size(agents{ai}.S));
agents{ai}.wtemp = 0;
sumB = 0;
for nj = agents{ai}.nei
agents{ai}.Stemp = agents{ai}.Stemp ...
+ B(ai, nj)*agents{nj}.S;
agents{ai}.wtemp = agents{ai}.wtemp ...
+ B(ai, nj)*agents{nj}.w;
sumB = sumB + B(ai, nj);
end
end
% temp 를 ref로 덮어쓴다.
for ai = 1:stage.nr_agent
agents{ai}.S = agents{ai}.Stemp;
agents{ai}.w = agents{ai}.wtemp;
% fprintf(' [%d] w:%.2f s:%.2f', ai, agents{ai}.w, agents{ai}.S);
end
% fprintf('\n');
% Push-Sum 에 대한 종료 조건
ps.Iter = ps.Iter + 1;
if ps.Iter >= ps.maxIter
break;
end
end % 여기서 Push-Sum이 끝났다.
for ai = 1:stage.nr_agent
agents{ai}.S = agents{ai}.S / agents{ai}.w;
end
% (DOI-6th and 7th line)
for ai = 1:stage.nr_agent
agents{ai}.R = chol(agents{ai}.S);
agents{ai}.Q = agents{ai}.V / agents{ai}.R;
end
% DOI 에 대한 종료 조건
doi.Iter = doi.Iter + 1;
if doi.Iter > doi.maxIter
break;
end
end
%% DOI 결과를 저장한다.
Qdoi = [];
for ai = 1:stage.nr_agent
Qdoi = [Qdoi ; agents{ai}.Q];
end
save('Qresults.mat', 'Qref', 'Qdoi', 'QorthogonalIter');
%% 최종 결과를 plot한다.
clc;
clear all;
test_index = 1;
load(['MultiAgentFormation' num2str(test_index) '.mat']);
load(['Qresults' num2str(test_index) '.mat']);
[Qref Qdoi QorthogonalIter];
error_norm = norm(abs(Qref)-abs(QorthogonalIter));
error_max = max(max(abs(Qref)-abs(QorthogonalIter)));
fprintf('Error norm: %.3f max: %.3f \n', error_norm, error_max);
error_norm = norm(abs(Qref)-abs(Qdoi));
error_max = max(max(abs(Qref)-abs(Qdoi)));
fprintf('Error norm: %.3f max: %.3f rate: %.2f %% \n' ...
, error_norm, error_max, 100*error_norm/norm(abs(Qref)) );
figure();
hold on;
for ai = 1:stage.nr_agent
plot(agents{ai}.pos(1), agents{ai}.pos(2)...
, 's' ...
, 'LineWidth', 2 ...
, 'MarkerEdgeColor','k' ...
, 'MarkerFaceColor','g' ...
, 'MarkerSize',10)
str = sprintf(' [%d]', ai);
text(agents{ai}.pos(1), agents{ai}.pos(2), str);
for nj = agents{ai}.nei
plot([agents{ai}.pos(1) agents{nj}.pos(1)], ...
[agents{ai}.pos(2) agents{nj}.pos(2)], 'b-');
end
end
axis([stage.xmin stage.xmax stage.ymin stage.ymax]);
grid on; hold off;
title('Multi-Agent Coordination');
%% Spectral Clustering을 한 결과를 보여준다.
nr_k = 10;
colors = rand(nr_k, 3);
Qtest = Qref; % Qdoi, Qref
[kmeans_label kmeans_center] ...
= kmeans(Qtest, nr_k ...
, 'Distance','cosine' ... % sqEuclidean , cosine
, 'Replicates', 5);
fig_kmeans = figure(3);
hold on;
for ai = 1:stage.nr_agent
for nj = agents{ai}.nei
plot([agents{ai}.pos(1) agents{nj}.pos(1)], ...
[agents{ai}.pos(2) agents{nj}.pos(2)], '--' ...
, 'Color', [0.5 0.6 0.8] ...
, 'LineWidth', 1 );
end
end
for ai = 1:stage.nr_agent
plot(agents{ai}.pos(1), agents{ai}.pos(2)...
, 's' ...
, 'LineWidth', 2 ...
, 'MarkerEdgeColor','k' ...
, 'MarkerFaceColor', colors(kmeans_label(ai), :) ...
, 'MarkerSize',10)
% str = sprintf(' [%d] %d', ai, kmeans_label(ai));
str = sprintf(' <%d>', kmeans_label(ai));
text(agents{ai}.pos(1), agents{ai}.pos(2), str);
end
hold off;
title('k means result');