%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This test routine presents a demonstration of the IVCTRPM routine which computes 
% LS-estimates with Instrumental Variable of Continuous-Time MIMO RPM 
% (reinitialized partial moment) models  
%
% - The system is a 2-input, 3-output MIMO system.
% - An output error noise is considered in data. The SNR is 10 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 = 10;   % Signal to noise ratio (dB)

%% Define a continuous-time MIMO system 
% A matrix of transfer functions 
ny = 3;       % Number of output
nu = 2;       % Number of input
na = [2;2;3]; % Transfer function orders
nb = [1 0;0 1;1 0];   % Numbers of zero - Strictly proper transfer functions min(na)>max(nb)
pole=[-1 -0.1 0;-0.5+0.5*i -0.5-0.5*i 0;-0.5 -1 -1.5];
zero=[-0.3 0;0 1;0.2 0];
sysc=tf(zeros([1 nu]));
% Input delays (seconds)
nk = [1 2;0.4 1.6;2 4];    % 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

%%  Simulate of the continuous-time MIMO system with an output-error noise
% The exogenous inputs
u = idinput([N nu],'PRBS',[0 1/20],[-1 1]);
u = u';
% The outputs
y = lsim(sysc,u,[0:ts:(N-1)*ts],zeros(max(na),1),'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; % Add noise to the outputs
DATA = iddata(y,u',ts);

%% Select the design parameters Kr (reinitialization intervals) of the RPM model. 
%  The design parameter T_hat=Kr*t_s (t_s : samplig period) of the RPM model should
%  be selected such that T_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 T_hat and to use the fitting
%  criterion 
Kr = [80 40 50]; % A row vector with na columns and EVEN integers
%Kr = [60 20 30]; % A row vector with na columns and EVEN integers
%Kr = [100 60 70]; % A row vector with na columns and EVEN integers

%% Estimate the CT RPM model with the same structure that the system one
M_IV = ivctrpm(DATA,[na nb nk],Kr,'rect');
M_RPM = lsctrpm(DATA,[na nb nk],Kr,'rect');

%% Step responses 
% the continuous-time MIMO system
M_true = sysc;
% Step responses
step(M_true,'b',M_IV,'g',M_RPM,'r')
legend('True system','IV estimate','LS estimate',4)

%% Display results 
display('Continuous-time MIMO system')
M_true
display('Continuous-time RPM model with Instrumental Variable')
M_IV

% Model simulation
y_hat = lsim(M_IV,u,[0:ts:(N-1)*ts],zeros(max(na),1),'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))));
FIT3=100*(1 - norm(y_hat(:,3) - DATA.y(:,3))/norm(DATA.y(:,3)-mean(DATA.y(:,3))));
display('Fits of IV estimate: '),display(num2str([FIT1 FIT2 FIT3]))

% Model simulation
y_hat = lsim(M_RPM,u,[0:ts:(N-1)*ts],zeros(max(na),1),'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))));
FIT3=100*(1 - norm(y_hat(:,3) - DATA.y(:,3))/norm(DATA.y(:,3)-mean(DATA.y(:,3))));
display('Fits of LS estimate: '),display(num2str([FIT1 FIT2 FIT3]))
