% Test script for zDomainFit on noisy data
% 
% L. Ferraioli 02-12-08
% 
% $Id: test_ao_zDomainFit_3.m,v 1.3 2009/04/20 14:31:14 luigi Exp $
% 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Smooth psd model
% 
% We define a smooth psd model and generate colored noise with such a psd
% shape. Then we fit sqrt(psd) of noisy data following two different
% procedure to decide when the fit is satisfactory and we can exit from the
% fitting routine.
% The first procedure is based on the check of residuals spectral flatness.
% The second procedure is based on the check of residuals log distance to
% fitted data.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Useful constants

fs = 10; % sampling frequency Hz
Nsecs = 1e5; % Number of seconds in the data stream
Nfft = 1e4; % Number of samples in the fft
pls  = plist('Nfft', Nfft,'Order',0); % plist for spectra

%% Building a frequency response AO

% Create a frequency-series AO
% pl_data = plist('fsfcn', '0.01./(0.01+f)', 'f1', 1e-6, 'f2', 5, 'nf', 1000);
pl_data = plist('fsfcn', '(1e-3./(f).^2 + 1e3./(0.001+f) + 1e5.*f.^2).*1e-10', 'f1', 1e-6, 'f2', 5, 'nf', 300);
mod = ao(pl_data);
mod.setName('psd model');

iplot(mod)

%% Building white noise

a = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));
a.setName('white noise')
a.setYunits('m')

%% Calling the noise generator

pl = plist(...
    'model', mod, ...
    'MaxIter', 50, ...
    'PoleType', 2, ...
    'MinOrder', 15, ...
    'MaxOrder', 25, ...
    'Weights', 2, ...
    'Plot', false,...
    'Disp', false,...
    'RMSEVar', 8,...
    'FitTolerance', 2);

ac = noisegen1D(a, pl);

%% Spetrum and Transfer function

acxx = ac.psd(pls); % spectrum of data
etf = tfe(a,ac,pls); % calculating transfer functions from data

iplot(acxx)
iplot(etf(1,2))

%% Fitting Sqrt Spectrum - Residuals spectral flatness checking

% Fitting parameter list
pl_fit1 = plist('FS',fs,...
  'AutoSearch','on',...
  'StartPoles',[],...
  'StartPolesOpt','c2',...
  'maxiter',60,...
  'minorder',19,...
  'maxorder',25,...
  'weights',[],...
  'weightparam','abs',...
  'ResLogDiff',[],...
  'ResFlat',0.7,...
  'RMSE',5,...
  'Plot','off',...
  'ForceStability','off',...
  'CheckProgress','on');

% Do fit
[filt2,bsp1,bspres1,bsprmse1] = zDomainFit(sqrt(acxx./2), pl_fit1); % division by 2 is necessary because acxx is the one-sided psd
bsp1.setName('Fit1');

%% Checing spectral flatness
% checking the spectral flatness of the normalized residuals in order to be
% sure that the procedure works correctly
kk = abs(bspres1./bsp1);
rf = utils.math.spflat(kk.data.y);
iplot(abs(bspres1./bsp1))

%% Fitting Spectrum - Residuals log difference checking

% Fitting parameter list
pl_fit2 = plist('FS',fs,...
  'AutoSearch','on',...
  'StartPoles',[],...
  'StartPolesOpt','c2',...
  'maxiter',60,...
  'minorder',17,...
  'maxorder',20,...
  'weights',[],...
  'weightparam','abs',...
  'ResLogDiff',0.5,...
  'ResFlat',0.7,...
  'RMSE',5,...
  'Plot','off',...
  'ForceStability','off',...
  'CheckProgress','on');

% Do fit
[filt2,bsp2,bspres2,bsprmse2] = zDomainFit(sqrt(acxx./2), pl_fit2); % division by 2 is necessary because acxx is the one-sided psd
bsp2.setName('Fit2');

%% Model calculated at the same frequencies of the fit AOs

% This step is useful in order to compare the results of the two fitting
% procedures
f = acxx.data.x;
pl_datar = plist('fsfcn', '(1e-3./(f).^2 + 1e3./(0.001+f) + 1e5.*f.^2).*1e-10', 'f', f);
modr = ao(pl_datar);
modr.setName('psd model');

%% Checking the two fit results
% plot the normalized difference between fit results and model data in
% order to check the fit accuracy
plpl = plist('Legends',{'Fit1','Fit2'});
iplot(abs(bsp1-sqrt(modr))./sqrt(modr),abs(bsp2-sqrt(modr))./sqrt(modr),plpl)

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Peaked psd model
% 
% We define a psd model with some resonance peaks and generate colored
% noise with such a psd shape. Then we fit sqrt(psd) of noisy data
% following two different procedure to decide when the fit is satisfactory
% and we can exit from the fitting routine.
% The first procedure is based on the check of residuals spectral flatness.
% The second procedure is based on the check of residuals log distance to
% fitted data.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Building a frequency response AO

% Create a psd model with two peak resonance at 5e-2 and 7e-2 Hz
func = '(1e-3./(f).^2 + 1e3./(0.001+f) + 1e5.*f.^2).*1e-10 + 1e-6./((f./7e-2).^2-1).^2 + 1e-5./((f./5e-2).^2-1).^2';
pl_data = plist('fsfcn', func, 'f1', 1e-6, 'f2', 5, 'nf', 300);
mod = ao(pl_data);
mod.setName('psd model');

iplot(mod)

%% Building white noise

a = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs, 'yunits', 'm'));
a.setName('white noise')

%% Calling the noise generator

pl = plist(...
    'model', mod, ... % Multiplication by fs needed to preserve energy
    'MaxIter', 70, ...
    'PoleType', 2, ...
    'MinOrder', 30, ...
    'MaxOrder', 45, ...
    'Weights', 2, ...
    'Plot', false,...
    'Disp', false,...
    'RMSEVar', 8,...
    'FitTolerance', 2);

ac = noisegen1D(a, pl);

%% Spetrum and Transfer function

acxx = ac.psd(pls); % spectrum of data

iplot(acxx)

%% Fitting Sqrt Spectrum - Residuals spectral flatness checking

% Fitting parameter list
pl_fit1 = plist('FS',fs,...
  'AutoSearch','on',...
  'StartPoles',[],...
  'StartPolesOpt','c2',...
  'maxiter',60,...
  'minorder',15,...
  'maxorder',25,...
  'weights',[],...
  'weightparam','abs',...
  'ResLogDiff',[],...
  'ResFlat',0.7,...
  'RMSE',6,...
  'Plot','off',...
  'ForceStability','off',...
  'CheckProgress','on');

% Do fit
[filt1,bsp1,bspres1,bsprmse1] = zDomainFit(sqrt(acxx./2), pl_fit1); % division by 2 is necessary because acxx is the one-sided psd
bsp1.setName('Fit1');

%%

iplot(sqrt(acxx./2),bsp1)

%% Checing spectral flatness
% checking the spectral flatness of the normalized residuals in order to be
% sure that the procedure works correctly
kk = abs(bspres1./bsp1);
rf = utils.math.spflat(kk.data.y);
iplot(abs(bspres1./bsp1))

%% Fitting Spectrum - Residuals log difference checking

% Fitting parameter list
pl_fit2 = plist('FS',fs,...
  'AutoSearch','on',...
  'StartPoles',[],...
  'StartPolesOpt','c2',...
  'maxiter',60,...
  'minorder',15,...
  'maxorder',25,...
  'weights',[],...
  'weightparam','abs',...
  'ResLogDiff',0.5,...
  'ResFlat',0.7,...
  'RMSE',6,...
  'Plot','off',...
  'ForceStability','off',...
  'CheckProgress','on');

% Do fit
[filt2,bsp2,bspres2,bsprmse2] = zDomainFit(sqrt(acxx./2), pl_fit2); % division by 2 is necessary because acxx is the one-sided psd
bsp2.setName('Fit2');

%%

iplot(sqrt(acxx./2),bsp2)

%% Model calculated at the same frequencies of the fit AOs

% This step is useful in order to compare the results of the two fitting
% procedures
f = acxx.data.x;
func = '(1e-3./(f).^2 + 1e3./(0.001+f) + 1e5.*f.^2).*1e-10 + 1e-6./((f./7e-2).^2-1).^2 + 1e-5./((f./5e-2).^2-1).^2';
pl_datar = plist('fsfcn', func, 'f', f);
modr = ao(pl_datar);
modr.setName('psd model');

%% Checking the two fit results
% plot the normalized difference between fit results and model data in
% order to check the fit accuracy
plpl = plist('Legends',{'Fit1','Fit2'});
iplot(abs(bsp1-sqrt(modr))./sqrt(modr),abs(bsp2-sqrt(modr))./sqrt(modr),plpl)