% losn_oppg13.m
% Solution to exercise 9, System Identification
% Written by David Di Ruscio, 17/11-97. Updated 8/4-2011

%%% GIVEN A STATE SPACE MODEL DESCRIBED BY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  x_k+1 = A x_k + B u_k + C v_k
%  y_k   = D x_k + E u_k + F v_k
% where
a1=-1.5; a0=0.7; b1=2; b0=-1.3;
A=[-a1,1;-a0,0]; B=[b1;b0]; C=[0.5;0.3]; D=[1,0]; E=-1; F=1;

% Calculate gains, hd and hs
hd=D*inv(eye(2)-A)*B+E   % Deterministic gain from input, u_k to output, y_k
hs=D*inv(eye(2)-A)*C+F   % Stochastic gain from noise, e_k to output, y_k

%%%%%%%%%%% GENERATE INPUT AND OUTPUT DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=1000; u=utype(N,6); randn('seed',0); e=randn(N,1);
y=dlsim(A,[B C],D,[E F],[u e]);

% transfer function from u_k to y_k
% num=[E b1+a1*E b0+a0*E]; den=[1,a1,a0];
% th=arx([y u],[2,3,0]);
% [a,b,d,e,c]=th2ss(th);

[N,r]=size(u);

%%%%% IDENTIFY MODELS WITH DSR, PCR/LS, ARX, ARMAX, PEM AND OE %%%%%%%%%%%%%%%%
% Note: only DSR, ARMAX and PEM are general enough to identify the above model

%a)%%%%%%%%%%%%%%%%%%%%%%% D S R %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L=3; [a_dsr,b_dsr,d_dsr,e_dsr,c_dsr,f_dsr,x0_dsr]=dsr(y,u,L,1,L,1,2);
hd_dsr=ddcgain(a_dsr,b_dsr,d_dsr,e_dsr);
hs_dsr=ddcgain(a_dsr,c_dsr,d_dsr,f_dsr);
ym_dsr=dlsim(a_dsr,b_dsr,d_dsr,e_dsr,u,x0_dsr);

%b)%%%%%%%%%%%%%%%%%%%%%%% P C R %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% assume model on the form y_k = Hd*u_k which gives normal eq. y'=Hd*u'
hd_pcr=y'*u*pinv(u'*u);             % Least Squares=PCR with two components
ym_pcr = (hd_pcr*u')';

%c)%%%%%%%%%%%%%%%%%%%%%%% A R X %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
na=2; nb=3; nk=0; 
th_arx=arx([y u],[na,nb,nk]);
[a_arx,b_arx,d_arx,e_arx,c_arx]=th2ss(th_arx);
ym_arx=dlsim(a_arx,b_arx,d_arx,e_arx,u);
hd_arx=ddcgain(a_arx,b_arx,d_arx,e_arx);

%d)%%%%%%%%%%%%%%%%%%%%%%% A R M A X %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
na=2; nb=3; nc=2; nk=0; th_armax=armax([y u],[na,nb,nc,nk]);
[a_armax,b_armax,d_armax,e_armax,c_armax]=th2ss(th_armax);
ym_armax=dlsim(a_armax,b_armax,d_armax,e_armax,u);
hd_armax=ddcgain(a_armax,b_armax,d_armax,e_armax);
hs_armax=ddcgain(a_armax,c_armax,d_armax,1);

%e)%%%%%%%%%%%%%%%%%%%%%%% P E M %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
im_pem=2;
if im_pem==1
 n=2; m=1; ms=canform(n,m,[1,1,1]);                % Modellstructur
 th0=ms2th(ms);                                    % Transform til theta form.
 th_pem=pem([y u],th0);                            % Id. parametre.
 [a_pem,b_pem,d_pem,e_pem,c_pem,x0_pem]=th2ss(th_pem);                
else
% or as follows, for siso systems.
% po=[na nb nc nd nf nk]=[2 3 2 0 0 0]; th=pem([y u],po);
 po=[2 3 2 0 0 0]; 
 th_pem=pem([y u],po);
end
[a_pem,b_pem,d_pem,e_pem,c_pem,x0_pem]=th2ss(th_pem);                
ym_pem=dlsim(a_pem,b_pem,d_pem,e_pem,u,x0_pem);
hd_pem=ddcgain(a_pem,b_pem,d_pem,e_pem);
hs_pem=ddcgain(a_pem,c_pem,d_pem,1);

%%%%%%%%%%%%%%%%%%%%%%%%%% O E %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nb=3; na=2; nk=0; th_oe=oe([y u],[nb,na,nk]);
[a_oe,b_oe,d_oe,e_oe,c_oe,x0_oe]=th2ss(th_oe);
hd_oe=ddcgain(a_oe,b_oe,d_oe,e_oe);

% Comments
% -DSR OG PEM are general methods, i.e. the entire Kalman filter model is estimated.
% -ARMAX is also identifying a complete Kalman filter model, but is restrictedmen 
% to Single Output systems. For an SISO system then ARMAX may with advantage be used instead of PEM.
% -ARX og OE are special functions which only are capable of identifying
% special case models.
% -The parameters in an ARX model may be identified with the Ordinary Least Squares (OLS) method.
