%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This test routine presents a demonstration of the IVDTRPM routine which computes 
% LS-estimates with Instrumental Variable of Discrete-Time MIMO RPM 
% (reinitialized partial moment) models  
%
% - The system is a 2-input, 2-output MIMO system.
% - A colored noise is considered in data by an ARMAX model simultation. The SNR is 20 dB.
% - The design parameters can be otpimized. 
% - A comparison between the LS-estimate and the LS-estimate with IV is
% performed.
%
%	Author  : Régis Ouvrard, University of Poitiers
%	e-mail  : regis.ouvrard@univ-poitiers.fr
%	Last Revised : 30 March 2010
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   This routine has been tested with MATLAB Version 7.3 and updates 

clear

%%  Specify Experiment Conditions 
N = 1000;   % Number of data samples
ts = 0.2;   % Sampling Period
SNR = 20;   % Signal to noise ratio (dB)

%% Define a continuous-time MIMO system 
% A matrix of transfer functions 
ny = 2;     % Number of output
nu = 2;     % Number of input
na = [2;2]; % Transfer function orders
nb = [1 1;1 1]; % Numbers of zero - Strictly proper transfer functions min(na)>max(nb)
pole=[-1 -0.1;-0.5+i -0.5-i];
zero=[-0.3 0.3;-0.7 1];
sysc=tf(zeros([1 nu]));
% Input delays (seconds)
nk = [0 0;0 0];    % A ny|nu matrix 
% Build the transfer functions
for i = 1:ny
    den = [zeros(1,na(i)) 1];
    for j = 1:na(i)   % Build the i_th denominator
        den(1,na(i)+1-j:na(i)+1) = conv(den(1,na(i)+2-j:na(i)+1),[1/-pole(i,j) 1]);
    end
    num = [zeros(nu,max(nb(i,:))),ones(nu,1)];
    for k = 1:nu  % Build the nu numerators for the i_th output
        for j = 1:nb(i,k)
            num(k,max(nb(i,:))+1-j:max(nb(i,:))+1) = conv(num(k,max(nb)+2-j:max(nb)+1),[1/-zero(i,j+k-1) 1]);
        end
        sysc(i,k) = tf(num(k,:)/den(1:1),den/den(1:1),'InputDelay',nk(i,k));   % Get unity gain
        set(sysc,'InputName',['u1';'u2']);
    end
set(sysc,'OutputName',[sysc.OutputName(1:i-1,1);'y' num2str(i)]);
end

%% Discretize the system
sysd = c2d(sysc,ts,'tustin');
[A,B,C,D] = ssdata(sysd);

%% Choose the DT transfer-function model orders
na = [2;2];         % Denominator orders
nb = [3 3;3 3];     % Numbers of numerator parameters
nk = round(nk/ts);  % Input delays (samples)

%%  Simulate of the DT system like an ARMAX model
nx = size(A,1);
% The exogenous input
u = idinput([N nu],'PRBS',[0 1/20],[-1 1]);
u=u';
% Include delays
for r=1:nu, 
    udel(r,:) = [zeros(1,nk(1,r)) u(r,1:N-nk(1,r))]; 
end;
e = sqrtm([0.03 0;0 0.3]/10^(SNR/10))*randn(size(C,1),N); % Noise sequence
K = dlqe(A,eye(nx),C,eye(nx),0.1*eye(ny)); % Estimate of the Kalman filter gain
x = zeros(nx,N+1); 
y = zeros(ny,N);
for t=1:N,
 	y(:,t)   = C*x(:,t) + D*udel(:,t) +   e(:,t);  % Simulate output
 	x(:,t+1) = A*x(:,t) + B*udel(:,t) + K*e(:,t);  % Simulate state with innovations structure (ARMAX structure)
end
DATA=iddata(y',u',ts);
% %%  Simulate of the DT system like an OE model
% % The exogenous inputs
% u = idinput([N nu],'PRBS',[0 1/20],[-1 1]);
% % The outputs
% y = lsim(sysd,u','zoh');
% % The noise sequences
% Rvar = var(y)/10^(SNR/10);
% for i = 1:ny
%     e(:,i) = sqrt(Rvar(i))*randn(N,1); 
% end
% y = y + e;
% DATA = iddata(y,u,ts);

%% Select the design parameters K_hat (reinitialization intervals) of the RPM model. 
%  The design parameter K_hat (reinitialization interval) of the RPM model should
%  be selected such that K_hat is near the double of the main time constant. 
%  With the step response, you can compute the main time by using the 5% settling time 
%  which is equivalent to three times the main time constant approximately.
%  A different way consists to test various K_hat and to use the fitting
%  criterion 
Kr=[20 20]; % A row vector with na columns

%% Estimate the DT RPM model 
M_IV=ivdtrpm(DATA,[na nb nk],Kr);
M_RPM=lsdtrpm(DATA,[na nb nk],Kr);

%% Step responses 
% The disrcete-time system
M_true=sysd;
% Step responses
step(M_true,'b',M_IV,'g',M_RPM,'r')
legend('True system','IV estimate','LS estimate',4)

%% Display results 
display('Discrete-time system')
M_true
display('DT RPM model estimated with Instrumental Variable')
M_IV

% Model simulation
y_hat = lsim(M_IV,u,'zoh');
% Compute the fit
FIT1=100*(1 - norm(y_hat(:,1) - DATA.y(:,1))/norm(DATA.y(:,1)-mean(DATA.y(:,1))));
FIT2=100*(1 - norm(y_hat(:,2) - DATA.y(:,2))/norm(DATA.y(:,2)-mean(DATA.y(:,2))));
display('Fits of IV estimate: '),display(num2str([FIT1 FIT2]))

% Model simulation
y_hat = lsim(M_RPM,u,'zoh');
% Compute the fit
FIT1=100*(1 - norm(y_hat(:,1) - DATA.y(:,1))/norm(DATA.y(:,1)-mean(DATA.y(:,1))));
FIT2=100*(1 - norm(y_hat(:,2) - DATA.y(:,2))/norm(DATA.y(:,2)-mean(DATA.y(:,2))));
display('Fits of LS estimate: '),display(num2str([FIT1 FIT2]))
