%% Time-Varying Systems and Computations
% Klaus Diepold, Sven Gronauer, Simon Schumann, Zongxie Chen
%
%% Equivalence Transformations
% In this script, we use equivalence transformations to find the input normal and output normal representation of a random state-space system.
%% 1. Generate random realization
% We start by simply setting the desired state dimensions for each time step.

state_dims = [0,1,2,3,4,3,2,1,0]

% With the given state dimension for each time step, we create the system matrices of a causal system. (Note that the system is a single-input-single-output system, each step involves one input value u_i and one output value y_i)
% \left\lbrack \begin{array}{c}
% x_{i+1} \\
% y_i
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% A_i  & B_i \\
% C_i  & D_i
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% x_i \\
% u_i
% \end{array}\right\rbrack

A = {}; B = {}; C = {}; D = {};
K = length(state_dims) - 1; % Number of stages
for i = 1:K
    A{i} = rand(state_dims(i+1), state_dims(i));
    B{i} = rand(state_dims(i+1), 1);
    C{i} = rand(1, state_dims(i));
    D{i} = rand(1,1);
end

% For later comparison we want to find the corresponding transfer operator matrix T. Therefore, we first find the diagonal representation of the realization:

A_diag = blkdiag(A{:});
B_diag = blkdiag(B{:});
C_diag = blkdiag(C{:});
D_diag = blkdiag(D{:});
%plot the structure of the system:
figure
spy([A_diag, B_diag; C_diag, D_diag])

% Then, we define the causal shift operator Z and an identity matrix I of approatiate size which will be needed to compute the transfer operator (matrix) T.

shifted_by = size(A{1},2);
Z = diag(ones(1,size(A_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_diag));

% T=D_{\textrm{diag}} +C_{\textrm{diag}} \cdot {\left(I-Z\cdot A_{\textrm{diag}} \right)}^{-1} \cdot Z\cdot B_{\textrm{diag}}

T = D_diag + C_diag*(I - Z*A_diag)^-1*Z*B_diag;

% Finally, we verify that T corresponds to a causal system.

figure
spy(abs(T) > 1e-10)

%
%% 2. Find transformations to obtain Input Normal Form (INF)
% To be in the input normal form, a system has to fullfill the following equality:
% A_k A_k \prime +B_k B_k \prime =I
% Let's first check out if our randomly initialized system fullfills this requirement.

figure
plot_rows = ceil(sqrt(K));
plot_cols = ceil(sqrt(K));
for i=1:K
    I = A{i}*A{i}' + B{i}*B{i}';
    I_ref = eye(size(I));
    err = norm(I-I_ref, 'fro');
    disp(['||AA´ + BB´ - I|| = ', num2str(err)])
    subplot(plot_rows,plot_cols,i)
    spy(abs(I) > 1e-10)
end

% Obviously our system is currently not in input normal form. But we can apply equivalence transformations to find its input normal form.
% Recall that we can find an alternative realization with the same input/output behaviour by applying non singular transformations R_k:
% \hat{\Sigma} =\left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k \\
% {\hat{C} }_k  & {\hat{D} }_k
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% {\left(R_{k+1} \right)}^{-1} A_k R_k  & {\left(R_{k+1} \right)}^{-1} B_k \\
% C_k R_k  & D_k
% \end{array}\right\rbrack
% Taking into account the requirement for the input normal form, we can write
% {\left(R_{k+1} \right)}^{-1} \left\lbrack \begin{array}{cc}
% A_k R_k  & B_k
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% {\left(R_k \right)}^T {\left(A_k \right)}^T \\
% {\left(B_k \right)}^T
% \end{array}\right\rbrack {\left(R_{k+1} \right)}^{-T} =\left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% {\left({\hat{A} }_k \right)}^T \\
% {\left({\hat{B} }_k \right)}^T
% \end{array}\right\rbrack \overset{!}{=} \;I\;
% Observe that the input normal form requires that \left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k
% \end{array}\right\rbrack is a unitary matrix.
% Next, applying a cholesky factorization on both sides results in
% {\left(R_{k+1} \right)}^{-1} \left\lbrack \begin{array}{cc}
% A_k R_k  & B_k
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k
% \end{array}\right\rbrack
% \left\lbrack \begin{array}{cc}
% A_k R_k  & B_k
% \end{array}\right\rbrack =R_{k+1} \left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k
% \end{array}\right\rbrack
% Starting out with an empty transformation for the first timestep k=1, we know all entities on the left hand side and we can e.g. perform LQ-Decomposition to obtain the transformation for the following timestep k+1.
% \left\lbrack \begin{array}{cc}
% A_k R_k  & B_k
% \end{array}\right\rbrack =L\cdot Q with R_{k+1} :=L and \left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k
% \end{array}\right\rbrack :=Q with {\textrm{QQ}}^T =I.
% This way we can recursively determine all state-transformations necessary to obtain the input normal form of the realization.

R_inf{1} = []; % Initial transformation is empty
for i=1:K
    % LQ decomposition
    [Q,L] = qr([A{i}*R_inf{i}, B{i}]',0);
    Q = Q';
    L = L';
    % Get non singular transformation for next step
    R_inf{i+1} = L;
end

% Finally, we apply the determined equivalence transformations to the realization to find the input normal form:

A_inf = {}; B_inf = {}; C_inf = {}; D_inf = {};
for i=1:K
    A_inf{i} = R_inf{i+1}^-1 * A{i} * R_inf{i};
    B_inf{i} = R_inf{i+1}^-1 * B{i};
    C_inf{i} = C{i} * R_inf{i};
    D_inf{i} = D{i};
end

% Let's check if the new realization is really in input normal form:

figure
plot_rows = ceil(sqrt(K));
plot_cols = ceil(sqrt(K));
for i=1:K
    I = A_inf{i}*A_inf{i}' + B_inf{i}*B_inf{i}';
    I_ref = eye(size(I));
    err = norm(I-I_ref, 'fro');
    disp(['||AA´ + BB´ - I|| = ', num2str(err)])
    subplot(plot_rows,plot_cols,i)
    spy(abs(I) > 1e-10)
end

% Now, let's check if the new system still correspond to the same transfer operator. To do that we first generate the diagonal representation and then compute T.

A_inf_diag = blkdiag(A_inf{:});
B_inf_diag = blkdiag(B_inf{:});
C_inf_diag = blkdiag(C_inf{:});
D_inf_diag = blkdiag(D_inf{:});

% Compute T matrix:

shifted_by = size(A_inf{1},2);
Z = diag(ones(1,size(A_inf_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_inf_diag));
T_inf = D_inf_diag + C_inf_diag*(I - Z*A_inf_diag)^-1*Z*B_inf_diag;

% Compare both transfer operators

err = norm(T - T_inf, "fro")

%
%% 3. Find transformations to obtain Output Normal Form (ONF)
% To be in the output normal form, a system has to fullfill the following equality:
% A_k \prime A_k +C_k \prime C_k =I
% Let's first check out if our randomly initialized system fullfills this requirement.

figure
plot_rows = ceil(sqrt(K));
plot_cols = ceil(sqrt(K));
for i=1:K
    I = A{i}'*A{i} + C{i}'*C{i};
    I_ref = eye(size(I));
    err = norm(I-I_ref, 'fro');
    disp(['||A´A + C´C - I|| = ', num2str(err)])
    subplot(plot_rows,plot_cols,i)
    spy(abs(I) > 1e-10)
end

% Obviously our system is currently not in output normal form. But we can apply equivalence transformations to it.
% Recall that we can find an alternative realization with the same input/output behaviour by applying non singular transformations R_k:
% \hat{\Sigma} =\left\lbrack \begin{array}{cc}
% {\hat{A} }_k  & {\hat{B} }_k \\
% {\hat{C} }_k  & {\hat{D} }_k
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% {\left(R_{k+1} \right)}^{-1} A_k R_k  & {\left(R_{k+1} \right)}^{-1} B_k \\
% C_k R_k  & D_k
% \end{array}\right\rbrack
% Taking into account the requirement for the output normal form, we can write
% {\left(R_k \right)}^T \left\lbrack \begin{array}{cc}
% {\left(A_k \right)}^T {\left(R_{k+1} \right)}^{-T}  & {\left(C_k \right)}^T
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% {\left(R_{k+1} \right)}^{-1} A_k \\
% C_k
% \end{array}\right\rbrack R_k =\left\lbrack \begin{array}{cc}
% {\left({\hat{A} }_k \right)}^T  & {\left({\hat{C} }_k \right)}^T
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% {\hat{A} }_k \\
% {\hat{C} }_k
% \end{array}\right\rbrack \overset{!}{=} I
% Observe that the output normal form requires that \left\lbrack \begin{array}{cc}
% {\left({\hat{A} }_k \right)}^T  & {\left({\hat{C} }_k \right)}^T
% \end{array}\right\rbrack is a unitary matrix.
% Next, applying a cholesky factorization on both sides results in
% \left\lbrack \begin{array}{c}
% {\left(R_{k+1} \right)}^{-1} A_k \\
% C_k
% \end{array}\right\rbrack R_k =\left\lbrack \begin{array}{c}
% {\hat{A} }_k \\
% {\hat{C} }_k
% \end{array}\right\rbrack \;
% \left\lbrack \begin{array}{c}
% {\left(R_{k+1} \right)}^{-1} A_k \\
% C_k
% \end{array}\right\rbrack =\left\lbrack \begin{array}{c}
% {\hat{A} }_k \\
% {\hat{C} }_k
% \end{array}\right\rbrack {\left(R_k \right)}^{-1} \;
% Starting out with an empty transformation for the last timestep k=K+1, we know all entities on the left hand side and we can e.g. perform QR-Decomposition (we will rename R to U to avoid confusion) to obtain the transformation for the previous timestep k-1.
% \left\lbrack \begin{array}{c}
% {\left(R_{k+1} \right)}^{-1} A_k \\
% C_k
% \end{array}\right\rbrack =Q\cdot U with \left\lbrack \begin{array}{c}
% {\hat{A} }_k \\
% {\hat{C} }_k
% \end{array}\right\rbrack :=Q and {\left(R_k \right)}^{-1} :=U\leftrightarrow R_k :=U^{-1}
% This way we can backwards recursively determine all state-transformations necessary to obtain the output normal form of the realization.

R_onf{K+1} = []; % Last transformation is empty
for i=K:-1:1
    % QR decomposition
    [Q,U] = qr([R_onf{i+1}^-1 * A{i}; C{i}],0);
    % Get non singular transformation for next step
    R_onf{i} = U^-1;
end

% Finally, we apply the determined equivalence transformations to the realization to find the output normal form:

A_onf = {}; B_onf = {}; C_onf = {}; D_onf = {};
for i=1:K
    A_onf{i} = R_onf{i+1}^-1 * A{i} * R_onf{i};
    B_onf{i} = R_onf{i+1}^-1 * B{i};
    C_onf{i} = C{i} * R_onf{i};
    D_onf{i} = D{i};
end

% Confirm that the new realization is really in output normal form.

figure
plot_rows = ceil(sqrt(K));
plot_cols = ceil(sqrt(K));
for i=1:K
    I = A_onf{i}'*A_onf{i} + C_onf{i}'*C_onf{i};
    I_ref = eye(size(I));
    err = norm(I-I_ref, 'fro');
    disp(['||A´A + C´C - I|| = ', num2str(err)])
    subplot(plot_rows,plot_cols,i)
    spy(abs(I) > 1e-10)
end

% Now, let's check if the new system still correspond to the same transfer operator. To do that we first generate the diagonal representation and then compute T.

A_onf_diag = blkdiag(A_onf{:});
B_onf_diag = blkdiag(B_onf{:});
C_onf_diag = blkdiag(C_onf{:});
D_onf_diag = blkdiag(D_onf{:});

% Compute T matrix:

shifted_by = size(A_onf{1},2);
Z = diag(ones(1,size(A_onf_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_onf_diag));
T_onf = D_onf_diag + C_onf_diag*(I - Z*A_onf_diag)^-1*Z*B_onf_diag;

% Compare both transfer operators

err = norm(T - T_onf, "fro")