% test utils.math.linifitsvd
% 
% 05-06-2009 L Ferraioli
%       CREATION
% 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $Id: mdc3_exp3_loop_v3.m,v 1.2 2009/09/24 09:48:12 luigi Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Loading data

% load cell arrays with parameters names and values

fprintf('===== loading data... =====\n')

% laod parnames and values
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\parnames_10perc.mat
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_nomvalues.mat
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_truevalues.mat
% load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\usedparams_10perc.mat


% set ordered used parameters
% usedparams = {'dH','dsH','dS11','dS1D','dSD1','dSDD','dh1','dsh1',...
%   'dh2','dsh2','dx1','dx2'};
usedparams = {'dH','dsH','dS11','dS1D','dSD1','dSDD',...
  'dh2','dsh2','dx1','dx2'};

%% load Coloring filters

fprintf('===== loading coloring filters... =====\n')

cf11 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF11.mat');
cf11.setName;
cf12 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF12.mat');
cf12.setName;
cf21 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF21.mat');
cf21.setName;
cf22 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF22.mat');
cf22.setName;

%% load Whitening filters

fprintf('===== loading whitening filters... =====\n')

wf11 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF11.mat');
wf11.setName;
wf12 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF12.mat');
wf12.setName;
wf21 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF21.mat');
wf21.setName;
wf22 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF22.mat');
wf22.setName;

%% get non-linear response model
% the model is non-linear in the parameter dependence
fprintf('===== Get TF Model... =====\n')

% H is a 2x2 matrix of smodels representing the transfer function from
% controllers guidance to ifo output
H = matrix(plist('built-in','mdc3_ifo2ifo_v2'));

%% Get derivative models
% those are derivatives of the model with respect to the parameters

fprintf('===== Get derivative models... =====\n')

% differentiate respect to the stiffness
T_dH = copy(H,1);
T_dsH = copy(H,1);

T_dS11 = copy(H,1);
T_dS1D = copy(H,1);
T_dSD1 = copy(H,1);
T_dSDD = copy(H,1);

T_dh1 = copy(H,1);
T_dsh1 = copy(H,1);

T_dh2 = copy(H,1);
T_dsh2 = copy(H,1);

T_dx1 = copy(H,1);
T_dx2 = copy(H,1);

% differentiate respect to the stiffness
for ii = 1:numel(H.objs)
  T_dH.objs(ii) = diff(H.objs(ii),'dH');
  T_dsH.objs(ii) = diff(H.objs(ii),'dsH');

  T_dS11.objs(ii) = diff(H.objs(ii),'dS11');
  T_dS1D.objs(ii) = diff(H.objs(ii),'dS1D');
  T_dSD1.objs(ii) = diff(H.objs(ii),'dSD1');
  T_dSDD.objs(ii) = diff(H.objs(ii),'dSDD');

  T_dh1.objs(ii) = diff(H.objs(ii),'dh1');
  T_dsh1.objs(ii) = diff(H.objs(ii),'dsh1');

  T_dh2.objs(ii) = diff(H.objs(ii),'dh2');
  T_dsh2.objs(ii) = diff(H.objs(ii),'dsh2');

  T_dx1.objs(ii) = diff(H.objs(ii),'dx1');
  T_dx2.objs(ii) = diff(H.objs(ii),'dx2');
end

%% *** Signals generation ***

% ************************************************************************
% The output of each experiment is generated according to the following
% scheme
% i) generation of the output of the system to a given input
% ii) generation of the output noise time series
% iii) output signals and noise are added to get the final output signals
%      for each experiment
% ************************************************************************

%% load input signal
% load input data series in accordance to TN 3045

fprintf('===== Loading input signals... =====\n')

oi1 = ao('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\signals_noise\exp1_1_oi1.mat');
oi1.setName;
oi1.setYunits('m');
oid = ao('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\signals_noise\exp1_2_oid.mat');
oid.setName;
oid.setYunits('m');

%% get signals - true values

fprintf('===== get true values... =====\n')

% calculate the response of the system for the first two experiments of TN
% 3045
% Exp 3.1 input on first channel, no input on the differential channel
% Exp 3.2 no input on the first channel, input on the differential channel
% Output is always taken from both channels

% get response with true params
for ii = 1:numel(H.objs)
  H.objs(ii).setParams(parnames,exp3_truevalues);
end

s11 = fftfilt(oi1,H.objs(1,1));
s12 = fftfilt(oid,H.objs(1,2));
s21 = fftfilt(oi1,H.objs(2,1));
s22 = fftfilt(oid,H.objs(2,2));

% get output signals for exp 3.1
s1_exp_3_1 = s11;
s1_exp_3_1.setName;
sd_exp_3_1 = s21;
sd_exp_3_1.setName;

% get output signals for exp 3.2
s1_exp_3_2 = s12;
s1_exp_3_2.setName;
sd_exp_3_2 = s22;
sd_exp_3_2.setName;

%% Adding noise to signals

fprintf('===== adding noise to signals... =====\n')

% get params
Nsecs = s1_exp_3_1.nsecs;
fs    = s1_exp_3_1.fs;
plcf = plist('bank','parallel');

% starting noise generation exp1.1
a1_exp_3_1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));
a2_exp_3_1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));

% coloring noise exp 1.1
na1_exp_3_1 = filter(a1_exp_3_1,cf11,plcf) + filter(a2_exp_3_1,cf12,plcf);
na2_exp_3_1 = filter(a1_exp_3_1,cf21,plcf) + filter(a2_exp_3_1,cf22,plcf);

% starting noise generation exp 1.2
a1_exp_3_2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));
a2_exp_3_2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));

% coloring noise exp 1.2
na1_exp_3_2 = filter(a1_exp_3_2,cf11,plcf) + filter(a2_exp_3_2,cf12,plcf);
na2_exp_3_2 = filter(a1_exp_3_2,cf21,plcf) + filter(a2_exp_3_2,cf22,plcf);

% adding noise to signals, These signals are the output of our experiments
o1_exp_3_1 = s1_exp_3_1 + na1_exp_3_1;
od_exp_3_1 = sd_exp_3_1 + na2_exp_3_1;

o1_exp_3_2 = s1_exp_3_2 + na1_exp_3_2;
od_exp_3_2 = sd_exp_3_2 + na2_exp_3_2;



% %% plot
% 
% iplot(o1_exp_3_1,plist('Legends', {'i1->o1'}))
% iplot(od_exp_3_1,plist('Legends', {'i1->od'}))
% iplot(o1_exp_3_2,plist('Legends', {'id->o1'}))
% iplot(od_exp_3_2,plist('Legends', {'id->od'}))

%% Run loop to get parameters

% ************************************************************************
% The linear fit scheme is the following
%          
% y - y0 = sum( p(dH/dp) )
% 
% y is the output data series
% y0 is the calculated nominal response
% dH/dp are derivatives of model with respect to parameters
% p are the parameters
% 
% If a whitening step is required it can be applied as:
% 
% WF( y - y0 ) = WF( sum( p(dH/dp) ) )
%              = sum( p WF( dH/dp ) )
% 
% ************************************************************************
% Fit loop for parameters extraction 
% 
% 1) Response of the derivatives of the model with respect to the
% parameters is calculated according to the parameters nominal values.
% These represent what I call, fit basis.
% 2) Fit basis is whitened in order to consider the effect of the whitening
% filter on data.
% 3) Fit matrices are then built up from fit basis.
% 4) Ground experiments results are input as additional information on some
% parameters.
% 5) Nominal system response is calculated.
% 6) Nominal response is subtracted from output signals for the different
% experiments.
% 7) Whitening filter is applied to the difference y - y0.
% 8) Linear fit is performed, information from different experiments and
% ground experiments is joined to increase fit accuracy.
% 9) The values of the parameters obtained are used to calculate a new set
% of nominal values which are used in the following loop.
% ************************************************************************

% run a loop to estimate system parameters
fprintf('===== start loop iteration... =====\n')


N = 1; % Number of iterations


% f = logspace(-5,log10(fs/2),300).';

plcf = plist('bank','parallel');

% init storage struct
mdc3_exp3_loop_results = struct('a',cell(1,N),...
  'Ca',cell(1,N),...
  'Corra',cell(1,N),...
  'Vu',cell(1,N),...
  'bu',cell(1,N),...
  'Cbu',cell(1,N),...
  'mse',cell(1,N),...
  'params',cell(1,N));



% for ii = 1:N
  
  fprintf('===== iter %s =====\n',num2str(1))

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  % get template
  fprintf('===== Get template - iter %s =====\n',num2str(1))
  
  % set nominal values
  for jj = 1:numel(H.objs)
    T_dH.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dsH.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dS11.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dS1D.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dSD1.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dSDD.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dh1.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dsh1.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dh2.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dsh2.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dx1.objs(jj).setParams(parnames,exp3_nomvalues);
    T_dx2.objs(jj).setParams(parnames,exp3_nomvalues);
  end

  % get response for dH
  ds11 = fftfilt(oi1,T_dH.objs(1,1));
  ds12 = fftfilt(oid,T_dH.objs(1,2));
  ds21 = fftfilt(oi1,T_dH.objs(2,1));
  ds22 = fftfilt(oid,T_dH.objs(2,2));

  dH_1_exp_3_1 = ds11;
  dH_1_exp_3_1.setName;
  dH_12_exp_3_1 = ds21;
  dH_12_exp_3_1.setName;
  dH_1_exp_3_2 = ds12;
  dH_1_exp_3_2.setName;
  dH_12_exp_3_2 = ds22;
  dH_12_exp_3_2.setName;
  
  
  % get response for dsH
  ds11 = fftfilt(oi1,T_dsH.objs(1,1));
  ds12 = fftfilt(oid,T_dsH.objs(1,2));
  ds21 = fftfilt(oi1,T_dsH.objs(2,1));
  ds22 = fftfilt(oid,T_dsH.objs(2,2));

  dsH_1_exp_3_1 = ds11;
  dsH_1_exp_3_1.setName;
  dsH_12_exp_3_1 = ds21;
  dsH_12_exp_3_1.setName;
  dsH_1_exp_3_2 = ds12;
  dsH_1_exp_3_2.setName;
  dsH_12_exp_3_2 = ds22;
  dsH_12_exp_3_2.setName;

  % get response for dS11
  ds11 = fftfilt(oi1,T_dS11.objs(1,1));
  ds12 = fftfilt(oid,T_dS11.objs(1,2));
  ds21 = fftfilt(oi1,T_dS11.objs(2,1));
  ds22 = fftfilt(oid,T_dS11.objs(2,2));

  dS11_1_exp_3_1 = ds11;
  dS11_1_exp_3_1.setName;
  dS11_12_exp_3_1 = ds21;
  dS11_12_exp_3_1.setName;
  dS11_1_exp_3_2 = ds12;
  dS11_1_exp_3_2.setName;
  dS11_12_exp_3_2 = ds22;
  dS11_12_exp_3_2.setName;

  % get response for dS1D
  ds11 = fftfilt(oi1,T_dS1D.objs(1,1));
  ds12 = fftfilt(oid,T_dS1D.objs(1,2));
  ds21 = fftfilt(oi1,T_dS1D.objs(2,1));
  ds22 = fftfilt(oid,T_dS1D.objs(2,2));

  dS1D_1_exp_3_1 = ds11;
  dS1D_1_exp_3_1.setName;
  dS1D_12_exp_3_1 = ds21;
  dS1D_12_exp_3_1.setName;
  dS1D_1_exp_3_2 = ds12;
  dS1D_1_exp_3_2.setName;
  dS1D_12_exp_3_2 = ds22;
  dS1D_12_exp_3_2.setName;
  

  % get response for SD1
  ds11 = fftfilt(oi1,T_dSD1.objs(1,1));
  ds12 = fftfilt(oid,T_dSD1.objs(1,2));
  ds21 = fftfilt(oi1,T_dSD1.objs(2,1));
  ds22 = fftfilt(oid,T_dSD1.objs(2,2));

  dSD1_1_exp_3_1 = ds11;
  dSD1_1_exp_3_1.setName;
  dSD1_12_exp_3_1 = ds21;
  dSD1_12_exp_3_1.setName;
  dSD1_1_exp_3_2 = ds12;
  dSD1_1_exp_3_2.setName;
  dSD1_12_exp_3_2 = ds22;
  dSD1_12_exp_3_2.setName;

  % get response for SDD
  ds11 = fftfilt(oi1,T_dSDD.objs(1,1));
  ds12 = fftfilt(oid,T_dSDD.objs(1,2));
  ds21 = fftfilt(oi1,T_dSDD.objs(2,1));
  ds22 = fftfilt(oid,T_dSDD.objs(2,2));

  dSDD_1_exp_3_1 = ds11;
  dSDD_1_exp_3_1.setName;
  dSDD_12_exp_3_1 = ds21;
  dSDD_12_exp_3_1.setName;
  dSDD_1_exp_3_2 = ds12;
  dSDD_1_exp_3_2.setName;
  dSDD_12_exp_3_2 = ds22;
  dSDD_12_exp_3_2.setName;

  % get response for dh1
  ds11 = fftfilt(oi1,T_dh1.objs(1,1));
  ds12 = fftfilt(oid,T_dh1.objs(1,2));
  ds21 = fftfilt(oi1,T_dh1.objs(2,1));
  ds22 = fftfilt(oid,T_dh1.objs(2,2));
  
  dh1_1_exp_3_1 = ds11;
  dh1_1_exp_3_1.setName;
  dh1_12_exp_3_1 = ds21;
  dh1_12_exp_3_1.setName;
  dh1_1_exp_3_2 = ds12;
  dh1_1_exp_3_2.setName;
  dh1_12_exp_3_2 = ds22;
  dh1_12_exp_3_2.setName;
  
  % get response for dsh1
  ds11 = fftfilt(oi1,T_dsh1.objs(1,1));
  ds12 = fftfilt(oid,T_dsh1.objs(1,2));
  ds21 = fftfilt(oi1,T_dsh1.objs(2,1));
  ds22 = fftfilt(oid,T_dsh1.objs(2,2));
  
  dsh1_1_exp_3_1 = ds11;
  dsh1_1_exp_3_1.setName;
  dsh1_12_exp_3_1 = ds21;
  dsh1_12_exp_3_1.setName;
  dsh1_1_exp_3_2 = ds12;
  dsh1_1_exp_3_2.setName;
  dsh1_12_exp_3_2 = ds22;
  dsh1_12_exp_3_2.setName;


  % get response for dh2
  ds11 = fftfilt(oi1,T_dh2.objs(1,1));
  ds12 = fftfilt(oid,T_dh2.objs(1,2));
  ds21 = fftfilt(oi1,T_dh2.objs(2,1));
  ds22 = fftfilt(oid,T_dh2.objs(2,2));

  dh2_1_exp_3_1 = ds11;
  dh2_1_exp_3_1.setName;
  dh2_12_exp_3_1 = ds21;
  dh2_12_exp_3_1.setName;
  dh2_1_exp_3_2 = ds12;
  dh2_1_exp_3_2.setName;
  dh2_12_exp_3_2 = ds22;
  dh2_12_exp_3_2.setName;
  
  % get response for dsh2
  ds11 = fftfilt(oi1,T_dsh2.objs(1,1));
  ds12 = fftfilt(oid,T_dsh2.objs(1,2));
  ds21 = fftfilt(oi1,T_dsh2.objs(2,1));
  ds22 = fftfilt(oid,T_dsh2.objs(2,2));

  dsh2_1_exp_3_1 = ds11;
  dsh2_1_exp_3_1.setName;
  dsh2_12_exp_3_1 = ds21;
  dsh2_12_exp_3_1.setName;
  dsh2_1_exp_3_2 = ds12;
  dsh2_1_exp_3_2.setName;
  dsh2_12_exp_3_2 = ds22;
  dsh2_12_exp_3_2.setName;

  % get response for dx1
  ds11 = fftfilt(oi1,T_dx1.objs(1,1));
  ds12 = fftfilt(oid,T_dx1.objs(1,2));
  ds21 = fftfilt(oi1,T_dx1.objs(2,1));
  ds22 = fftfilt(oid,T_dx1.objs(2,2));

  dx1_1_exp_3_1 = ds11;
  dx1_1_exp_3_1.setName;
  dx1_12_exp_3_1 = ds21;
  dx1_12_exp_3_1.setName;
  dx1_1_exp_3_2 = ds12;
  dx1_1_exp_3_2.setName;
  dx1_12_exp_3_2 = ds22;
  dx1_12_exp_3_2.setName;


  % get response for dx2
  ds11 = fftfilt(oi1,T_dx2.objs(1,1));
  ds12 = fftfilt(oid,T_dx2.objs(1,2));
  ds21 = fftfilt(oi1,T_dx2.objs(2,1));
  ds22 = fftfilt(oid,T_dx2.objs(2,2));

  dx2_1_exp_3_1 = ds11;
  dx2_1_exp_3_1.setName;
  dx2_12_exp_3_1 = ds21;
  dx2_12_exp_3_1.setName;
  dx2_1_exp_3_2 = ds12;
  dx2_1_exp_3_2.setName;
  dx2_12_exp_3_2 = ds22;
  dx2_12_exp_3_2.setName;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  % do whitening on template
  fprintf('===== do whitening on template - iter %s =====\n',num2str(1))

  plwf = plist('bank','parallel');

  dH_1w_exp_3_1  = filter(dH_1_exp_3_1,wf11,plwf) + filter(dH_12_exp_3_1,wf12,plwf);
  dH_12w_exp_3_1 = filter(dH_1_exp_3_1,wf21,plwf) + filter(dH_12_exp_3_1,wf22,plwf);
  
  dsH_1w_exp_3_1  = filter(dsH_1_exp_3_1,wf11,plwf) + filter(dsH_12_exp_3_1,wf12,plwf);
  dsH_12w_exp_3_1 = filter(dsH_1_exp_3_1,wf21,plwf) + filter(dsH_12_exp_3_1,wf22,plwf);

  dS11_1w_exp_3_1  = filter(dS11_1_exp_3_1,wf11,plwf) + filter(dS11_12_exp_3_1,wf12,plwf);
  dS11_12w_exp_3_1 = filter(dS11_1_exp_3_1,wf21,plwf) + filter(dS11_12_exp_3_1,wf22,plwf);

  dS1D_1w_exp_3_1  = filter(dS1D_1_exp_3_1,wf11,plwf) + filter(dS1D_12_exp_3_1,wf12,plwf);
  dS1D_12w_exp_3_1 = filter(dS1D_1_exp_3_1,wf21,plwf) + filter(dS1D_12_exp_3_1,wf22,plwf);

  dSD1_1w_exp_3_1  = filter(dSD1_1_exp_3_1,wf11,plwf) + filter(dSD1_12_exp_3_1,wf12,plwf);
  dSD1_12w_exp_3_1 = filter(dSD1_1_exp_3_1,wf21,plwf) + filter(dSD1_12_exp_3_1,wf22,plwf);

  dSDD_1w_exp_3_1  = filter(dSDD_1_exp_3_1,wf11,plwf) + filter(dSDD_12_exp_3_1,wf12,plwf);
  dSDD_12w_exp_3_1 = filter(dSDD_1_exp_3_1,wf21,plwf) + filter(dSDD_12_exp_3_1,wf22,plwf);

  dh1_1w_exp_3_1  = filter(dh1_1_exp_3_1,wf11,plwf) + filter(dh1_12_exp_3_1,wf12,plwf);
  dh1_12w_exp_3_1 = filter(dh1_1_exp_3_1,wf21,plwf) + filter(dh1_12_exp_3_1,wf22,plwf);
  
  dsh1_1w_exp_3_1  = filter(dsh1_1_exp_3_1,wf11,plwf) + filter(dsh1_12_exp_3_1,wf12,plwf);
  dsh1_12w_exp_3_1 = filter(dsh1_1_exp_3_1,wf21,plwf) + filter(dsh1_12_exp_3_1,wf22,plwf);

  dh2_1w_exp_3_1  = filter(dh2_1_exp_3_1,wf11,plwf) + filter(dh2_12_exp_3_1,wf12,plwf);
  dh2_12w_exp_3_1 = filter(dh2_1_exp_3_1,wf21,plwf) + filter(dh2_12_exp_3_1,wf22,plwf);
  
  dsh2_1w_exp_3_1  = filter(dsh2_1_exp_3_1,wf11,plwf) + filter(dsh2_12_exp_3_1,wf12,plwf);
  dsh2_12w_exp_3_1 = filter(dsh2_1_exp_3_1,wf21,plwf) + filter(dsh2_12_exp_3_1,wf22,plwf);

  dx1_1w_exp_3_1  = filter(dx1_1_exp_3_1,wf11,plwf) + filter(dx1_12_exp_3_1,wf12,plwf);
  dx1_12w_exp_3_1 = filter(dx1_1_exp_3_1,wf21,plwf) + filter(dx1_12_exp_3_1,wf22,plwf);

  dx2_1w_exp_3_1  = filter(dx2_1_exp_3_1,wf11,plwf) + filter(dx2_12_exp_3_1,wf12,plwf);
  dx2_12w_exp_3_1 = filter(dx2_1_exp_3_1,wf21,plwf) + filter(dx2_12_exp_3_1,wf22,plwf);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  dH_1w_exp_3_2  = filter(dH_1_exp_3_2,wf11,plwf) + filter(dH_12_exp_3_2,wf12,plwf);
  dH_12w_exp_3_2 = filter(dH_1_exp_3_2,wf21,plwf) + filter(dH_12_exp_3_2,wf22,plwf);
  
  dsH_1w_exp_3_2  = filter(dsH_1_exp_3_2,wf11,plwf) + filter(dsH_12_exp_3_2,wf12,plwf);
  dsH_12w_exp_3_2 = filter(dsH_1_exp_3_2,wf21,plwf) + filter(dsH_12_exp_3_2,wf22,plwf);

  dS11_1w_exp_3_2  = filter(dS11_1_exp_3_2,wf11,plwf) + filter(dS11_12_exp_3_2,wf12,plwf);
  dS11_12w_exp_3_2 = filter(dS11_1_exp_3_2,wf21,plwf) + filter(dS11_12_exp_3_2,wf22,plwf);

  dS1D_1w_exp_3_2  = filter(dS1D_1_exp_3_2,wf11,plwf) + filter(dS1D_12_exp_3_2,wf12,plwf);
  dS1D_12w_exp_3_2 = filter(dS1D_1_exp_3_2,wf21,plwf) + filter(dS1D_12_exp_3_2,wf22,plwf);

  dSD1_1w_exp_3_2  = filter(dSD1_1_exp_3_2,wf11,plwf) + filter(dSD1_12_exp_3_2,wf12,plwf);
  dSD1_12w_exp_3_2 = filter(dSD1_1_exp_3_2,wf21,plwf) + filter(dSD1_12_exp_3_2,wf22,plwf);

  dSDD_1w_exp_3_2  = filter(dSDD_1_exp_3_2,wf11,plwf) + filter(dSDD_12_exp_3_2,wf12,plwf);
  dSDD_12w_exp_3_2 = filter(dSDD_1_exp_3_2,wf21,plwf) + filter(dSDD_12_exp_3_2,wf22,plwf);

  dh1_1w_exp_3_2  = filter(dh1_1_exp_3_2,wf11,plwf) + filter(dh1_12_exp_3_2,wf12,plwf);
  dh1_12w_exp_3_2 = filter(dh1_1_exp_3_2,wf21,plwf) + filter(dh1_12_exp_3_2,wf22,plwf);
  
  dsh1_1w_exp_3_2  = filter(dsh1_1_exp_3_2,wf11,plwf) + filter(dsh1_12_exp_3_2,wf12,plwf);
  dsh1_12w_exp_3_2 = filter(dsh1_1_exp_3_2,wf21,plwf) + filter(dsh1_12_exp_3_2,wf22,plwf);

  dh2_1w_exp_3_2  = filter(dh2_1_exp_3_2,wf11,plwf) + filter(dh2_12_exp_3_2,wf12,plwf);
  dh2_12w_exp_3_2 = filter(dh2_1_exp_3_2,wf21,plwf) + filter(dh2_12_exp_3_2,wf22,plwf);
  
  dsh2_1w_exp_3_2  = filter(dsh2_1_exp_3_2,wf11,plwf) + filter(dsh2_12_exp_3_2,wf12,plwf);
  dsh2_12w_exp_3_2 = filter(dsh2_1_exp_3_2,wf21,plwf) + filter(dsh2_12_exp_3_2,wf22,plwf);

  dx1_1w_exp_3_2  = filter(dx1_1_exp_3_2,wf11,plwf) + filter(dx1_12_exp_3_2,wf12,plwf);
  dx1_12w_exp_3_2 = filter(dx1_1_exp_3_2,wf21,plwf) + filter(dx1_12_exp_3_2,wf22,plwf);

  dx2_1w_exp_3_2  = filter(dx2_1_exp_3_2,wf11,plwf) + filter(dx2_12_exp_3_2,wf12,plwf);
  dx2_12w_exp_3_2 = filter(dx2_1_exp_3_2,wf21,plwf) + filter(dx2_12_exp_3_2,wf22,plwf);

  
  %%% Build fit matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  fprintf('===== Build fit matrices - iter %s =====\n',num2str(1))

  % init data struct
  exps = struct();

  % exp 3.1 ch1
  dHw_exp_3_1 = [dH_1w_exp_3_1.y];
  dsHw_exp_3_1 = [dsH_1w_exp_3_1.y];
  dS11w_exp_3_1 = [dS11_1w_exp_3_1.y];
  dS1Dw_exp_3_1 = [dS1D_1w_exp_3_1.y];
  dSD1w_exp_3_1 = [dSD1_1w_exp_3_1.y];
  dSDDw_exp_3_1 = [dSDD_1w_exp_3_1.y];
  dh1w_exp_3_1 = [dh1_1w_exp_3_1.y];
  dsh1w_exp_3_1 = [dsh1_1w_exp_3_1.y];
  dh2w_exp_3_1 = [dh2_1w_exp_3_1.y];
  dsh2w_exp_3_1 = [dsh2_1w_exp_3_1.y];
  dx1w_exp_3_1 = [dx1_1w_exp_3_1.y];
  dx2w_exp_3_1 = [dx2_1w_exp_3_1.y];

  K1_exp_3_1 = [dHw_exp_3_1 dsHw_exp_3_1 dS11w_exp_3_1 dS1Dw_exp_3_1...
    dSD1w_exp_3_1 dSDDw_exp_3_1...
    dh2w_exp_3_1 dsh2w_exp_3_1 dx1w_exp_3_1 dx2w_exp_3_1];


  % cut the first 100 samples to remove WF transients effects
  K1_exp_3_1(1:100,:) = [];
  % fill struct basis
  exps(1).fitbasis = K1_exp_3_1;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % exp 3.1 ch12
  dHw_exp_3_1 = [dH_12w_exp_3_1.y];
  dsHw_exp_3_1 = [dsH_12w_exp_3_1.y];
  dS11w_exp_3_1 = [dS11_12w_exp_3_1.y];
  dS1Dw_exp_3_1 = [dS1D_12w_exp_3_1.y];
  dSD1w_exp_3_1 = [dSD1_12w_exp_3_1.y];
  dSDDw_exp_3_1 = [dSDD_12w_exp_3_1.y];
  dh1w_exp_3_1 = [dh1_12w_exp_3_1.y];
  dsh1w_exp_3_1 = [dsh1_12w_exp_3_1.y];
  dh2w_exp_3_1 = [dh2_12w_exp_3_1.y];
  dsh2w_exp_3_1 = [dsh2_12w_exp_3_1.y];
  dx1w_exp_3_1 = [dx1_12w_exp_3_1.y];
  dx2w_exp_3_1 = [dx2_12w_exp_3_1.y];

  K12_exp_3_1 = [dHw_exp_3_1 dsHw_exp_3_1 dS11w_exp_3_1 dS1Dw_exp_3_1...
    dSD1w_exp_3_1 dSDDw_exp_3_1...
    dh2w_exp_3_1 dsh2w_exp_3_1 dx1w_exp_3_1 dx2w_exp_3_1];
  
  clear dHw_exp_3_1 dsHw_exp_3_1 dS11w_exp_3_1 dS1Dw_exp_3_1 dSD1w_exp_3_1...
    dSDDw_exp_3_1 dh1w_exp_3_1 dsh1w_exp_3_1 dh2w_exp_3_1 dsh2w_exp_3_1...
    dx1w_exp_3_1 dx2w_exp_3_1

  % cut the first 100 samples to remove WF transients effects
  K12_exp_3_1(1:100,:) = [];
  % fill struct basis
  exps(2).fitbasis = K12_exp_3_1;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % exp 3.2 ch1
  dHw_exp_3_2 = [dH_1w_exp_3_2.y];
  dsHw_exp_3_2 = [dsH_1w_exp_3_2.y];
  dS11w_exp_3_2 = [dS11_1w_exp_3_2.y];
  dS1Dw_exp_3_2 = [dS1D_1w_exp_3_2.y];
  dSD1w_exp_3_2 = [dSD1_1w_exp_3_2.y];
  dSDDw_exp_3_2 = [dSDD_1w_exp_3_2.y];
  dh1w_exp_3_2 = [dh1_1w_exp_3_2.y];
  dsh1w_exp_3_2 = [dsh1_1w_exp_3_2.y];
  dh2w_exp_3_2 = [dh2_1w_exp_3_2.y];
  dsh2w_exp_3_2 = [dsh2_1w_exp_3_2.y];
  dx1w_exp_3_2 = [dx1_1w_exp_3_2.y];
  dx2w_exp_3_2 = [dx2_1w_exp_3_2.y];

  K1_exp_3_2 = [dHw_exp_3_2 dsHw_exp_3_2 dS11w_exp_3_2 dS1Dw_exp_3_2...
    dSD1w_exp_3_2 dSDDw_exp_3_2...
    dh2w_exp_3_2 dsh2w_exp_3_2 dx1w_exp_3_2 dx2w_exp_3_2];


  % cut the first 100 samples to remove WF transients effects
  K1_exp_3_2(1:100,:) = [];
  % fill struct basis
  exps(3).fitbasis = K1_exp_3_2;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % exp 3.2 ch12
  dHw_exp_3_2 = [dH_12w_exp_3_2.y];
  dsHw_exp_3_2 = [dsH_12w_exp_3_2.y];
  dS11w_exp_3_2 = [dS11_12w_exp_3_2.y];
  dS1Dw_exp_3_2 = [dS1D_12w_exp_3_2.y];
  dSD1w_exp_3_2 = [dSD1_12w_exp_3_2.y];
  dSDDw_exp_3_2 = [dSDD_12w_exp_3_2.y];
  dh1w_exp_3_2 = [dh1_12w_exp_3_2.y];
  dsh1w_exp_3_2 = [dsh1_12w_exp_3_2.y];
  dh2w_exp_3_2 = [dh2_12w_exp_3_2.y];
  dsh2w_exp_3_2 = [dsh2_12w_exp_3_2.y];
  dx1w_exp_3_2 = [dx1_12w_exp_3_2.y];
  dx2w_exp_3_2 = [dx2_12w_exp_3_2.y];

  K12_exp_3_2 = [dHw_exp_3_2 dsHw_exp_3_2 dS11w_exp_3_2 dS1Dw_exp_3_2...
    dSD1w_exp_3_2 dSDDw_exp_3_2...
    dh2w_exp_3_2 dsh2w_exp_3_2 dx1w_exp_3_2 dx2w_exp_3_2];
  
  clear dHw_exp_3_2 dsHw_exp_3_2 dS11w_exp_3_2 dS1Dw_exp_3_2...
    dSD1w_exp_3_2 dSDDw_exp_3_2 dh1w_exp_3_2 dsh1w_exp_3_2...
    dh2w_exp_3_2 dsh2w_exp_3_2 dx1w_exp_3_2 dx2w_exp_3_2
  
  % cut the first 100 samples to remove WF transients effects
  K12_exp_3_2(1:100,:) = [];
  % fill struct basis
  exps(4).fitbasis = K12_exp_3_2;

  
  %%% Input on groud measured parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  fprintf('===== Input on-groud measured parameters - iter %s =====\n',num2str(1))

  % init struct
  groundexps = struct;

  % value for S11
  groundexps(1).pos = 3;
  groundexps(1).value = 0;
  groundexps(1).err = 1e-4;

  % value for S1D
  groundexps(2).pos = 4;
  groundexps(2).value = 0;
  groundexps(2).err = 1e-3;

  % value for SDD
  groundexps(3).pos = 6;
  groundexps(3).value = 0;
  groundexps(3).err = 1e-4;

  
  %%% Get signals with nominal params %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  fprintf('===== Get signals with nominal params - iter %s =====\n',num2str(1))
  
  % get response with nominal params
  for kk = 1:numel(H.objs)
    H.objs(kk).setParams(parnames,exp3_nomvalues);
  end

  s11 = fftfilt(oi1,H.objs(1,1));
  s12 = fftfilt(oid,H.objs(1,2));
  s21 = fftfilt(oi1,H.objs(2,1));
  s22 = fftfilt(oid,H.objs(2,2));

  % get signals for exp 3.1
  ns1_exp_3_1 = s11;
  ns1_exp_3_1.setName;
  nsd_exp_3_1 = s21;
  nsd_exp_3_1.setName;

  % get signals for exp 3.2
  ns1_exp_3_2 = s12;
  ns1_exp_3_2.setName;
  nsd_exp_3_2 = s22;
  nsd_exp_3_2.setName;
  
  % subtract nominal response from true signals
  do1_exp_3_1 = o1_exp_3_1 - ns1_exp_3_1;
  dod_exp_3_1 = od_exp_3_1 - nsd_exp_3_1;
  
  do1_exp_3_2 = o1_exp_3_2 - ns1_exp_3_2;
  dod_exp_3_2 = od_exp_3_2 - nsd_exp_3_2;
  
  % do whitening
  o1w_exp_3_1 = filter(do1_exp_3_1,wf11,plcf) + filter(dod_exp_3_1,wf12,plcf);
  odw_exp_3_1 = filter(do1_exp_3_1,wf21,plcf) + filter(dod_exp_3_1,wf22,plcf);
  
  o1w_exp_3_2 = filter(do1_exp_3_2,wf11,plcf) + filter(dod_exp_3_2,wf12,plcf);
  odw_exp_3_2 = filter(do1_exp_3_2,wf21,plcf) + filter(dod_exp_3_2,wf22,plcf);
  
  
  %%% Build fit vectors %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  o1w_exp_3_1 = o1w_exp_3_1.y;
  % cut the first 100 samples to remove WF transients effects
  o1w_exp_3_1(1:100,:) = [];
  exps(1).fitdata = o1w_exp_3_1;
  
  odw_exp_3_1 = odw_exp_3_1.y;
  % cut the first 100 samples to remove WF transients effects
  odw_exp_3_1(1:100,:) = [];
  exps(2).fitdata = odw_exp_3_1;
  
  o1w_exp_3_2 = o1w_exp_3_2.y;
  % cut the first 100 samples to remove WF transients effects
  o1w_exp_3_2(1:100,:) = [];
  exps(3).fitdata = o1w_exp_3_2;
  
  odw_exp_3_2 = odw_exp_3_2.y;
  % cut the first 100 samples to remove WF transients effects
  odw_exp_3_2(1:100,:) = [];
  exps(4).fitdata = odw_exp_3_2;
  
  
  %%% do fit %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  fprintf('===== Do fit - iter %s =====\n',num2str(1))
%   [a,Ca,eCa,Corra,eCorra,Vu,bu,Cbu,eCbu,mse] = mdc3_exp1_linfit_v2(exps,groundexps);
  [a,Ca,Corra,Vu,bu,Cbu,Fbu,mse] = utils.math.linfitsvd(exps,groundexps);
  
  % store results
  mdc3_exp3_loop_results(1).a = a;
  mdc3_exp3_loop_results(1).Ca = Ca;
%   mdc3_exp3_loop_results(1).eCa = eCa;
  mdc3_exp3_loop_results(1).Corra = Corra;
%   mdc3_exp3_loop_results(1).eCorra = eCorra;
  mdc3_exp3_loop_results(1).Vu = Vu;
  mdc3_exp3_loop_results(1).bu = bu;
  mdc3_exp3_loop_results(1).Cbu = Cbu;
%   mdc3_exp3_loop_results(1).eCbu = eCbu;
  mdc3_exp3_loop_results(1).mse = mse;
  
  % update nominal values with fit result
  for kk=1:numel(usedparams)
    for dd=1:numel(parnames)
      if strcmp(usedparams{kk},parnames{dd})
        exp3_nomvalues{dd} = exp3_nomvalues{dd} + a(kk);
      end
    end
  end
  
  % store parameter estimation
   mdc3_exp3_loop_results(1).params = exp3_nomvalues;
   
% end


% %% save results
% 
% % get date string
% str = date;
% % write filename
% filenm = sprintf('mdc3_exp3_loop_results_%s.mat',str);
% % save
% save(['C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\MDC3_Exp3\' filenm], 'mdc3_exp3_loop_results')
% 
%% MSE progression

mseprog = zeros(N,1);
for ii=1:N
  mseprog(ii) = mdc3_exp3_loop_results(ii).mse;
end

figure
plot(mseprog,'*-')
xlabel('Fit Step')
ylabel('Meam Square Error')
% 
% %% Get params
% 
% fit_vals = zeros(numel(usedparams),1);
% true_vals = zeros(numel(usedparams),1);
% pars = mdc3_exp3_loop_results(N).params;
% 
% 
% for ii=1:numel(usedparams)
%   for jj=1:numel(parnames)
%     if strcmp(usedparams{ii},parnames{jj})
%       fit_vals(ii) = pars{jj};
%       true_vals(ii) = exp3_truevalues{jj};
%     end
%   end
% end
% 
% %% Testing
% 
% % input true values
% dH = true_vals(1);
% dsH = true_vals(2);
% dS11 = true_vals(3);
% dS1D = true_vals(4);
% dSD1 = true_vals(5);
% dSDD = true_vals(6);
% dh1 = true_vals(7);
% dsh1 = true_vals(8);
% dh2 = true_vals(9);
% dsh2 = true_vals(10);
% dx1 = true_vals(11);
% dx2 = true_vals(12);
% 
% 
% % load nominal and true values
% load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_nomvalues.mat
% load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_truevalues.mat
% 
% 
% %%% get response with true params
% for ii = 1:numel(H.objs)
%   H.objs(ii).setParams(parnames,exp3_truevalues);
% end
% 
% s11 = fftfilt(oi1,H.objs(1,1));
% s12 = fftfilt(oid,H.objs(1,2));
% s21 = fftfilt(oi1,H.objs(2,1));
% s22 = fftfilt(oid,H.objs(2,2));
% 
% % get signals for exp 3.1
% s1_exp_3_1 = s11;
% s1_exp_3_1.setName;
% sd_exp_3_1 = s21;
% sd_exp_3_1.setName;
% 
% % get signals for exp 3.2
% s1_exp_3_2 = s12;
% s1_exp_3_2.setName;
% sd_exp_3_2 = s22;
% sd_exp_3_2.setName;
% 
% %%% get response with nominal params
% for kk = 1:numel(H.objs)
%   H.objs(kk).setParams(parnames,exp3_nomvalues);
% end
% 
% s11 = fftfilt(oi1,H.objs(1,1));
% s12 = fftfilt(oid,H.objs(1,2));
% s21 = fftfilt(oi1,H.objs(2,1));
% s22 = fftfilt(oid,H.objs(2,2));
% 
% % get signals for exp 3.1
% ns1_exp_3_1 = s11;
% ns1_exp_3_1.setName;
% nsd_exp_3_1 = s21;
% nsd_exp_3_1.setName;
% 
% % get signals for exp 3.2
% ns1_exp_3_2 = s12;
% ns1_exp_3_2.setName;
% nsd_exp_3_2 = s22;
% nsd_exp_3_2.setName;
% 
% % subtract template from true signals
% ds1_exp_3_1 = s1_exp_3_1 - ns1_exp_3_1;
% dsd_exp_3_1 = sd_exp_3_1 - nsd_exp_3_1;
% 
% ds1_exp_3_2 = s1_exp_3_2 - ns1_exp_3_2;
% dsd_exp_3_2 = sd_exp_3_2 - nsd_exp_3_2;
% 
% %%% Get fit basis
% 
% % set nominal values
% for jj = 1:numel(H.objs)
%   T_dH.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dsH.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dS11.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dS1D.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dSD1.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dSDD.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dh1.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dsh1.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dh2.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dsh2.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dx1.objs(jj).setParams(parnames,exp3_nomvalues);
%   T_dx2.objs(jj).setParams(parnames,exp3_nomvalues);
% end
% 
% % get response for dH
% ds11 = fftfilt(oi1,T_dH.objs(1,1));
% ds12 = fftfilt(oid,T_dH.objs(1,2));
% ds21 = fftfilt(oi1,T_dH.objs(2,1));
% ds22 = fftfilt(oid,T_dH.objs(2,2));
% 
% dH_1_exp_3_1 = ds11;
% dH_1_exp_3_1.setName;
% dH_12_exp_3_1 = ds21;
% dH_12_exp_3_1.setName;
% dH_1_exp_3_2 = ds12;
% dH_1_exp_3_2.setName;
% dH_12_exp_3_2 = ds22;
% dH_12_exp_3_2.setName;
% 
% % get response for dsH
% ds11 = fftfilt(oi1,T_dsH.objs(1,1));
% ds12 = fftfilt(oid,T_dsH.objs(1,2));
% ds21 = fftfilt(oi1,T_dsH.objs(2,1));
% ds22 = fftfilt(oid,T_dsH.objs(2,2));
% 
% dsH_1_exp_3_1 = ds11;
% dsH_1_exp_3_1.setName;
% dsH_12_exp_3_1 = ds21;
% dsH_12_exp_3_1.setName;
% dsH_1_exp_3_2 = ds12;
% dsH_1_exp_3_2.setName;
% dsH_12_exp_3_2 = ds22;
% dsH_12_exp_3_2.setName;
% 
% % get response for dS11
% ds11 = fftfilt(oi1,T_dS11.objs(1,1));
% ds12 = fftfilt(oid,T_dS11.objs(1,2));
% ds21 = fftfilt(oi1,T_dS11.objs(2,1));
% ds22 = fftfilt(oid,T_dS11.objs(2,2));
% 
% dS11_1_exp_3_1 = ds11;
% dS11_1_exp_3_1.setName;
% dS11_12_exp_3_1 = ds21;
% dS11_12_exp_3_1.setName;
% dS11_1_exp_3_2 = ds12;
% dS11_1_exp_3_2.setName;
% dS11_12_exp_3_2 = ds22;
% dS11_12_exp_3_2.setName;
% 
% % get response for dS1D
% ds11 = fftfilt(oi1,T_dS1D.objs(1,1));
% ds12 = fftfilt(oid,T_dS1D.objs(1,2));
% ds21 = fftfilt(oi1,T_dS1D.objs(2,1));
% ds22 = fftfilt(oid,T_dS1D.objs(2,2));
% 
% dS1D_1_exp_3_1 = ds11;
% dS1D_1_exp_3_1.setName;
% dS1D_12_exp_3_1 = ds21;
% dS1D_12_exp_3_1.setName;
% dS1D_1_exp_3_2 = ds12;
% dS1D_1_exp_3_2.setName;
% dS1D_12_exp_3_2 = ds22;
% dS1D_12_exp_3_2.setName;
% 
% 
% % get response for SD1
% ds11 = fftfilt(oi1,T_dSD1.objs(1,1));
% ds12 = fftfilt(oid,T_dSD1.objs(1,2));
% ds21 = fftfilt(oi1,T_dSD1.objs(2,1));
% ds22 = fftfilt(oid,T_dSD1.objs(2,2));
% 
% dSD1_1_exp_3_1 = ds11;
% dSD1_1_exp_3_1.setName;
% dSD1_12_exp_3_1 = ds21;
% dSD1_12_exp_3_1.setName;
% dSD1_1_exp_3_2 = ds12;
% dSD1_1_exp_3_2.setName;
% dSD1_12_exp_3_2 = ds22;
% dSD1_12_exp_3_2.setName;
% 
% % get response for SDD
% ds11 = fftfilt(oi1,T_dSDD.objs(1,1));
% ds12 = fftfilt(oid,T_dSDD.objs(1,2));
% ds21 = fftfilt(oi1,T_dSDD.objs(2,1));
% ds22 = fftfilt(oid,T_dSDD.objs(2,2));
% 
% dSDD_1_exp_3_1 = ds11;
% dSDD_1_exp_3_1.setName;
% dSDD_12_exp_3_1 = ds21;
% dSDD_12_exp_3_1.setName;
% dSDD_1_exp_3_2 = ds12;
% dSDD_1_exp_3_2.setName;
% dSDD_12_exp_3_2 = ds22;
% dSDD_12_exp_3_2.setName;
% 
% % get response for dh1
% ds11 = fftfilt(oi1,T_dh1.objs(1,1));
% ds12 = fftfilt(oid,T_dh1.objs(1,2));
% ds21 = fftfilt(oi1,T_dh1.objs(2,1));
% ds22 = fftfilt(oid,T_dh1.objs(2,2));
% 
% dh1_1_exp_3_1 = ds11;
% dh1_1_exp_3_1.setName;
% dh1_12_exp_3_1 = ds21;
% dh1_12_exp_3_1.setName;
% dh1_1_exp_3_2 = ds12;
% dh1_1_exp_3_2.setName;
% dh1_12_exp_3_2 = ds22;
% dh1_12_exp_3_2.setName;
% 
% % get response for dsh1
% ds11 = fftfilt(oi1,T_dsh1.objs(1,1));
% ds12 = fftfilt(oid,T_dsh1.objs(1,2));
% ds21 = fftfilt(oi1,T_dsh1.objs(2,1));
% ds22 = fftfilt(oid,T_dsh1.objs(2,2));
% 
% dsh1_1_exp_3_1 = ds11;
% dsh1_1_exp_3_1.setName;
% dsh1_12_exp_3_1 = ds21;
% dsh1_12_exp_3_1.setName;
% dsh1_1_exp_3_2 = ds12;
% dsh1_1_exp_3_2.setName;
% dsh1_12_exp_3_2 = ds22;
% dsh1_12_exp_3_2.setName;
% 
% 
% % get response for dh2
% ds11 = fftfilt(oi1,T_dh2.objs(1,1));
% ds12 = fftfilt(oid,T_dh2.objs(1,2));
% ds21 = fftfilt(oi1,T_dh2.objs(2,1));
% ds22 = fftfilt(oid,T_dh2.objs(2,2));
% 
% dh2_1_exp_3_1 = ds11;
% dh2_1_exp_3_1.setName;
% dh2_12_exp_3_1 = ds21;
% dh2_12_exp_3_1.setName;
% dh2_1_exp_3_2 = ds12;
% dh2_1_exp_3_2.setName;
% dh2_12_exp_3_2 = ds22;
% dh2_12_exp_3_2.setName;
% 
% % get response for dsh2
% ds11 = fftfilt(oi1,T_dsh2.objs(1,1));
% ds12 = fftfilt(oid,T_dsh2.objs(1,2));
% ds21 = fftfilt(oi1,T_dsh2.objs(2,1));
% ds22 = fftfilt(oid,T_dsh2.objs(2,2));
% 
% dsh2_1_exp_3_1 = ds11;
% dsh2_1_exp_3_1.setName;
% dsh2_12_exp_3_1 = ds21;
% dsh2_12_exp_3_1.setName;
% dsh2_1_exp_3_2 = ds12;
% dsh2_1_exp_3_2.setName;
% dsh2_12_exp_3_2 = ds22;
% dsh2_12_exp_3_2.setName;
% 
% % get response for dx1
% ds11 = fftfilt(oi1,T_dx1.objs(1,1));
% ds12 = fftfilt(oid,T_dx1.objs(1,2));
% ds21 = fftfilt(oi1,T_dx1.objs(2,1));
% ds22 = fftfilt(oid,T_dx1.objs(2,2));
% 
% dx1_1_exp_3_1 = ds11;
% dx1_1_exp_3_1.setName;
% dx1_12_exp_3_1 = ds21;
% dx1_12_exp_3_1.setName;
% dx1_1_exp_3_2 = ds12;
% dx1_1_exp_3_2.setName;
% dx1_12_exp_3_2 = ds22;
% dx1_12_exp_3_2.setName;
% 
% % get response for dx2
% ds11 = fftfilt(oi1,T_dx2.objs(1,1));
% ds12 = fftfilt(oid,T_dx2.objs(1,2));
% ds21 = fftfilt(oi1,T_dx2.objs(2,1));
% ds22 = fftfilt(oid,T_dx2.objs(2,2));
% 
% dx2_1_exp_3_1 = ds11;
% dx2_1_exp_3_1.setName;
% dx2_12_exp_3_1 = ds21;
% dx2_12_exp_3_1.setName;
% dx2_1_exp_3_2 = ds12;
% dx2_1_exp_3_2.setName;
% dx2_12_exp_3_2 = ds22;
% dx2_12_exp_3_2.setName;
% 
% 
% 
% %%% Get model response
% mod1_3_1 = dH_1_exp_3_1.*dH + dsH_1_exp_3_1.*dsH + dS11_1_exp_3_1.*dS11 + dS1D_1_exp_3_1.*dS1D +...
%   dSD1_1_exp_3_1.*dSD1 + dSDD_1_exp_3_1.*dSDD + dh1_1_exp_3_1.*dh1 + dsh1_1_exp_3_1.*dsh1 + ...
%   dh2_1_exp_3_1.*dh2 + dsh2_1_exp_3_1.*dsh2 + dx1_1_exp_3_1.*dx1 + dx2_1_exp_3_1.*dx2;
% mod1_3_1.setName;
% 
% mod12_3_1 = dH_12_exp_3_1.*dH + dsH_12_exp_3_1.*dsH + dS11_12_exp_3_1.*dS11 + dS1D_12_exp_3_1.*dS1D +...
%   dSD1_12_exp_3_1.*dSD1 + dSDD_12_exp_3_1.*dSDD + dh1_12_exp_3_1.*dh1 + dsh1_12_exp_3_1.*dsh1 + ...
%   dh2_12_exp_3_1.*dh2 + dsh2_12_exp_3_1.*dsh2 + dx1_12_exp_3_1.*dx1 + dx2_12_exp_3_1.*dx2;
% mod12_3_1.setName;
% 
% mod1_3_2 = dH_1_exp_3_2.*dH + dsH_1_exp_3_2.*dsH + dS11_1_exp_3_2.*dS11 + dS1D_1_exp_3_2.*dS1D +...
%   dSD1_1_exp_3_2.*dSD1 + dSDD_1_exp_3_2.*dSDD + dh1_1_exp_3_2.*dh1 + dsh1_1_exp_3_2.*dsh1 + ...
%   dh2_1_exp_3_2.*dh2 + dsh2_1_exp_3_2.*dsh2 + dx1_1_exp_3_2.*dx1 + dx2_1_exp_3_2.*dx2;
% mod1_3_2.setName;
% 
% mod12_3_2 = dH_12_exp_3_2.*dH + dsH_12_exp_3_2.*dsH + dS11_12_exp_3_2.*dS11 + dS1D_12_exp_3_2.*dS1D +...
%   dSD1_12_exp_3_2.*dSD1 + dSDD_12_exp_3_2.*dSDD + dh1_12_exp_3_2.*dh1 + dsh1_12_exp_3_2.*dsh1 + ...
%   dh2_12_exp_3_2.*dh2 + dsh2_12_exp_3_2.*dsh2 + dx1_12_exp_3_2.*dx1 + dx2_12_exp_3_2.*dx2;
% mod12_3_2.setName;
% 
% 
% %% plot
% 
% i1o1lin = mod1_3_1;
% i1o1lin.setName('i1->o1 lin');
% i1oDlin = mod12_3_1;
% i1oDlin.setName('i1->oD lin');
% 
% iDo1lin = mod1_3_2;
% iDo1lin.setName('iD->o1 lin');
% iDoDlin = mod12_3_2;
% iDoDlin.setName('iD->oD lin');
% 
% i1o1nlin = s1_exp_3_1-ns1_exp_3_1-mod1_3_1;
% i1o1nlin.setName('i1->o1 nlin');
% i1oDnlin = sd_exp_3_1-nsd_exp_3_1-mod12_3_1;
% i1oDnlin.setName('i1->oD nlin');
% 
% iDo1nlin = s1_exp_3_2-ns1_exp_3_2-mod1_3_2;
% iDo1nlin.setName('iD->o1 nlin');
% iDoDnlin = sd_exp_3_2-nsd_exp_3_2-mod12_3_2;
% iDoDnlin.setName('iD->oD nlin');
% 
% iplot(i1o1lin,i1o1nlin)
% iplot(i1oDlin,i1oDnlin)
% iplot(iDo1lin,iDo1nlin)
% iplot(iDoDlin,iDoDnlin)
% 
% %% Get Euclidean Norm
% 
% n1lin = norm(i1o1lin.y);
% n2lin = norm(i1oDlin.y);
% n3lin = norm(iDo1lin.y);
% n4lin = norm(iDoDlin.y);
% 
% n1nlin = norm(i1o1nlin.y);
% n2nlin = norm(i1oDnlin.y);
% n3nlin = norm(iDo1nlin.y);
% n4nlin = norm(iDoDnlin.y);
%