function motCell=identifyFxFyStage(mode) %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|-------> actVel % | 3|-------> actPos % +-----------+ % % the returned motor objects mot1 and mot2 contains: % % currstep : gathered data with Python: file 'curr_step%d.mat' % w,mag,phase : gathered data with Python: file 'full_bode_mot%d.mat' % 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// ) % - curr_step[1|2].mat % - full_bode_mot[1|2].mat % - model[1|2].mat % % loadData reads members currstep,w,mag,phase,meas % % mode bits: % 0 1 : select motor 1 fy % 1 2 : select motor 2 fx % 2 4 : add ss-models and do obser/contr checks % 3 8 : identify_currstep % 4 16 : identify_tf (TODO!) % The default value for mode is 7 %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 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=identify_currstep(obj) %identification of second order transfer function out of the current step recorded data. opt=tfestOptions; opt.Display='off'; tfc = tfest(obj.currstep, 2, 0,opt); s=splitlines(string(evalc('tfc')));disp(join(s(5:7),newline)); s=str2ndOrd(tfc); t=(0:199)*50E-6; [y,t]=step(tfc,t); f=figure();f.Position=[200,100,900,500]; 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'); %print(sprintf('figures/currstep_%d',obj.id),'-dpng','-r0'); print(f,sprintf('figures/currstep_%d',obj.id),'-depsc'); end function tf2=identify_tf(obj) opt=tfestOptions; opt.Display='off'; opt.initializeMethod='iv'; %opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion opt.WeightingFilter=[1,2;10,100]*(2*pi); % Hz->rad/s conversion figure(); tf2 = tfest(obj.meas, 2, 0, opt);disp(str2ndOrd(tf2)); subplot(1,1,1); h=bodeplot(tf2,'r',obj.meas,'b',obj.w); setoptions(h,'FreqUnits','Hz','Grid','on'); p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p); ax=h.getaxes(); legend(ax(1),'Location','sw',{'real','tf2'}); frq=obj.w/(2*pi) [m1,p1,w1]=bode(obj.meas,obj.w); [m2,p2,w2]=bode(tf2,obj.w); m1=20*log10(reshape(m1,[],1));p1=reshape(p1,[],1); m2=20*log10(reshape(m2,[],1));p2=reshape(p2,[],1); me=m1-m2; pe=p1-p2; figure(); ax1=subplot(2,1,1); title('remaining mag (dB) and phase error') semilogx(frq,me,'r'); ax2=subplot(2,1,2); semilogx(frq,pe,'r'); linkaxes([ax1,ax2],'x') ax2.YLim=[-90 90];ax2.YLimMode='manual'; ax2.XLim=[frq(1), frq(1000)];ax2.XLimMode='manual'; grid(ax1,'on');grid(ax2,'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.']); %tf(ss) % display all transfer functions end function y=myNorm(y) %normalizes num and den by factor 1000 %y.*10.^(3*(length(y):-1:1)) end function plotBode(mot) try figure() catch return end h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_cp(3,1),'b',mot.ss_dp(3,1),'m',mot.ss_p(2,1),'c',mot.ss_q(2,1),'k',mot.w); setoptions(h,'FreqUnits','Hz','Grid','on'); p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p); ax=h.getaxes(); legend(ax(1),'Location','sw',{'real','plant','cp','dp','p','q'}); print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc'); end function mot=fyStage(mot) %current loop iqCmd->iqMeas tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); %simplified current loop iqCmd->iqMeas (first order tf) tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'}); %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'}); %simplified force(=current) to velocity iqForce->(actVel) tfv=tf(mot.mdl.numv,mot.mdl.denv,'InputName','iqForce','OutputName',{'actVel'}); %resonance iqMeas->iqForce tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqForce'); %current to position iqForce->actPos %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos'); %check observable/controlable of transfer functions ssLst=["tfc","tfd","tfp","tfq","tfv","tf1","tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfc*tfv","tfd*tfv","tfd*tfv*tf1"]; sys=[]; for s = ssLst eval('sys=ss('+s+');') chkCtrlObsv(sys,char(s)); end % sample code: %tfc iqCmd-> iqMeas %tfp iqForce->(actVel,actPos) %tf1 resonance iqMeas->iqForce %connect(tfc,tf2,'iqCmd','iqForce'); %connect(tfc,tf2,'iqCmd',{'iqMeas','iqForce'}); %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','iqForce','actPos'}); %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','actPos'}); % best plant approximation % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ mot.ss_plt=connect(tfc,tfp,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_plt.Name='best plant approximation'; chkCtrlObsv(mot.ss_plt,'ss_plt fyStage'); % best plant approximation without friction (always -40dB) % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ mot.ss_cqr=connect(tfc,tfq,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_cqr.Name='plant no friction'; chkCtrlObsv(mot.ss_plt,'ss_cqr fyStage'); %without resonance % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ s=tfp.InputName{1};tfp.InputName{1}='iqMeas'; mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_cp.Name='without resonance'; chkCtrlObsv(mot.ss_cp,'ss_cp fyStage'); tfp.InputName{1}=s;%restore %simplified current, without resonance % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ s=tfp.InputName{1};tfp.InputName{1}='iqMeas'; mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_dp.Name='simplified current, without resonance'; chkCtrlObsv(mot.ss_dp,'ss_dp fyStage'); tfp.InputName{1}=s;%restore %no current loop, no resonance % u +-----------+ y %iqCmd------->|1 1|-------> actVel % | 2|-------> actPos % +-----------+ mot.ss_p=ss(tfp); mot.ss_p.Name='no current loop, no resonance'; chkCtrlObsv(mot.ss_p,'ss_p fyStage'); %simplified mechanics, no current loop, no resonance % u +-----------+ y %iqCmd------->|1 1|-------> actVel % | 2|-------> actPos % +-----------+ mot.ss_q=ss(tfq); mot.ss_q.Name='simplified mechanics, no current loop, no resonance'; chkCtrlObsv(mot.ss_q,'ss_q fyStage'); %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(mot) %current loop iqCmd->iqMeas tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); %simplified current loop iqCmd->iqMeas (first order tf) tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'}); %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'}); %resonance iqMeas->iqForce tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqF1'); %resonance iqMeas->iqForce tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqF1','OutputName','iqF2'); %resonance iqMeas->iqForce tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF2','OutputName','iqF3'); %resonance iqMeas->iqForce tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF3','OutputName','iqForce'); %current to position iqForce->actPos %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos'); %check observable/controlable of transfer functions ssLst=["tfc","tfd","tfp","tfq","tf1","tf2","tf3","tf4",... "tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfd*tfp*tf1","tfc*tf1","tfd*tf1","tfc*tf1*tf2","tfd*tf1*tf2"]; sys=[]; for s = ssLst eval('sys=ss('+s+');') chkCtrlObsv(sys,char(s)); end % best plant approximation % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ mot.ss_plt=connect(tfc,tfp,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'}); chkCtrlObsv(mot.ss_plt,'ss_plt fxStage'); % best plant approximation without friction (always -40dB) % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ mot.ss_cqr=connect(tfc,tfq,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_cqr.Name='plant no friction'; chkCtrlObsv(mot.ss_cqr,'ss_plt0 fxStage'); %without resonance % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ s=tfp.InputName{1};tfp.InputName{1}='iqMeas'; mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'}); chkCtrlObsv(mot.ss_cp,'ss_cp fxStage'); tfp.InputName{1}=s;%restore %simplified current, without resonance % u +-----------+ y %iqCmd------->|1 1|-------> iqMeas % | 2|-------> actVel % | 3|-------> actPos % +-----------+ s=tfp.InputName{1};tfp.InputName{1}='iqMeas'; mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'}); chkCtrlObsv(mot.ss_dp,'ss_dp fxStage'); tfp.InputName{1}=s;%restore %no current loop, no resonance % u +-----------+ y %iqCmd------->|1 1|-------> actVel % | 2|-------> actPos % +-----------+ mot.ss_p=ss(tfp); chkCtrlObsv(mot.ss_p,'ss_p fxStage'); %simplified mechanics, no current loop, no resonance % u +-----------+ y %iqCmd------->|1 1|-------> actVel % | 2|-------> actPos % +-----------+ mot.ss_q=ss(tfq); chkCtrlObsv(mot.ss_q,'ss_q fxStage'); plotBode(mot) end close all motCell=cell(2,1); for motid= 1:2 mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid); mot.id=motid; if bitand(mode,motid) if bitand(mode,4) if motid==1 mot=fyStage(mot); else mot=fxStage(mot); end end if bitand(mode,8) %identification of second order transfer function out of the current step recorded data. identify_currstep(mot); end if bitand(mode,16) %identification of second order transfer function out of the position recorded data. identify_tf(mot); end end motCell{motid}=mot; end %controlSystemDesigner('bode',1,mot1.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited end