Merge pull request #4 from decenter2021/dev

Dev

Examples/NTanksNetworkControlOneStep.m

@@ -453,7 +453,7 @@
 cte.iLQReps = 1e-4;         % parameter for the stopping criterion of iLQR
 end
 
-%% getConstantsNTankNetwork - Description
+%% getEquilibriumMatrices - Description
 % This function computes matrices alpha and beta according to [1] for
 % equilibrium level computation
 % Output:  -alpha, beta: matrices to compute water level 

LQRFiniteHorizonLTI.m

@@ -46,7 +46,7 @@
 n = size(A,1); % Get value of n from the size of A 
 % Initialise Finite Horizon with One Step gain and covariance matrices
 [K,P] = LQROneStepSequenceLTI(A,B,Q,R,E,opts.W);
-Z = vectorZ(vec(E)); % Compute matrix Z
+%Z = vectorZ(vec(E)); % Compute matrix Z
 Pprev = zeros(n,n); % Previous iteration
 Kinf = NaN;
 counterSteadyState = 0; % Counter for the number of iterations for which a steady-state solution was found
@@ -76,7 +76,7 @@
         else
             C_eq = B'*Q*A*Lambda;
         end
-        % Adjust gain using efficient solver
+        % Adjust gain using efficient solver [1]
         K{k,1} = sparseEqSolver(S,Lambda,...
             C_eq,E);
 
@@ -232,3 +232,8 @@
 
 end
 end
+
+%[1] Pedroso, Leonardo, and Pedro Batista. 2021. "Efficient Algorithm for the 
+% Computation of the Solution to a Sparse Matrix Equation in Distributed Control 
+% Theory" Mathematics 9, no. 13: 1497. https://doi.org/10.3390/math9131497
+

LQROneStepLTI.m

@@ -32,25 +32,12 @@
 
 %% Gain computation
 n = size(A,1); % Get value of n from the size of A 
-m = size(B,2); % Get value of n from the size of B 
 Pinf = Q; % terminal condition
 Pprev = NaN; % Previous iteration
 it = opts.maxIt;
 while it > 0 % LQ iterations
-    Kinf = zeros(m,n);
-    S = R+B'*Pinf*B;
-    for i = 1:n
-        L = zeros(n);
-        L (i,i) = 1; % Generate matrix L_i
-        M = zeros(m);
-        for j = 1:m % Gererate matrix M_i
-            if E(j,i) ~= 0
-                M(j,j) = 1;
-            end
-        end
-        % Compute the ith term of the summation 
-        Kinf = Kinf + (eye(m)-M+M*S*M)\M'*B'*Pinf*A*L';
-    end
+    % Compute gain with efficient algorithm [1]
+    Kinf = sparseEqSolver(R+B'*Pinf*B,eye(n),B'*Pinf*A,E);
     % Update P
     Pinf = Q + Kinf'*R*Kinf+(A-B*Kinf)'*Pinf*(A-B*Kinf);
     it = it-1;
@@ -70,3 +57,8 @@
     end
 end    
 end
+
+%[1] Pedroso, Leonardo, and Pedro Batista. 2021. "Efficient Algorithm for the 
+% Computation of the Solution to a Sparse Matrix Equation in Distributed Control 
+% Theory" Mathematics 9, no. 13: 1497. https://doi.org/10.3390/math9131497
+

LQROneStepLTV.m

@@ -29,42 +29,19 @@
     fprintf('----------------------------------------------------------------------------------\n');
 end
 %% Gain computation
-persistent n
-if isempty(n)
-    n = size(system{1,1},1); % Get value of n from the size of A 
-end
-persistent m
-if isempty(m)
-    m = size(system{1,2},2); % Get value of n from the size of B 
-end
-persistent M L
-if isempty(M) || isempty(L)
-   M = cell(m,1);
-   L = cell(m,1);
-   for j = 1:n
-        L{j,1} = zeros(n);
-        L{j,1}(j,j) = 1; % Generate matrix L_i
-        M{j,1} = zeros(m);
-        for i = 1:m % Gererate matrix M_i
-            if E(i,j) ~= 0
-                M{j,1}(i,i) = 1;
-            end
-        end
-   end
-end
+n = size(system{1,1},1); % Get value of n from the size of A 
 P = cell(T+1,1); % Initialize cell arrays
 K = cell(T,1);
 P{T+1,1} = system{T+1,3}; % terminal condition  
 for k = T:-1:1
-   K{k,1} = zeros(m,n);
-   S = system{k,4}+system{k,2}'*P{k+1,1}*system{k,2};
-   for j = 1:n
-        % Compute the ith term of the summation   
-        K{k,1} = K{k,1} + ...
-         (eye(m)-M{j,1}+M{j,1}*S*M{j,1})\M{j,1}*system{k,2}'*P{k+1,1}*system{k,1}*L{j,1};
-   end
+   % Compute gain with efficient algorithm [1]
+   K{k,1} = sparseEqSolver(system{k,4}+system{k,2}'*P{k+1,1}*system{k,2},eye(n),system{k,2}'*P{k+1,1}*system{k,1},E);
    % Update P
    P{k,1} = system{k,3}+K{k,1}'*system{k,4}*K{k,1}+...
        (system{k,1}-system{k,2}*K{k,1})'*P{k+1,1}*(system{k,1}-system{k,2}*K{k,1});
 end       
-end
+end
+
+%[1] Pedroso, Leonardo, and Pedro Batista. 2021. "Efficient Algorithm for the 
+% Computation of the Solution to a Sparse Matrix Equation in Distributed Control 
+% Theory" Mathematics 9, no. 13: 1497. https://doi.org/10.3390/math9131497

Tutorials/kalmanFiniteHorizonLTVTutorial.m

@@ -0,0 +1,72 @@
+%% Tutorial of kalmanFiniteHorizonLTV
+%% Synthetic random system
+T = 50;
+n = 5;
+o = 3;
+rng(1); % Pseudo-random seed for consistency
+% Alternatively comment out rng() to generate a random system
+% Do not forget to readjust the synthesys parameters of the methods
+system = cell(T,4);
+% Initial matrices (just to compute the predicted covariance at k = 1)
+A0 = rand(n,n)-0.5;
+Q0 = rand(n,n)-0.5;
+Q0 = Q0*Q0';
+for i = 1:T
+    if i == 1
+        system{i,1} = A0+(1/10)*(rand(n,n)-0.5);
+        system{i,2} = rand(o,n)-0.5;
+    else % Generate time-varying dynamics preventing erratic behaviour
+        system{i,1} = system{i-1,1}+(1/10)*(rand(n,n)-0.5);
+        system{i,2} = system{i-1,2}+(1/10)*(rand(o,n)-0.5);
+    end
+    system{i,3} = rand(n,n)-0.5;
+    system{i,3} = system{i,3}*system{i,3}';
+    system{i,4} = rand(o,o)-0.5;
+    system{i,4} = system{i,4}*system{i,4}';
+end
+E = round(rand(n,o));
+
+%% Synthesize regulator gain using the finite-horizon algorithm
+% Generate random initial predicted covariance for the initial time instant 
+P0 = rand(n,n);
+P0 = P0*P0';
+% Algorithm paramenters (optional)
+opts.verbose = true;
+opts.epsl = 1e-5;
+opts.maxOLIt = 100; 
+% Synthesize regulator gain using the finite-horizon method
+[K,P] = kalmanFiniteHorizonLTV(system,E,T,P0,opts);
+
+%% Simulate error dynamics
+% Initialise error cell
+x = cell(T,1);
+% Generate random initial error
+x0 = transpose(mvnrnd(zeros(n,1),P0));
+% Init predicted covariance matrix
+Ppred = A0*P0*A0'+Q0;
+for j = 1:T
+    % Error dynamics
+    if j == 1
+        x{j,1} = (system{j,1}-K{j,1}*system{j,2})*x0;
+    else
+        x{j,1} = (system{j,1}-K{j,1}*system{j,2})*x{j-1,1};
+    end
+end
+
+%% Plot the norm of the estimation error
+% Plot the ||x||_2 vs instant of the simulation 
+figure;
+hold on;
+set(gca,'FontSize',35);
+ax = gca;
+ax.XGrid = 'on';
+ax.YGrid = 'on';
+xPlot = zeros(T,1);
+for j = 1:T
+   xPlot(j,1) =norm(x{j,1}(:,1)); 
+end
+plot(1:T, xPlot(:,1),'LineWidth',3);
+set(gcf, 'Position', [100 100 900 550]);
+ylabel('$\|\mathbf{x}_{FH}(k)\|_2$','Interpreter','latex');
+xlabel('$k$','Interpreter','latex');
+hold off;

Tutorials/kalmanOneStepLTVTutorial.m

@@ -0,0 +1,66 @@
+%% Tutorial of kalmanOneStepLTV
+%% Synthetic random system
+T = 50;
+n = 5;
+o = 3;
+rng(1); % Pseudo-random seed for consistency
+% Alternatively comment out rng() to generate a random system
+% Do not forget to readjust the synthesys parameters of the methods
+system = cell(T,4);
+% Initial matrices (just to compute the predicted covariance at k = 1)
+A0 = rand(n,n)-0.5;
+Q0 = rand(n,n)-0.5;
+Q0 = Q0*Q0';
+for i = 1:T
+    if i == 1
+        system{i,1} = A0+(1/10)*(rand(n,n)-0.5);
+        system{i,2} = rand(o,n)-0.5;
+    else % Generate time-varying dynamics preventing erratic behaviour
+        system{i,1} = system{i-1,1}+(1/10)*(rand(n,n)-0.5);
+        system{i,2} = system{i-1,2}+(1/10)*(rand(o,n)-0.5);
+    end
+    system{i,3} = rand(n,n)-0.5;
+    system{i,3} = system{i,3}*system{i,3}';
+    system{i,4} = rand(o,o)-0.5;
+    system{i,4} = system{i,4}*system{i,4}';
+end
+E = round(rand(n,o));
+
+%% Simulate error dynamics
+% Generate random initial predicted covariance for the initial time instant 
+P0 = rand(n,n);
+P0 = P0*P0';
+% Initialise error cell
+x = cell(T,1);
+% Generate random initial error
+x0 = transpose(mvnrnd(zeros(n,1),P0));
+% Init predicted covariance matrix
+Ppred = A0*P0*A0'+Q0;
+for j = 1:T
+    % Synthesize regulator gain using the one-step method
+    [K,Ppred,~] = kalmanOneStepLTV(system(j,:),E,Ppred);
+    % Error dynamics
+    if j == 1
+        x{j,1} = (system{j,1}-K*system{j,2})*x0;
+    else
+        x{j,1} = (system{j,1}-K*system{j,2})*x{j-1,1};
+    end 
+end
+
+%% Plot the norm of the estimation error
+% Plot the ||x||_2 vs instant of the simulation 
+figure;
+hold on;
+set(gca,'FontSize',35);
+ax = gca;
+ax.XGrid = 'on';
+ax.YGrid = 'on';
+xPlot = zeros(T,1);
+for j = 1:T
+   xPlot(j,1) =norm(x{j,1}(:,1)); 
+end
+plot(1:T, xPlot(:,1),'LineWidth',3);
+set(gcf, 'Position', [100 100 900 550]);
+ylabel('$\|\mathbf{x}_{OS}(k)\|_2$','Interpreter','latex');
+xlabel('$k$','Interpreter','latex');
+hold off;

kalmanCentralizedLTV.m

@@ -0,0 +1,24 @@
+function [K,Ppred,Pfilt] = kalmanCentralizedLTV(system,Pprev)
+%% Description
+% This function computes the centralized kalman filter gain for a time-instant
+% k, i.e., K(k), subject to a sparsity constraint.
+% Input:    - system: 1x4 cell whose rows contain matrices A,C,Q and R i.e.,
+%               - system{1,1} = A(k)
+%               - system{2,2} = C(k)
+%               - system{3,3} = Q(k)
+%               - system{4,4} = R(k)
+%           - Pprev: Previous predicted error covariance matrix, i.e., P(k|k-1)
+% Output:   - K: filter gain K(k)
+%           - Ppred: Predicted error covarinace matrix P(k+1|k)
+%           - Pfilt: Predicted error covarinace matrix P(k|k)
+
+%% Gain computation 
+n = size(system{1,1},1); % Get value of n from the size of A 
+% Compute gain
+K = Pprev*transpose(system{1,2})/(system{1,2}*Pprev*transpose(system{1,2})+system{1,4});
+% Update the covariance matrix after the filtering step, i.e., P(k|k)
+Pfilt = K*system{1,4}*K'+...
+    (eye(n)-K*system{1,2})*Pprev*((eye(n)-K*system{1,2})');
+% Compute P(k+1|k)
+Ppred = system{1,1}*Pfilt*system{1,1}'+system{1,3};
+end

kalmanFiniteHorizonLTI.m

@@ -50,7 +50,7 @@
 n = size(A,1); % Get value of n from the size of A 
 % Initialise Finite Horizon with One Step gain and covariance matrices
 [K,P] = OneStepSequenceLTI(A,C,Q,R,E,opts.W,opts.P0);
-Z = vectorZ(vec(E)); % Compute matrix Z
+%Z = vectorZ(vec(E)); % Compute matrix Z
 Pprev = zeros(n,n); % Previous iteration
 Kinf = NaN;
 counterSteadyState = 0; % Counter for the number of iterations for which a steady-state solution was found
@@ -75,7 +75,7 @@
            end
            Lambda = Lambda + transpose(Gamma)*Gamma;
         end      
-        % Adjust gain using efficient solver
+        % Adjust gain using efficient solver [1]
         K{i,1} = sparseEqSolver(Lambda,C*P_*transpose(C)+R,...
             Lambda*P_*transpose(C),E);
         % Old solver commented out
@@ -226,3 +226,8 @@
         (eye(n)-K{l,1}*C)*P_*transpose(eye(n)-K{l,1}*C);
 end
 end
+
+%[1] Pedroso, Leonardo, and Pedro Batista. 2021. "Efficient Algorithm for the 
+% Computation of the Solution to a Sparse Matrix Equation in Distributed Control 
+% Theory" Mathematics 9, no. 13: 1497. https://doi.org/10.3390/math9131497
+