% Solution Exercise 1 in Exercise booklet.
% Purpose: System identification of autonomeous systems

% Generate some output data y0, y1, y2, y3, y4 by simulating a known model.
A=0.9; D=1; x0=1;
[y,x]=dlsim(A,0,D,0,zeros(5,1),x0);

% Solutio
m=1;                               % Number of output variables in y_k
Y0=[y(1) y(2) y(3)                 % Hankelmatrix for calculating system order n, O_2 and X0 with L=2
    y(2) y(3) y(4)];

Y1=[y(2) y(3) y(4)                 % Shifted Hankelmatrix for calculating system matrix A.
    y(3) y(4) y(5)];
 
% b) SVD decomposition
[U,S,V]=svd(Y0);
sv=diag(S)'                        % The singular values
n=input('Specify number of singular values different from zero? : ') % Same as n=rank(Y0)                      

U1=U(:,1:n); S1=S(1:n,1:n); V1=V(:,1:n);

% c)
O=U1; 
X0=S1*V1';

% e)
x0=X0(:,1)
D=O(1:m,:)
A=pinv(O'*O)*O'*Y1*X0'*pinv(X0*X0')

% TEST: 
% We may simulate the identified model and compare the simulated output
% with the given data.
y_s=dlsim(A,0,D,0,zeros(5,1),x0)               % y_s should be "equal" to the given data in y.

% Simulate model
figure(1)
Y=dlsim(A,0,D,0,zeros(50,1),x0);
plot(Y), grid
title('Simulated model output')
xlabel('Time samples'), ylabel('y_k')