diff --git a/DistributedMinimaxAdaptiveControl.m b/DistributedMinimaxAdaptiveControl.m new file mode 100644 index 0000000000000000000000000000000000000000..3086f171e0e8f25f625ed18fafb251c84558b289 --- /dev/null +++ b/DistributedMinimaxAdaptiveControl.m @@ -0,0 +1,450 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This code simulates distributed minimax adaptive control algorithm for +% uncertain networked systems. +% +% Copyrights Authors: 1) Venkatraman Renganathan - Lund University, Sweden. +% 2) Anders Rantzer - Lund University, Sweden. +% 3) Olle Kjellqvist - Lund University, Sweden. +% +% Email: venkatraman.renganathan@control.lth.se +% anders.rantzer@control.lth.se +% olle.kjellqvist@control.lth.se +% +% Date last updated: 23 October, 2023. +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Make a fresh start +clear all; close all; clc; + +% set properties for plotting +set(groot,'defaultAxesTickLabelInterpreter','latex'); +set(groot,'defaulttextinterpreter','latex'); +set(groot,'defaultLegendInterpreter','latex'); +addpath(genpath('src')); + +% Flag deciding whether to generate new data or load existing data +% dataPrepFlag = 1: Generates new network data +% dataPrepFlag = 0: Loads existing network data +dataPrepFlag = 1; + +% When dataPrepFlag = 1: Generate new data for network +if(dataPrepFlag) + + disp('Generating new data for network as dataPrepFlag = 1.'); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Define the network data using graph structure + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Specify Number of nodes + N = 10000; + % Specify Number of edges + M = N - 1; + % Create a random graph with N nodes and M edges + [adjMatrix, I, graphVariable] = GenerateRandomTreeGraph(N); + % Get number of edges on graph + numEdges = size(graphVariable.Edges.EndNodes, 1); + fprintf('Generated random graph is acyclic and has only one component \n'); + + % Plot the graph - large graph can be time consuming & may not be informative + if N <= 300 + plot(graphVariable); + else + disp('The graph is too large to display.'); + end + + % Infer state and input dimensions + [n,m] = size(I); + + % Get the degree vector + degreeVec = sum(adjMatrix, 1)'; + + % Get one of the node indeces with maximum degree for plotting + [~, maxDegreeNodeIndex] = max(degreeVec); + + % Form input matrix B with parameter b and matrix I such that B/b = I + b = 0.1; + B = b*I; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Prepare the dynamics matrix A for each Model + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Specify the number of possible plant network dynamics + numModels = 2; + + % Populate all the possible plant models + for dIter = 1:numModels + % Placeholders for a matrix for each component + ai = zeros(n, 1); + % Flag for stopping while loop + dynamicsPreparationFlag = 1; + % Counter for populating ai vector + iter = 1; + % Loop through until ai^2 + bb^{T} < ai is satisfied + while dynamicsPreparationFlag + % Generate a random ai + ai(iter, 1) = rand; + % Check condition + if(ai(iter, 1)^2 - ai(iter, 1) + 2*b^2*degreeVec(iter, 1) < 0) + iter = iter + 1; + end + % if counter exceeds limit, form the A matrix & stop while loop + if(iter > n) + A = diag(ai); + dynamicsPreparationFlag = 0; + end + end + % Prepare A and B matrices + AMatrices{dIter} = A; + BMatrices{dIter} = B; + % Compute H_infinity control as u = Kx, with K = B'*(A-I)^{-1} + Kinfinity{dIter} = BMatrices{dIter}'*inv(AMatrices{dIter} - eye(n)); +% % Check for correctness (condition on connectivity & speed of propagation) +% if(all(eig(AMatrices{dIter}) < 1)) +% fprintf('# %d Dynamics agrees the condition A is Schur \n', dIter); +% end +% if(eig(AMatrices{dIter}^2 + BMatrices{dIter}*BMatrices{dIter}' - AMatrices{dIter}) < 0) +% fprintf('# %d Dynamics agrees the condition A^2 + BB^{T} < A \n', dIter); +% end +% if(any(eig(AMatrices{dIter} + BMatrices{dIter}*Kinfinity{dIter}) > 1)) +% disp('Closed loop Eigenvalue > 1. So, there exists atleast an unstable state.'); +% else +% disp('Closed loop Hinfinity controlled system is stable') +% end + end + disp('Finished Computing H_infinity Control Gains'); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Collect the neighbor information for all nodes in network + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Get the different values of ai for all nodes in the network + aiValuesMatrix = zeros(N, numModels); + for k = 1:numModels + aiValuesMatrix(:,k) = diag(AMatrices{k}); + end + + % Placeholder for network data + networkData = {}; + + % Loop through all nodes in the network + for i = 1:N + % Placeholder for node i data + ithNodeData = {}; + % Get neighbors of node i + ithNodeData.neighborIndices = find(adjMatrix(i,:) > 0); + % Find the number of neighbors of node i + ithNodeData.numNeighbors = size(find(adjMatrix(i,:) > 0), 2); + % Find dimension of data to be stored by node i = (1 + 1 + d_i) + ithNodeData.Dimension = 2 + ithNodeData.numNeighbors; + % Placeholder to store i node data as covariance matrix + ithNodeData.ZbarMatrix = zeros(ithNodeData.Dimension, ithNodeData.Dimension); + % Append node i data to networkData + networkData{end+1} = ithNodeData; + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Compute l2 Gain Bounds for Distributed Minimax Adaptive Control (DMAC) + % Both upper & lower bounds can be computed before DMAC control design. + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Compute lower bound for the l2 gain + AVector = zeros(n, numModels); + for dIter = 1:numModels + AVector(:, dIter) = diag(AMatrices{dIter}); + end + % Find min & max a values for each node from all possible numModels values + minAVector = min(AVector, [], 2); + maxAVector = max(AVector, [], 2); + % Form the Max A matrix using maxAVector + maxAMatrix = diag(maxAVector); + % Find the network level global minimum and maximum a value + netMinA = min(minAVector); + netMaxA = max(maxAVector); + % Compute the lower bound for l2 gain + gammaLowerBound = sqrt(norm(inv((maxAMatrix - eye(n))^2 + B*B'))); + %gammaLowerBound = sqrt(norm(inv((netMaxA*eye(n) - eye(n))^2 + B*B'))); + disp('Finished computing gamma lower bound.'); + + % Compute the upper bound for l2 gain + netMaxA2 = netMaxA*netMaxA; + netMinA2 = netMinA*netMinA; + netMaxA3 = netMaxA*netMaxA*netMaxA; + f1 = ((1-netMaxA)*(netMaxA-netMinA)^(2))/8; + f2 = (-2*netMaxA3 + 4*netMaxA2 - 2*netMinA2 + 4*netMaxA*netMinA - 2*netMaxA - 2*netMinA)/4; + f3 = (4*netMaxA3 - 14*netMaxA2 + 16*netMaxA - 4*netMinA - 18)/(4*(1 - netMaxA)); + f4 = -1/((netMaxA - 1)^(2)); + betas = roots([f1 f2 f3 f4]); + positiveBetas = 10e6*ones(3,1); + for ii = 1:3 + if(betas(ii) > 0) + positiveBetas(ii) = betas(ii); + end + end + betaMin = min(positiveBetas); + gammaUpperBound = sqrt(betaMin); + disp('Finished computing gamma upper bound.'); + + % Display the results + fprintf('Gamma Lower Bound = %.4f \n', gammaLowerBound); + fprintf('Gamma Upper Bound = %.4f \n', gammaUpperBound); + + % Data generation finished + disp('Finished generating new data for network.'); + + % Save the generated network data into mat file. + disp('Saving the generated network data.'); + save('networkDynamicsData.mat'); +else + disp('Loading existing data for network as dataPrepFlag = 0.'); + load('networkDynamicsData.mat'); +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Simulate the trajectory with Hinfinity & Minimax Adaptive Control (MAC) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Frequency response parameters +Ts = 0.01; +z = tf('z', Ts); + +disp('Simulating network with both H_infinity & Minimax Adaptive Controllers'); + +% Set time horizon +T = 30; + +% Define true dynamics to be used from all the possible models in simulation +modelNum = 2; + +% Based on true modelNum specified, get the true dynamics & Gain matrices +A_true = AMatrices{modelNum}; +B_true = BMatrices{modelNum}; +K_true = Kinfinity{modelNum}; + +% Select disturbance. 0: none, 1: white noise, 2: sinusoid, 3: constant step +disturbFlag = 1; +if(disturbFlag == 0) + % Zero Adversarial disturbance + w_advers = zeros(n, T); +elseif(disturbFlag == 1) + % White Noise Adversarial disturbance + w_advers = 0.1*randn(n, T); +elseif(disturbFlag == 2) + % Sinusoidal Adversarial disturbance + w_advers = zeros(n, T); + % Find the transfer function from disturbance to [x,u] + T_d_to_z_i = [eye(n); K_true]*inv(z*eye(n) - (A_true + B*K_true)); + % Find the frequency where worst gain occurs + [gpeaki, fpeaki] = getPeakGain(T_d_to_z_i); + % Evaluate the transfer function at the peak frequency + sysi_at_fmax = evalfr(T_d_to_z_i, exp(j*fpeaki*Ts)); + % Perform a Singular value decomposition + [Ui, Si, Vi] = svd(sysi_at_fmax); + % Get the angle of Vi complex vector + viAngle = angle(Vi); + % Generate sinusoidal adversarial disturbance at that frequency + % Find the transfer function from disturbance to [x,u] + T_d_to_z_i = [eye(n); K_true]*inv(z*eye(n) - (A_true + B*K_true)); + % Find the frequency where worst gain occurs + [gpeaki, fpeaki] = getPeakGain(T_d_to_z_i); + % Evaluate the transfer function at the peak frequency + sysi_at_fmax = evalfr(T_d_to_z_i, exp(j*fpeaki*Ts)); + % Perform a Singular value decomposition + [Ui, Si, Vi] = svd(sysi_at_fmax); + % Get the angle of Vi complex vector + viAngle = angle(Vi); + for t = 1:T + for i = 1:n + w_advers(i,t) = sin(fpeaki*(t) + viAngle(i))*real(Vi(i,1)); + end + end +elseif(disturbFlag == 3) + w_advers = ones(n, T+1); % Step Adversarial disturbance +end + +% Define Placeholders for states and inputs for MAC policy +x_minmax = zeros(n, T+1); % MAC States +u_minmax = zeros(n, T); % MAC Inputs (dim := n, one for each node) +U_minmax = zeros(m, T); % MAC Inputs (uij = ui-uj, dim := m, one for each edge) + +% Define Placeholders for states and inputs for H_infty control policy +x_hinfty = zeros(n, T+1); % H_infty States +u_hinfty = zeros(m, T); % H_infty Inputs (dimension := m, one for each edge: u_{ij}, (i,j) \in E) + +% Generate & populate the same initial condition for both MAC & H_infty +x_0 = 15*ones(n,1); +x_minmax(:, 1) = x_0; +x_hinfty(:, 1) = x_0; + +% Loop through and simulate until the end of time horizon +for t = 1:T + + fprintf('Simulating t = %d \n', t); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Simulate the Hinfty system + %%%%%%%%%%%%%%%%%%%%%%%%%%%% + % H_infty full state feedback control + u_hinfty(:,t) = K_true*x_hinfty(:,t); + % H_infty state update + x_hinfty(:,t+1) = A_true*x_hinfty(:,t) + B_true*u_hinfty(:,t) + w_advers(:,t); + + %%%%%%%%%%%%%%%%%%%%%%%%% + % Simulate the MAC system + %%%%%%%%%%%%%%%%%%%%%%%%% + % Place holder to store the model that best describes disturbance trajectory + NodeBestModelValues = zeros(N, 1); + + % Initialize Edge Counter to 0 + edgeCounter = 0; + + % Loop via all nodes in network to find MAC Control & update the state + for i = 1:N + % Store the neighbor indices of node i + iNeighborIndices = networkData{i}.neighborIndices; + % Compute the disturbance trajectory for each model + ithNodeDisturbanceSum = zeros(numModels, 1); + for k = 1:numModels + ithNodeSumMatrix = [1 aiValuesMatrix(i,k) b*ones(1,networkData{i}.numNeighbors)]'; + ithNodeDisturbanceSum(i,1) = trace(ithNodeSumMatrix'*networkData{i}.ZbarMatrix*ithNodeSumMatrix); + end + % Get model value corresponding to the least ithNodeDisturbanceSum + [NodeBestModelValues(i,1), ~] = min(ithNodeDisturbanceSum); + % MAC full state feedback control for node i + u_minmax(i, t) = (b/(NodeBestModelValues(i,1) - 1))*x_minmax(i, t); + % MAC state update for node i + iNodeContribution = A_true(i,i)*x_minmax(i, t); + iNeighborsContribution = 0; + for j = 1:networkData{i}.numNeighbors + iNeighborsContribution = iNeighborsContribution + (u_minmax(i, t) - u_minmax(iNeighborIndices(1, j), t)); + end + x_minmax(i, t+1) = iNodeContribution + b*iNeighborsContribution + w_advers(i,t); + %%%%%%%%%%%%%%%%%%%% + % Data Storage + %%%%%%%%%%%%%%%%%%%% + % Place holder to store control inputs of neighbors + iNeighborsControls = zeros(networkData{i}.numNeighbors, 1); + % Loop through all neighbors of node i + for j = 1:networkData{i}.numNeighbors + iNeighborsControls(j, 1) = u_minmax(i, t) - u_minmax(iNeighborIndices(1, j), t); + end + % Update the Z Matrix for each node + iNodeZbarUpdate = [-x_minmax(i, t+1); x_minmax(i, t); iNeighborsControls]'; + networkData{i}.ZbarMatrix = networkData{i}.ZbarMatrix + iNodeZbarUpdate*iNodeZbarUpdate'; + end + % Fill the U_minmax control along each edge for node i at time t + for e = 1:numEdges + ethEdgeStart = graphVariable.Edges.EndNodes(e, 1); + ethEdgeEnd = graphVariable.Edges.EndNodes(e, 2); + U_minmax(e, t) = u_minmax(ethEdgeStart, t) - u_minmax(ethEdgeEnd, t); + end + +end + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Plot minimax and Hinfinity trajectories +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% +% PLOTTING STATES +figure1 = figure('Color',[1 1 1]); +grid on; +hold on; +tvec = 0:T; +% Prepare the legend text for plotting +i = 1; +text_1 = '$\left \Vert x^{\dagger}_{'; +text_2 = num2str(i); +text_3 = '}'; +text1 = strcat(text_1, strcat(text_2, text_3)); +text_1 = ' - x^{\star}_{'; +text_2 = num2str(i); +if(disturbFlag == 1) + text_3 = '} \right \|_{1}$: white noise $w$'; +elseif(disturbFlag == 2) + text_3 = '} \right \|_{1}$: sinusoidal $w$'; +elseif(disturbFlag == 3) + text_3 = '} \right \|_{1}$: step $w$'; +end +text2 = strcat(text1, strcat(text_1, strcat(text_2, text_3))); +legendInfos{i} = [text2]; +plot(tvec, abs(x_minmax(1,:) - x_hinfty(1,:))', 'o-'); + +tvec = 0:T-1; +% Prepare the legend text for plotting +i = 1; +text_1 = '$\left \Vert u^{\dagger}_{'; +text_2 = num2str(i); +text_3 = '}'; +text1 = strcat(text_1, strcat(text_2, text_3)); +text_1 = ' - u^{\star}_{'; +text_2 = num2str(i); +if(disturbFlag == 1) + text_3 = '} \right \|_{1}$: white noise $w$'; +elseif(disturbFlag == 2) + text_3 = '} \right \|_{1}$: sinusoidal $w$'; +elseif(disturbFlag == 3) + text_3 = '} \right \|_{1}$: step $w$'; +end +text2 = strcat(text1, strcat(text_1, strcat(text_2, text_3))); +legendInfos{i+1} = [text2]; +plot(tvec, abs(U_minmax(1,:) - u_hinfty(1,:))', 'o-'); + +xlabel('Time'); +legend(legendInfos, 'interpreter', 'latex'); +a = findobj(gcf, 'type', 'axes'); +h = findobj(gcf, 'type', 'line'); +set(h, 'linewidth', 3); +set(a, 'linewidth', 3); +set(a, 'FontSize', 50); + +% Convert matlab figs to tikz for pgfplots in latex document. +matlab2tikz('figurehandle',figure1,'filename','statesControlsDiff.tex' ,'standalone', true, 'showInfo', false); + +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % %% Compute the cumulative regret +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% % Define design parameters. +% Q = eye(n); % State penalty matrix +% R = eye(m); % Input penalty matrix +% +% % Initiate model based regret to zero for each model +% cumulativeRegret = zeros(T+1,1); +% +% % Loop through all finite set of linear system models +% inputSum = 0; +% stateSum = 0; +% % Loop through the entire time horizon +% for t = 1:T+1 +% % compute u'Ru and add it to control sum +% if(t < T+1) +% inputSum = inputSum + (U_minmax(:,t) - u_hinfty(:,t))'*R*(U_minmax(:,t) - u_hinfty(:,t)); +% end +% % compute x'Qx and add it to state sum +% stateSum = stateSum + (x_minmax(:,t) - x_hinfty(:,t))'*Q*(x_minmax(:,t) - x_hinfty(:,t)); +% +% % Record the regret incurred upto that time +% cumulativeRegret(t, 1) = stateSum + inputSum; +% end +% +% %% Plot the regret vs time +% Tvec = 0:T; +% figure3 = figure('Color',[1 1 1]); +% plot(Tvec, cumulativeRegret, '-ob'); +% xlabel('Time'); +% ylabel('Regret'); +% hold off; +% a = findobj(gcf, 'type', 'axes'); +% h = findobj(gcf, 'type', 'line'); +% set(h, 'linewidth', 4); +% set(a, 'linewidth', 4); +% set(a, 'FontSize', 40); +% +% %% Convert matlab figs to tikz for pgfplots in latex document. +% matlab2tikz('figurehandle',figure3,'filename','CumRegret.tex' ,'standalone', true, 'showInfo', false); \ No newline at end of file