function [mot1,mot2]=identifyFxFyStage()
  %loads recorded data of the current step and bode diagrams of the stages then plots the bode diagrams and identifies
  %the current step transfer function
  %
  %finally it builds a ss-Model of the stage with:
  %
  %  u          +-----------+           y
  %iqCmd------->|1         1|-------> iqMeas
  %             |          2|-------> iqVolts
  %             |          3|-------> actPos
  %             +-----------+
  %
  % the returned motor objects mot1 and mot2 contains:
  %
  % w,mag,phase : (gathered data with Python)
  % meas        : a MATLAB idfrd model with data w,mag,phase
  % mdl         : a structure with the python numerators and denominators for the transfer functions
  % tfc,tf_mdl  : various transfer functions
  % ssPlt       : the final continous state space model of the plant (not observable, not controlable)
  % ssMdl       : the simplified continous state space model for the observer  (observable, controlable)
  % ssMdlNC     : model without resonance and current loop
  %
  % The used data files (generated from Python) are:
  % (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/ )
  % - curr_step[1|2].mat
  % - full_bode_mot[1|2].mat
  % - model[1|2].mat


  %References:
  %create ss from tf MIMO:
  %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
  %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
  %https://ch.mathworks.com/help/control/ref/append.html

  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);
    t=(0:199)*50E-6;
    [y,t]=step(tfc,t);
    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',k,w0,damp);
  end

  function chkCtrlObsv(ss,s)
    P=ctrb(ss.A,ss.B);
    if rank(ss.A)==rank(P)
      ct='';%controlable
    else
      ct='not ';%not controlable
    end

    Q=obsv(ss.A,ss.C);
    if rank(ss.A)==rank(Q)
      ob='';%sys observable
    else
      ob='not ';%not observable
    end
    disp([s,' is ',ct,'controlable and ',ob,'observable.']);
  end

  function y=myNorm(y)
    %normalizes num and den by factor 1000
    %y.*10.^(3*(length(y):-1:1))
  end

  function plotBode(mot)
    t1=tf(mot.ssPlt);t2=tf(mot.ssMdl_c1);t3=tf(mot.ssMdl_12);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',t3(1,1),'m',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    ax=h.getaxes();
    legend(ax(1),'Location','sw',{'real','plant','no res','no cur + 1 res'});
  end

  function mot=fyStage()
    motid=1;
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid);
    mot.id=motid;
    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);
    %ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected

    tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop  iqCmd->iqMeas
    tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
    tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
    %state -space model:  ssc:current ssm:mechanics ssa:all (current+mechanics)

    % plant
    %  u          +-----------+           y
    %iqCmd------->|1         1|-------> iqMeas
    %             |          2|-------> iqVolts
    %             |          3|-------> actPos
    %             +-----------+
    ssc=ss(tfc);
    ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
    ssm=ss(tf1*tf2); %iqMeas->ActPos
    ssa=append(ssc,ssm);
    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
    mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
    mot.ssPlt.InputName{1}='iqCmd';
    mot.ssPlt.OutputName{1}='iqMeas';
    mot.ssPlt.OutputName{2}='iqVolts';
    mot.ssPlt.OutputName{3}='actPos';
    chkCtrlObsv(mot.ssPlt,'ssPlt fyStage');
    %tf(ssa) % display all transfer functions

    %simplified model without resonance
    %  u          +-----------+           y
    %iqCmd------->|1         1|-------> iqMeas
    %             |          2|-------> iqVolts
    %             |          3|-------> actPos
    %             +-----------+
    ssm=ss(tf1); %iqMeas->ActPos
    ssa=append(ssc,ssm);
    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
    mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
    mot.ssMdl_c1.InputName{1}='iqCmd';
    mot.ssMdl_c1.OutputName{1}='iqMeas';
    mot.ssMdl_c1.OutputName{2}='iqVolts';
    mot.ssMdl_c1.OutputName{3}='actPos';
    chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fyStage');

    %model without current loop, with one resonance
    %this assumes that the iqCmd->iqMeas is not relevant for motion
    %  u          +-----------+           y
    %iqMeas------>|1         1|-------> actPos
    %             +-----------+
    ssm=ss(tf1*tf2); %iqMeas->ActPos
    mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
    mot.ssMdl_12.InputName{1}='iqMeas';
    mot.ssMdl_12.OutputName{1}='actPos';
    chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fyStage');

    %model without current loop, no resonance
    %this assumes that the iqCmd->iqMeas is not relevant for motion
    %  u          +-----------+           y
    %iqMeas------>|1         1|-------> actPos
    %             +-----------+
    ssm=ss(tf1); %iqMeas->ActPos
    mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
    mot.ssMdl_1.InputName{1}='iqMeas';
    mot.ssMdl_1.OutputName{1}='actPos';
    chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fyStage');
    
    ssLst=["tfc","tf1","tf2","tfc*tf1","tf1*tf2","tfc*tf1*tf2"];
    sys=[];
    for s = ssLst
      eval('sys=ss('+s+');')
      %t=tf(sys);
      %disp(evalc('t'))
      chkCtrlObsv(sys,char(s));
    end

    %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);
    plotBode(mot)
  end

  function mot=fxStage()
    motid=2;
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid);
    mot.id=motid;
    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);

    tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop  iqCmd->iqMeas
    tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
    tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
    tf3=tf(mot.mdl.num3,mot.mdl.den3); %resonance
    tf4=tf(mot.mdl.num4,mot.mdl.den4); %resonance
    tf5=tf(mot.mdl.num5,mot.mdl.den5); %resonance

    %state -space model:  ssc:current ssm:mechanics ssa:all (current+mechanics)

    % plant
    %  u          +-----------+           y
    %iqCmd------->|1         1|-------> iqMeas
    %             |          2|-------> iqVolts
    %             |          3|-------> actPos
    %             +-----------+
    ssc=ss(tfc);
    ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
    ssm=ss(tf1*tf2*tf3*tf4*tf5); %iqMeas->ActPos
    ssa=append(ssc,ssm);
    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
    mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
    mot.ssPlt.InputName{1}='iqCmd';
    mot.ssPlt.OutputName{1}='iqMeas';
    mot.ssPlt.OutputName{2}='iqVolts';
    mot.ssPlt.OutputName{3}='actPos'  ;
    chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');

    %simplified model without resonance
    %  u          +-----------+           y
    %iqCmd------->|1         1|-------> iqMeas
    %             |          2|-------> iqVolts
    %             |          3|-------> actPos
    %             +-----------+
    ssm=ss(tf1); %iqMeas->ActPos
    ssa=append(ssc,ssm);
    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
    mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
    mot.ssMdl_c1.InputName{1}='iqCmd';
    mot.ssMdl_c1.OutputName{1}='iqMeas';
    mot.ssMdl_c1.OutputName{2}='iqVolts';
    mot.ssMdl_c1.OutputName{3}='actPos';
    chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fxStage');

    %model without current loop, with one resonance
    %this assumes that the iqCmd->iqMeas is not relevant for motion
    %  u          +-----------+           y
    %iqMeas------>|1         1|-------> actPos
    %             +-----------+
    ssm=ss(tf1*tf2); %iqMeas->ActPos
    mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
    mot.ssMdl_12.InputName{1}='iqMeas';
    mot.ssMdl_12.OutputName{1}='actPos';
    chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fxStage');   
    
    %model without current loop, no resonance
    %this assumes that the iqCmd->iqMeas is not relevant for motion
    %  u          +-----------+           y
    %iqMeas------>|1         1|-------> actPos
    %             +-----------+
    ssm=ss(tf1); %iqMeas->ActPos
    mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
    mot.ssMdl_1.InputName{1}='iqMeas';
    mot.ssMdl_1.OutputName{1}='actPos';
    chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fxStage');   
    
    ssLst=["tfc","tf1","tf2","tf3","tf4","tf5","tfc*tf1","tf1*tf2","tf1*tf2*tf3","tfc*tf1*tf2"];
    sys=[];
    for s = ssLst
      eval('sys=ss('+s+');')
      %t=tf(sys);
      %disp(evalc('t'))
      chkCtrlObsv(sys,char(s));
    end

    %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);
    plotBode(mot)
  end
  close all
  mot1=fyStage();
  mot2=fxStage();
  %controlSystemDesigner('bode',1,mot1.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited


end