%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This test routine presents a demonstration of the LSDTRPM routine which computes 
% LS-estimates of Discrete-Time MIMO RPM (reinitialized partial moment) models  
%
% - The system is a 2-input, 2-output MIMO system.
% - An output error noise is considered in data. The SNR is 20 dB.
% - The design parameters can be otpimized. 
% - A comparison between the RPM model and the ARX model with and without data filtering
% 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 3]; % Transfer function orders
nb = [1 0;1 0]; % Numbers of zero - Strictly proper transfer functions min(na)>max(nb)
pole=[-1 -0.1 0;-0.5+i -0.5-i -2];
zero=[-0.3 0;1 0];
sysc=tf(zeros([1 nu]));
% Input delays (seconds)
nk = [1 2;0.4 1.6];    % 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');

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

%%  Simulate of the DT system like an output-error 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_RPM=lsdtrpm(DATA,[na nb nk],Kr);

%% Step responses 
% The disrcete-time system
M_true=sysd;
% Step responses
figure(1),clf,step(M_true,'b',M_RPM,'g')
legend('True system','DT RPM model',4)

%% Display results 
display('Discrete-time system')
M_true
display('Discrete-time RPM model')
M_RPM

% 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 the RPM model: '),display(num2str([FIT1 FIT2]))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Comparison with the ARX routine
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Estimate the ARX model without data filtering
for i=1:ny
    u=DATA.u;
    y=DATA.y(:,i);
    DATA_temp = iddata(y,u,ts);
    M1=arx(DATA_temp,[na(i) nb(i,:) nk(i,:)]);
    % Build the MIMO model in tf form 
    M_ARX(i,1:nu)=tf({M1.b(1,M1.nk(1)+1:M1.nk(1)+M1.nb(1)) M1.b(2,M1.nk(2)+1:M1.nk(2)+M1.nb(2))},{M1.a M1.a},ts,'InputDelay',nk(i,:));
    set(M_ARX,'OutputName',[M_ARX.OutputName(1:i-1,1);'y' num2str(i)]);
    set(M_ARX,'InputName',['u1';'u2'])
end

% Model simulation
y_hat = lsim(M_ARX,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 the ARX model: '),display(num2str([FIT1 FIT2]))

%% Estimate the ARX model with filters (the filters are transfer functions
%% with the true denominators)
for i=1:ny
    u=DATA.u;
    y=DATA.y(:,i);
    DATA_temp = iddata(y,u,ts);
    % The filtering is performed with the 'focus' property
    M1=arx(DATA_temp,[na(i) nb(i,:) nk(i,:)],'focus',{1,sysd.den{i,1}});
    % Build the MIMO model in tf form 
    M_ARX2(i,1:nu)=tf({M1.b(1,M1.nk(1)+1:M1.nk(1)+M1.nb(1)) M1.b(2,M1.nk(2)+1:M1.nk(2)+M1.nb(2))},{M1.a M1.a},ts,'InputDelay',nk(i,:));
    set(M_ARX2,'OutputName',[M_ARX2.OutputName(1:i-1,1);'y' num2str(i)]);
    set(M_ARX2,'InputName',['u1';'u2'])
end
    
% Model simulation
y_hat = lsim(M_ARX2,DATA.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 the ARX model with filter: '),display(num2str([FIT1 FIT2]))

% Step responses
figure(2),clf,step(M_true,'b',M_RPM,'g',M_ARX2,'r')
legend('True system','DT RPM model','ARX model (filtering)',4)
