%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TEST csd2tf 2dim identification
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HISTORY:     23-04-2009 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERSION

'$Id: test2_csd2tf.m,v 1.1 2009/04/23 10:11:26 luigi Exp $';

%% Clear

clear all

%% Loading spectra

load ..\m-toolbox\test\mpsd.mat % load mpsd.mat first column is f then psd1, csd and psd2

f = mpsd(:,1);
% psd1 = mpsd(:,2);
% csd12 = mpsd(:,3);
% psd2 = mpsd(:,4);

csd = zeros(2,2,numel(f));
for ii = 1:numel(f)
  csd(:,:,ii) = [mpsd(ii,2) mpsd(ii,3);conj(mpsd(ii,3)) mpsd(ii,4)];
end

%% identification in z-domain

% identification params
params = struct('Nmaxiter',70, 'minorder',20,...
    'maxorder',40, 'spolesopt',2, 'weightparam',2, 'plot',0,...
    'ctp','chival','lrscond',4,'msevar',2,...
    'fs',10,'dterm',0, 'spy',0, 'fullauto',1,...
    'extweights', []);

ostruct = utils.math.csd2tf(csd,f,params);

%% Model response TF11

pfparams.type = 'disc';
pfparams.freq = f;
pfparams.fs = 10;
pfparams.res = ostruct(1).res(:,1);
pfparams.pol = ostruct(1).poles(:,1);
pfparams.dterm = ostruct(1).dterm(:,1);

% response of the model filter
pfr = utils.math.pfresp(pfparams);
y11 = pfr.resp;

%% Model response TF12

pfparams.type = 'disc';
pfparams.freq = f;
pfparams.fs = 10;
pfparams.res = ostruct(2).res(:,1);
pfparams.pol = ostruct(2).poles(:,1);
pfparams.dterm = ostruct(2).dterm(:,1);

% response of the model filter
pfr = utils.math.pfresp(pfparams);
y12 = pfr.resp;

%% Model response TF21

pfparams.type = 'disc';
pfparams.freq = f;
pfparams.fs = 10;
pfparams.res = ostruct(1).res(:,2);
pfparams.pol = ostruct(1).poles(:,1);
pfparams.dterm = ostruct(1).dterm(:,2);

% response of the model filter
pfr = utils.math.pfresp(pfparams);
y21 = pfr.resp;

%% Model response TF22

pfparams.type = 'disc';
pfparams.freq = f;
pfparams.fs = 10;
pfparams.res = ostruct(2).res(:,2);
pfparams.pol = ostruct(2).poles(:,1);
pfparams.dterm = ostruct(2).dterm(:,2);

% response of the model filter
pfr = utils.math.pfresp(pfparams);
y22 = pfr.resp;

%% Model PSD calculation

%     psd1(f) = tf11(f)*conj(tf11(f))+tf12(f)*conj(tf12(f))
%     csd(f) = tf11(f)*conj(tf21(f))+tf12(f)*conj(tf22(f))
%     psd2(f) = tf21(f)*conj(tf21(f))+tf22(f)*conj(tf22(f))

% mpsd1 = y11.*conj(y11)+y12.*conj(y12);
% mcsd = y11.*conj(y21)+y12.*conj(y22);
% mpsd2 = y21.*conj(y21)+y22.*conj(y22);

mpsd1 = conj(y11).*y11+conj(y12).*y12;
mcsd = conj(y11).*y21+conj(y12).*y22;
mpsd2 = conj(y21).*y21+conj(y22).*y22;

%% Checking result

figure()
loglog(f,squeeze(csd(1,1,:)),'k')
hold on
loglog(f,mpsd1,'r')
xlabel('Frequency [Hz]')
ylabel('psd')
legend('Original', 'Model')

figure()
loglog(f,abs(squeeze(csd(1,2,:))),'k')
hold on
loglog(f,abs(mcsd),'r')
xlabel('Frequency [Hz]')
ylabel('psd')
legend('Original', 'Model')

figure()
loglog(f,squeeze(csd(2,2,:)),'k')
hold on
loglog(f,mpsd2,'r')
xlabel('Frequency [Hz]')
ylabel('psd')
legend('Original', 'Model')

figure()
semilogy(ostruct(1).mse)
xlabel('Iteration')
ylabel('MSE')
legend('Mean Squared Error Progression for filters 11 and 21')

figure()
semilogy(ostruct(2).mse)
xlabel('Iteration')
ylabel('MSE')
legend('Mean Squared Error Progression for filters 12 and 22')