
% Compute Rejection transfer function
% based on OL and CL measurements with white noise injected.
% White noise>> noise of system (disturbance

% compare results with theory (Model identification + controller)

% to be updated for Paranal

%file_OL='TFOL2007-03-15T16.10.37.mat'
%file_CL='TFCL2007-03-15T16.14.31.mat'

[file_OL, PathName] = uigetfile('TF*', 'Open Loop File'); 
eval([ 'cd ''',PathName,'''']);

[file_CL, PathName] = uigetfile('TF*', 'Closed Loop File'); 
eval([ 'cd ''',PathName,'''']);


eval([ 'load( ''',file_OL,''')']);
eval([ 'load( ''',file_CL,''')']);

rej_garchingx=TxCL./TxOL;
rej_garchingy=TyCL./TyOL;

% System Definition  SQ
delay_fit=20e-3; %delay_fit=17.5e-3;
gain_fit=4;
fc=25;
Tau = 1/2/pi/fc;  
SQ = gain_fit*tf(1,[Tau,1]);
set(SQ,'iodelay',delay_fit);
hSQ = squeeze(freqresp(SQ, FOL*2*pi));
% Controller:
 GainC=0.005;
 C= GainC*tf([1,0],[1 -1],0.001);
 hC = squeeze(freqresp(C, FOL*2*pi));
%Controller with Phase compensation
% C = tf([0.0192176331967172 -0.0363287291707719 0.0171450120698483], [0.0900069674431828 -0.179929144646882 0.0899221772036989], 0.001);
% hC = squeeze(freqresp(C, FOL*2*pi));


% Quadcell
%GainQ=2/pi;
%TauQuad=1/2/pi/280;
%Q = GainQ*tf(1,[Tau,1])*tf(1,[TauQuad,1]);
%CL=hSQ./(1+hC.*hSQ);
rej=1./(1+hC.*hSQ);



%figure(10);
 %   set(gca,'FontSize',13);grid
  %  semilogx(FOL,20*log10(abs(TxOL)),'r.-',FOL,20*log10(abs(hSQ)),'b.-');grid
  %  title({['TFOL2007-02-22T16.44.41.mat'],['TFCL2007-02-22T15.22.07.mat']})
   % xlabel('Frequency in Hz');ylabel('Magnitude in dB')
   % legend('X','Y','Predicted')



figure(10);
    set(gca,'FontSize',13);grid
    semilogx(FOL,20*log10(abs(rej_garchingx)),'r.-',FOL,20*log10(abs(rej_garchingy)),'b.-',FOL,20*log10(abs(rej)),'k',FOL,-3*ones(1,length(FOL)),'k');grid
    semilogx(FOL,20*log10(abs(rej_garchingx)),'r.-',FOL,20*log10(abs(rej_garchingy)),'b.-',FOL,-3*ones(1,length(FOL)),'k');grid

    xlabel('Frequency in Hz');ylabel('Magnitude in dB')
    legend('X','Y','Predicted')
    title({[file_OL],[file_CL]})
    
figure(20)
    set(gca,'FontSize',13);grid
    semilogx(FOL,unwrap(angle(rej_garchingx))*180/pi,'r.-',FOL,unwrap(angle(rej_garchingy))*180/pi,'b.-');grid  
    xlabel('Frequency in Hz');ylabel('Phase in deg')
    legend('X','Y')
    title({[file_OL],[file_CL]})
    


