tutorial and examples for OS and FH LTV

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

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

kalmanFiniteHorizonLTV.m

@@ -0,0 +1,162 @@
+function [K,P] = kalmanFiniteHorizonLTV(system,E,T,P0,opts)
+%% Description
+% This function computes the finite-horizon kalman filter gain matrices 
+% subject to a sparsity constraint for all the instants of a window
+% {k,...,k+T-1}
+% Input:    - system: Tx4 cell whose rows contain matrices A,C,Q and R
+%             for the whole window, i.e.,
+%               - system{i,1} = A(k+i-1), i = 1,...,T
+%               - system{i,2} = B(k+i-1), i = 1,...,T
+%               - system{i,3} = Q(k+i-1), i = 1,...,T
+%               - system{i,4} = R(k+i-1), i = 1,...,T
+%           - E: a matrix that defines the sparsity pattern
+%           - T: Finite Horizon time window
+%           - P0 : nxn initial predicted covariance matrix
+%           - opts: optional input arguments
+%               - epsl: minimum relative improvement on the objective function
+%               - maxOLIt: maximum number of outer loop iterations until convergence 
+%               - verbose: display algorithm status messages
+% Output:   - K: Tx1 cell of gain matrices for all the iterations, i.e.,
+%               K(k+i-1), i = 1,...,T
+%           - P: Tx1 cell of covariance matrices for all the iterations,
+%           i.e., P(k+i-1), i = 1,...,T
+%% Argument handling
+if ~exist('opts','var') 
+    opts.verbose = false; % Default is not to display algorithm status messages
+elseif ~isfield(opts,'verbose')
+    opts.verbose = false; % Default is not to display algorithm status messages
+end
+if ~isfield(opts,'maxOLIt')
+    opts.maxOLIt = 100; % Default maximum number of iterations until convergence
+end
+if ~isfield(opts,'epsl')
+    opts.epsl = 1e-5; % Default minimum relative improvement on the objective function
+end
+if opts.verbose
+    fprintf('----------------------------------------------------------------------------------\n');
+    fprintf('Running finite-horizon algorithm with:\nepsl = %g | maxOLIt = %d.\n',opts.epsl,opts.maxOLIt);
+end
+%% Gain Computation
+n = size(system{1,2},2); % Get value of n from the size of A 
+K = cell(T,1); % Initialise cell to hold all gain matrices
+P = cell(T,1); % Initialise cell to hold all covariance matrices
+PprevIt = cell(T,1); % Initialise cell to hold all covariance matrices of the previous outer loop iteration
+% Initialise Finite Horizon with one step gain and covariance matrices
+for i = 1:T
+    if i == 1
+        [K{i,1},Ppred,P{i,1}] = kalmanOneStepLTV(system(i,:),E,P0);
+    else
+        [K{i,1},Ppred,P{i,1}] = kalmanOneStepLTV(system(i,:),E,Ppred);
+    end
+end
+% Z = vectorZ(vec(E)); % Compute matrix Z
+% Outer loop
+for k = 1:opts.maxOLIt
+    % Inner loop
+    for i = T:-1:1
+        % Update covariance matrix after the predict step
+        if i > 1 
+            P_ = system{i-1,1}*P{i-1,1}*transpose(system{i-1,1})+system{i-1,3};
+        else
+            P_ = P0;
+        end
+        % Compute summation to obtain matrix Lambda
+        Lambda = eye(n);
+        for m = i+1:T
+            % Compute summation to  obtain matrix Gamma
+            Gamma = eye(n);
+            for j = i+1:m
+                Gamma = Gamma*(eye(n)-K{i+1+m-j,1}*system{i+1+m-j,2})...
+                 *system{i+1+m-j-1,1}; 
+            end
+            Lambda = Lambda + transpose(Gamma)*Gamma;
+        end     
+        % Adjust gain using efficient solver [1]
+        K{i,1} = sparseEqSolver(Lambda,system{i,2}*P_*transpose(system{i,2})+system{i,4},Lambda*P_*transpose(system{i,2}),E);
+        % Old solver commented 
+        % K{i,1} = unvec(transpose(Z)/(Z*(kron(system{i,2}*P_*transpose(system{i,2})+system{i,4},Lambda))...
+        %    *transpose(Z))*Z*vec(Lambda*P_*transpose(system{i,2})),n);
+    end
+    % Recompute covariances 
+    for i = 1:T
+        if i >1
+            P_ = system{i,1}*P{i-1,1}*transpose(system{i,1})+system{i,3};
+        else
+            P_ = P0;
+        end
+    P{i,1} = K{i,1}*system{i,4}*transpose(K{i,1})+...
+          (eye(n)-K{i,1}*system{i,2})*P_*transpose(eye(n)-K{i,1}*system{i,2});
+    end 
+    % Check convergence
+    if k ~= 1
+        relDif = 0;
+        for l = 1:T
+            relDif = max(relDif,abs(trace(P{l,1})-trace(PprevIt{l,1}))/trace(PprevIt{l,1}));
+        end
+        if relDif < opts.epsl
+            if opts.verbose
+                fprintf(sprintf("Convergence reached with: epsl = %g\n",opts.epsl));
+                fprintf('A total of %d outer loop iterations were run.\n',k);
+                fprintf('----------------------------------------------------------------------------------\n');
+            end
+            break;
+        elseif k == opts.maxOLIt
+            if opts.verbose
+                fprintf("Finite-horizon algorithm was unable to reach convergence with the specified\nparameters: epsl = %g | T = %d | maxOLIt = %d\n",opts.epsl,T,opts.maxOLIt);
+                fprintf("Sugested actions:\n- Manually tune \'epsl' and \'maxOLIt\' (in this order);\n- Increase \'epsl\', the minimum relative improvement on the objective function\noptimization problem.\n- Increase \'maxOLIt\', the maximum number of outer loop iterations.\n");
+                fprintf('----------------------------------------------------------------------------------\n');
+            end
+        end
+    end
+    PprevIt = P;
+end
+end
+
+%% Auxiliary functions
+
+% Function that computes the vectorisation of a matrix
+% Input:    - in: matrix to be vectorised
+% Output:   - out: vec(in) 
+function out = vec(in)
+    out = zeros(size(in,2)*size(in,1),1);
+    for j = 1:size(in,2)
+       for i = 1:size(in,1)
+           out((j-1)*size(in,1)+i) = in(i,j);
+       end
+    end
+end
+
+% Function that returns a matrix given its vectorisation and number of rows
+% Input:    - in: vectorisation of a matrix
+%           - n: number of rows of the matrix whose vectorisation is
+%             input variable in
+% Output:   - out: matrix with n rows whose vectorisation is input variable
+%             in 
+function out = unvec(in,n)
+    out = zeros(n,size(in,1)/n);
+    for j = 1:size(in,1)
+       out(rem(j-1,n)+1, round(floor((j-1)/n))+1) = in(j); 
+    end
+end
+
+% Function which computes matrix Z such that Z*vec(K) contains the non-zero
+% elements of K according to the desired sparsity pattern
+% Input:    - vecE: the vectorisation of the matrix that defines the
+%             sparsity patern
+% Output:   - Z
+function Z = vectorZ(vecE)
+    vecE = vecE~=0; % Normalise the sparsity pattern to a logical array
+    Z = zeros(sum(vecE),size(vecE,1)); % Initialise matrix Z
+    nZeros = 0;
+    for i = 1:size(vecE,1)
+       if vecE(i) ~= 0
+           Z(i-nZeros,i) = 1; 
+       else
+           nZeros = nZeros+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

kalmanOneStepLTV.m

@@ -0,0 +1,29 @@
+function [K,Ppred,Pfilt] = kalmanOneStepLTV(system,E,Pprev)
+%% Description
+% This function computes the one-step 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)
+%           - E: sparsity pattern
+%           - 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 with sparse matrix solver [1]
+K = sparseEqSolver(eye(n),system{1,2}*Pprev*system{1,2}'+system{1,4},Pprev*system{1,2}',E);
+% 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
+
+%[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