PBSwissMX/matlab/identifyFxFyStage.m

function [mot1,mot2]=identifyFxFyStage()
  %sample idfrd
  %f = logspace(-1,1,100);
  %[mag,phase] = bode(idtf([1 .2],[1 2 1 1]),f);
  %response = mag.*exp(1j*phase*pi/180);
  %m = idfrd(response,f,0.08);

  function obj=loadData(path,motid)
    obj=struct();
    f=load(strcat(path,sprintf('curr_step%d.mat',motid)));
    obj.currstep=f;
    %prepend sone zeros to stable system identification
    obj.currstep=iddata([zeros(10,1); obj.currstep.data(:,2)],[zeros(10,1); obj.currstep.data(:,3)],50E-6);
    
    f=load(strcat(path,sprintf('full_bode_mot%d.mat',motid)));
    obj.w=f.frq*2*pi;            %convert from Hz to rad/s
    if motid==2
      f.db_mag(1:224)=f.db_mag(225); % reset bad values at low frequencies
    end
    obj.mag=10.^(f.db_mag/20);     %mag not in dB
    obj.phase=f.deg_phase*pi/180;  %phase in rad
    response = obj.mag.*exp(1j*obj.phase);
    obj.meas= idfrd(response,obj.w,0);

    fMdl=load(strcat(path,sprintf('model%d.mat',motid)));
    obj.mdl=fMdl;
    
  end

  function tfc=currstep(obj)
    opt=tfestOptions;
    opt.Display='off';
    tfc = tfest(obj.currstep, 2, 0,opt);
    s=str2ndOrd(tfc);
    %disp(s);
    %h = stepplot(tf1);
    %l=obj.currstep.OutputData
    t=(0:199)*50E-6;
    [y,t]=step(tfc,t);
    f=figure();
    subplot(1,2,1);
    plot(t*1000,obj.currstep.OutputData(11:210),'b',t*1000,y*1000,'r');
    xlabel('ms')
    ylabel('curr\_bits')
    grid on
    legend('real signal','model','Location','southeast')
    title(s);
    subplot(1,2,2);
    h=bodeplot(tfc,'r');
    setoptions(h,'FreqUnits','Hz','Grid','on');
  end

  function s=str2ndOrd(tf)
    den=tf.Denominator;
    num=tf.Numerator;
    k=num(1)/den(3);
    w0=sqrt(den(3));
    damp=den(2)/2/w0;
    s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp);
  end


  function mot=fyStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
    mot.tfc=currstep(mot);

    opt=tfestOptions;
    opt.Display='off';
    opt.initializeMethod='iv';
    opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion

    figure(); 
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
    mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);

    g11=tf(mot.mdl.numc,mot.mdl.denc);  % iqCmd->iqMeas  
    g12=tf([1 0],mot.mdl.denc*12); % iqCmd->iqVolts  : iqVolts= i_meas*R+i_meas'*L
    num=conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.numc);
    den=conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.denc);
    g13=tf(num,den); %iqCmd->ActPos
    %sys=ss([g11;g12])
    sys=ss([g11;g12;g13])
    
    
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    
  end
  function y=myNorm(y)
    %normalizes num and den by factor 1000
    %y.*10.^(3*(length(y):-1:1))    
  end

  function mot=fxStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2);
    currstep(mot);

    opt=tfestOptions;
    opt.Display='off';
    opt.initializeMethod='iv';
    opt.WeightingFilter=[1,4;10,670]*(2*pi); % Hz->rad/s conversion

    figure();
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
    mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
    mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);

    %create ss from tf MIMO:
    %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
    %Gspm=[tf_iqCmd_actVolts;tf_iqCmd_iqMeas;tf_iqCmd_actPos];
    %sys=ss(Gspm);
    
    %normalize: [1E6 1E3 1].*mot.mdl.denc
    numc=myNorm(mot.mdl.numc);
    denc=myNorm(mot.mdl.denc);
    num1=myNorm(mot.mdl.num1);
    den1=myNorm(mot.mdl.den1);
    num2=myNorm(mot.mdl.num2);
    den2=myNorm(mot.mdl.den2);
    num3=myNorm(mot.mdl.num3);
    den3=myNorm(mot.mdl.den3);
    num4=myNorm(mot.mdl.num4);
    den4=myNorm(mot.mdl.den4);
    num5=myNorm(mot.mdl.num5);
    den5=myNorm(mot.mdl.den5);
    num=myNorm(mot.mdl.num);
    den=myNorm(mot.mdl.den);

    %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
    %tf2ss MIMO
    %https://ch.mathworks.com/help/control/ref/append.html
    g1=tf(numc,denc);  % iqCmd->iqMeas  
    s1=ss(g1);
    s1.C=[s1.C; 1/s1.B(1) 0];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
    tf(s1) % display all transfer functions
    
    num=conv(conv(conv(conv(num1,num2),num3),num4),num5);
    den=conv(conv(conv(conv(den1,den2),den3),den4),den5);  
    g2=tf(num,den); %iqMeas->ActPos
    s2=ss(g2);

    s3=append(s1,s2);
    tf(s3)
    %connect iqMeas from s1 to iqMeas of s2
    s3.A(3,1)=1 %WHAT NUMBER ??? s3.B(3,2), s3.C2,:)?
    %remove the direct iqMeas input
    %s3.B(3,2)=0
    
    t_=tf(s3) 
    t_(3,1)
    figure;
    bode(t_(3,1))
    %compare with tf iqCmd->ActPos
    num=conv(conv(conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4),mot.mdl.num5),mot.mdl.numc);
    den=conv(conv(conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4),mot.mdl.den5),mot.mdl.denc);
    g4=tf(num,den); %iqCmd->ActPos
    figure;
    bode(g4)
    
    %sys=ss([g11;g12])
    sys=ss([g11;g12;g13])
    sys=ss(g13)

    
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    %controlSystemDesigner('bode',1,mot.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited

  end
  close all
  mot1=fyStage();
  mot2=fxStage();

end