function fdfit
% use of the FDIDENT toolbox to fit to high frequency suspension dynamics

% *************************************************************************
% read and plot the transfer function
% *************************************************************************

foo = load('2016-11-17_H1SUSETMY_L1_Actuation_HighFreqChar_asciidump.txt');
a = foo(:,1);
b = foo(:,2);
c = foo(:,3);
A = [a,b,c];
% a(1:8), 8 pts, 68.1292 to 1000.6 Hz
% a(9:17), 9 pts, 68.1292 to 824.2650 Hz
% a(18:26), 9 pts, 68.1292 to 824.2650 Hz
% a(27:35), 9 pts, 68.1292 to 824.2650 Hz
% a(36:297), 262 pts, 98 to 460 Hz
% a(298:348), 51 pts, 90 to 400 Hz
B = sortrows(A);
f = B(:,1);
C = B(:,2) + B(:,3) * i;
figure; subplot(2,1,1); loglog(f,abs(C)); 
title('Bode Diagram'); ylabel('Magnitude'); xlim([60 1000]); axis 'auto y';
subplot(2,1,2); semilogx(f,angle(C)*180/pi); 
xlabel('frequency (Hz)'); ylabel('Phase (deg'); xlim([60 1000]); axis 'auto y';

% *************************************************************************
% prepare the data
% *************************************************************************

% frd can't handle duplicate frequencies
% for the moment drop duplicates
[uf,ia,iu]=unique(f);
ur=B(ia,2);
ui=B(ia,3);
% the last two frequency pts are distant from all others; drop
%uf=uf(1:end-2);
%ur=ur(1:end-2);
%ui=ui(1:end-2);

% increase the amplitide temporarily because the power spectrum magnitude
% is very small in this example
uC=10^20*(ur+ui*i);
um=abs(uC);
%up=angle(uC)*180/pi;

% define variances
sys=frd(uC,uf);
%sys2=idfrd(uC,uf);
sys.FrequencyUnit='Hz';
sys.Name='quadL1';
fdat=fiddata(sys);
fdat.InputVariance=0;
OutputVariance=0.1*um.*um;
% define high variance for the narrow peaks with apparent noise so that 
% multiple high-Q peaks aren't attempted int he fit
OutputVariance(end-52:end-21)=2*um(end-52:end-21).*um(end-52:end-21);
fdat.OutputVariance=OutputVariance;

% bode plot system
opts = bodeoptions('cstprefs');
opts.FreqUnits = 'Hz';
opts.PhaseWrapping = 'on';
% phase wrapping control options don't seem to work
%opts.PhaseWrappingBranch = -180;
%opts.PhaseMatching = 'on';
%opts.PhaseMatchingFreq = 250;
%opts.PhaseMatchingValue = 0;
opts.MagUnits = 'abs';
opts.MagScale = 'log';
opts.XLim = {[60 1000]};
%opts.YLabel.String(1) = 'Magnitude (m/ct)';
figure; h = bodeplot(sys,opts);

figure; subplot(2,1,1); loglog(uf,abs(uC)); 
title('Bode Diagram'); ylabel('Magnitude'); xlim([60 1000]); axis 'auto y';
subplot(2,1,2); semilogx(uf,angle(uC)*180/pi); 
xlabel('frequency (Hz)'); ylabel('Phase (deg'); xlim([60 1000]); axis 'auto y';

% *************************************************************************
%fdtool - launches GUI for fdident toolbox
% *************************************************************************
% set user level interactive
% open "read frequency domain data"
% import, from workspace, fdat
% close "read frequency domain data"
% open "variances and averaging"
% default is to take variances from data, simply execute & close
% open "estimate plant model" for manual fitting
% set: run modifiers, stabilization, to reflection
% set: max iterations to 2000
% set fixed poles at zero to 6
% set order of denominator to be 6 greater than numerator, e.g. 20:26
% set delay to variable with starting value 0 (zero)
% select "show ELiS", then Start
% close "variances and averaging", then open "evaluate or compare plant
% models", then save selected model, choose type fidmodel, LTI ss, or LTI zpk
% however delay is ignored for LTI models

% iterative fdtool fitting results saved
% unstable, non-minimum phase, with delay:
%      m2228fid.mat, m2228ss.mat, m2228zpk.mat
% stable, non-minimum phase, with delay:
%      m2430fid.mat, m2430ss.mat, m2430zpk.mat
% stable, non-minimum phase, without delay:
%      m2228fid.mat, m2228ss.mat, m2228zpk.mat
% N.B.: phases don't seem right with ss model - use zpk

load m2228fid;
delay = m2228fid.delay;
load m2228zpk;
fr = logspace(1,3,1000);
H = freqresp(m2228zpk/10^20,fr,'Hz');
Hm=squeeze(abs(H));
bias=0;
Hp=(squeeze(angle(H))+delay*fr')*180/pi+bias;
figure; subplot(2,1,1); loglog(fr,Hm,f,abs(C),'gx'); set(gca,'fontsize',18);
title('Bode Diagram'); ylabel('Magnitude'); xlim([10 1100]); axis 'auto y';
subplot(2,1,2); semilogx(fr,Hp,fr,squeeze(angle(H))*180/pi,f,angle(C)*180/pi,'gx'); 
set(gca,'fontsize',18);
xlabel('frequency (Hz)'); ylabel('Phase (deg'); xlim([10 1100]); axis 'auto y';

% as an alternative to using the interactive tool, launch elis directly
% however I've not figured out how to impose N poles at zero frequency
% here's my attempt to impose 6 poles at zero frequency in a 22:28 model:
%fixobj = fidmodel('s',NaN*ones(1,22),[0 0 0 0 0 0 NaN NaN NaN NaN ...
%    NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN]);
%pdat=elis(fdat,'s',22,28,struct('delaytreat','v','stabilization','r', ...
%    'forceminimumphase','r','fixedpars',fixobj,'itmax',500));
%pdat=elis(fdat,'s',22,28,struct('delaytreat','v','stabilization','r', ...
%    'fixedpars',fixobj,'itmax',500));
%pdat=elis(fdat,'s',22,28,struct('delaytreat','v', ...
%    'fixedpars',fixobj,'itmax',500));
% figure; bode(pdat);


% *************************************************************************
% try fitting the transfer function in two parts
% *************************************************************************
% part A
sysA=frd(uC(1:243),uf(1:243));
ufA=uf(1:243);
umA=abs(uC(1:243));
upA=angle(uC(1:243))*180/pi;
%sys2=idfrd(uC,uf);
sysA.FrequencyUnit='rad/s';
sysA.Name='quadL1';
fdatA=fiddata(sysA);
fdatA.InputVariance=0;
OutputVariance=0.1*umA.*umA;
%OutputVariance(end-52:end-21)=2*um(end-52:end-21).*um(end-52:end-21);
%OutputVariance(end-20:end)=0.1*um(end-20:end).*um(end-20:end);
fdatA.OutputVariance=OutputVariance;
% best fit, stabilization = reflection, variable delay
% 6 poles at zero, 11 num, 15 denom
% NEED +2 in denom of part B
% saved as elisA (fidmodel)
% savded as elisAss (LTI ss)
% saved as elisAzpk (LTI zpk)

% part B
sysB=frd(uC(237:end),uf(237:end));
ufB=uf(237:end);
umB=abs(uC(237:end));
upB=angle(uC(237:end))*180/pi;
%sys2=idfrd(uC,uf);
sysB.FrequencyUnit='rad/s';
sysB.Name='quadL1';
fdatB=fiddata(sysB);
fdatB.InputVariance=0;
OutputVariance=0.1*umB.*umB;
%OutputVariance(end-52:end-21)=.5*um(end-52:end-21).*um(end-52:end-21);
%OutputVariance(end-22:end)=0.05*um(end-22:end).*um(end-22:end);
OutputVariance(end)=umB(end).*umB(end);
%OutputVariance(end-52:end-21)=2*um(end-52:end-21).*um(end-52:end-21);
%OutputVariance(end-20:end)=0.1*um(end-20:end).*um(end-20:end);
fdatB.OutputVariance=OutputVariance;
% best fit, stabilization = reflection, variable delay
% 0 poles at zero, 11 num, 13 denom
% saved as elisB (fidmodel)
% savded as elisBss (LTI ss)
% saved as elisBzpk (LTI zpk)

% match at overlapping frequencies?
%g = WHAT?
%elisC = g * elisA * elisB;

%fr = logspace(1,3,100);
%elisCss = ss(elisC);
%opts = bodeoptions('cstprefs');
%opts.FreqUnits = 'Hz';
%opts.PhaseWrapping = 'on';
%opts.MagUnits = 'abs';
%opts.MagScale = 'log';
%opts.XLim = {[50 1000]};
%figure; h = bodeplot(elisCss,sys,opts);

end

