%% Time-Varying Systems and Computations
% Klaus Diepold, Sven Gronauer, Simon Schumann
%
%% Hankel matrices and Realizations:
% In the following we demonstrate how to invert a matrix using its state-space representation.
% First we choose one of the following matrices as the T matrix:
% 1 - 6x6-matrix T from the lecture notes
% 2 - a random lower triangular matrix of dimension MxN
% 3 - a random square matrix, which is embedded into the lower triangular part of a bigger matrix
% 4 - a lower triangular matrix with band structure
% 5 - the inverse of a lower triangular matrix with band structure

% choose between various examples to run
choice = 1;

if choice == 1
    T = [1 0 0 0 0 0; 0.8 0.9 0 0 0 0; .2 .6 .8 0 0 0; .05 .24 .5 .7 0 0; .013 .096 .25 .4 .6 0; 0.03 0.038 .125 .240 .3 .5];
    [M,N] = size(T);

elseif choice ==2
    % random lower triangular matrix of size MxN
    M = 10;
    N = 10;
    tmp = randn(M,N);
    T = tril(tmp);
    
elseif choice ==3
    % matrix put into lower triangular part
    mm = 5;
    nn = 5;
    M = 2*mm-1;
    N = 2*nn-1;
    Matrix = randn(mm,nn);
    T = eye(M);
    T(mm:end,1:nn) = Matrix;
    
elseif choice == 4
    % lower band matrix of width kk
    M = 10;
    N = 10;
    tmp = randn(M,N);
    T = tril(tmp);
    kk = 3;
    Ttemp = T - tril(T,-(kk+1));
    T = Ttemp;

elseif choice == 5
    % lower band matrix of width kk
    M = 10;
    N = 10;
    tmp = randn(M,N);
    T = tril(tmp);
    kk = 3;
    Ttemp = T - tril(T,-(kk+1));
    T = inv(Ttemp);
end

% Let's have a look at the matrix T.

T

% We initialize cells for Hankel Operator, Observability and Reachability Operators.

H = {};
Obs = {};
Con = {};
r = zeros(M,1);  % Hankel rank
HSVD = zeros(M); % Hankel SVD factors

% Then, we extract Hankels and factorize them using SVD. Finally, we determine Observability and Reachability operators. (We go for Balanced Realization.)

H{1} = [];
Obs{1} = [];
Con{1} = [];
for i=2:M
    H{i} = fliplr(T(i:N,1:i-1));
    [m,n] = size(H{i});
    if ge(m,n) % tall or square matrix
        [U, S, V] = svd(H{i},0);
        r(i) = rank(S);
        HSVD(1:r(i),i) = diag(S(1:r(i),1:r(i))); % compute Hankel SVD
        d = r(i);                                % Dimensions
        Obs{i} = U(:,1:d)*sqrt(S(1:d,1:d));
        Con{i} = sqrt(S(1:d,1:d))*V(:,1:d)';    
    else % wide matrix
        [V, S, U] = svd(H{i}',0);
        r(i) = rank(S);
        HSVD(1:r(i),i) = diag(S(1:r(i),1:r(i)));
        d = r(i);
        Obs{i} = U(:,1:d)*sqrt(S(1:d,1:d));
        Con{i} = sqrt(S(1:d,1:d))*V(:,1:d)';
    end
end

% Now, we display the Observablity and Reachability Operators in blockdiagonal form.

bCon = sparse(blkdiag(Con{1:end}));
bObs = sparse(blkdiag(Obs{1:end}));
spy(bCon), title('Blockdiagonal Reachability Operator')
spy(bObs), title('Blockdiagonal Obervability Operator')

% Finally, we plot the Hankel ranks and the corresponding singular values.

plot(r), hold on, plot(r,'r*'), 
axis([1 size(r,1),0,max(r)+1])
grid
title('Hankel Rank')
HSVD

% How do the given matrices differ in their behavior? Can you find another matrix that also behaves differently?
%% From Hankel to State Space
% We Initialize cells for State-Space Representation (A,B,C,D matrices).

A = {}; B = {}; C = {}; D={};

% Then, we compute D matrices for each time step directly from transfer operator.

for k=1:M
    D{k}= T(k,k);    
end

% After that we identify A, B and C matrices for each time step k from Observability and Reachability Operators utilizing shift invariance.

A{1} = zeros(1,1);
C{1} = zeros(1,1);
for k=2:M
    C{k} = Obs{k}(1,:);
end
for k=1:M-1
    B{k} = Con{k+1}(:,1);
end
for k=2:M-1
    if k < M
        ObsUp = Obs{k}(2:end,:);
        A{k} = pinv(Obs{k+1})*ObsUp;    
    end
end
A{M} = zeros(1,1);
B{M} = zeros(1,1);

% Finally, we create blockdiagonal version of time-varying state-space representation, a shift operator Z and the identity matrix I

bA = blkdiag(A{1:end});
bB = blkdiag(B{1:end}); 
bC = blkdiag(C{1:end});
bD = blkdiag(D{1:end});
Z = sparse(diag(ones(size(bA,1)-1,1),-1));
I = speye(size(bA));

% We can now visualize the blockdiagonal SSS matrices

figure
spy([bA,bB;bC,bD])
title('Blockdiagonal Realization-Matrix')

% To show the accuracy of our result, we compute the transfer operator by exploiting the formula for Transfer Operator in terms of state-space matrices.

T_rec = bD + bC*inv(I - Z*bA)*Z*bB;

% Now, we can compare the reconstructed transfer operator with the originally given matrix T by determining the Frobenius norm of Error Matrix .

Errornorm = norm(T - T_rec,2);
disp(['Error Norm = ',num2str(Errornorm)])