% losn_oppg10.m
% Loesningsforslag til oppgave 10.

%%%%%%%%%%%%%%%%%%%%%% THE ACTUAL SYSTEM MATRICES %%%%%%%%%%%%%%%%%%%%%%%%%%%%
isys=1;                              
isys=dread('Spesifiser system 1 eller 2 !',isys);

if isys == 1                           % 1st order system.
 A=0.9; B=0.5;                         % Model for y1.dat           
 D=1;   E=-1;              
elseif isys == 2                       % 2nd order system.
 A=[1.5 1;-0.7 0]; B=[2;-1.3];         % Model for y2.dat
 D=[1 0];          E=-1.1;
end

%%%%%%%%%%%%%%%%%%%%%% EXPERIMENT DESIGN %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u=[-1;1;1;-1;1;-1;-1];                 % Experiment design, N=7.
y=dsrsim(A,B,D,E,u);                   % Generate data.
m=1; r=1;                              % SISO system.

% Merk: last inn datamatrisene u.dat, y1.dat og y2.dat i stedet for
% simuleringen over.

%%%%%%%%%%%%%%%%%%%%% USER SPECIFIED PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L=2;                                   % The # of block rows.
g=1;                                   % The model structure parameter.

%%%%%%%%%%%%%%%%%%%%%% COMPUTE STATE SPACE MODEL MATRICES %%%%%%%%%%%%%%%%%%%%
if g==1                                % Ordering the input and output data.
 U1 = [u(1) u(2) u(3) u(4) u(5)        % The # of rows is L+g=L+1=3.
       u(2) u(3) u(4) u(5) u(6)
       u(3) u(4) u(5) u(6) u(7)];
elseif g==0
 U1 = [u(1) u(2) u(3) u(4) u(5)        % The # of roes is L+g=L=2.
       u(2) u(3) u(4) u(5) u(6)];
end

Y1  = [y(1) y(2) y(3) y(4) y(5)
       y(2) y(3) y(4) y(5) y(6)];

Y2=   [y(2) y(3) y(4) y(5) y(6)
       y(3) y(4) y(5) y(6) y(7)];

[rowsU1,K]=size(U1);

disp('a) Hankelmatrisene er:')
U1, Y1, Y2

disp('c) En projeksjonsmatrise slik at U1*Up=0 er')
Up = eye(K) - U1'*inv(U1*U1')*U1       % Define projection matrix
                                       % so that U1*Up = 0.

Z2=Y2*Up;                              % Compute matrices in Z2 = At*Z1.
Z1=Y1*Up;

[U,S,V] = svd(Z1);                     % Find system order etc.
disp('Singulaerverdiene til Z1 er')
sv=diag(S(1:L*m,1:L*m))'

n=0;                                   % Try to find order automatically.
for i=1:L*m; if sv(i)/sv(1) > 1.0e-7; n=n+1; end; end 
n=dread('Spesifiser systemets orden ?',n);

disp('e) Observerbarhetsmatrisen er')
O=U(:,1:n), S1=S(1:n,1:n); V1=V(:,1:n); % Find observability matrix etc.

disp('f) Systemmatrisene A og D er')
d=O(1:m,:)                              % Find D and A.
a=O'*Z2*V1*inv(S1)

disp('g) Utvidet systemmatrise')
At=O*a*O'                               % Find Atilde and Btilde
disp('f) Utvidet systemmatrise')
Bt=(Y2-At*Y1)*U1'*pinv(U1*U1')
       
if g==1                                 % Find E and B.
 col3=Bt(:,3); col2=Bt(:,2); col1=Bt(:,1); 

 h2=col3;                               % Note h1=[0;E]
 h1=col2+At*h2;                         %      h2=[E;DB]
 ob=col1+At*h1;                         %      ob=[DB;DAB]
 
 e=h2(2);
else
 col2=Bt(:,2); col1=Bt(:,1) 

 h1=col2;                               %      h2=[E;DB]
 ob=col1+At*h1;                         %      ob=[DB;DAB]
 
 e=0;
end

disp('i) E og B matrisen er')
e
b=O'*ob