sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2019-02-07 15:04:53 +01:00
parent db14034175
commit 3e83b82a5d
4 changed files with 273 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

415
matlab/identifyFxFyStage.m
View File

@@ -1,5 +1,5 @@
function [mot1,mot2]=identifyFxFyStage()
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
%
@@ -7,13 +7,14 @@ function [mot1,mot2]=identifyFxFyStage()
%
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
%
% the returned motor objects mot1 and mot2 contains:
%
% w,mag,phase : (gathered data with Python)
% 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
@@ -22,12 +23,21 @@ function [mot1,mot2]=identifyFxFyStage()
% 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/ )
% (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/<date>/ )
% - 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 : add ss-models and do checks for motor 1 fy
% 1 2 : add ss-models and do checks for setup motor 2 fx
% 2 4 : identify_currstep
% 3 8 : identify_tf2
% The default value for mode is 3
%References:
%create ss from tf MIMO:
%https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
@@ -43,9 +53,6 @@ function [mot1,mot2]=identifyFxFyStage()
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);
@@ -55,7 +62,8 @@ function [mot1,mot2]=identifyFxFyStage()
obj.mdl=fMdl;
end
function tfc=currstep(obj)
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);
@@ -77,6 +85,42 @@ function [mot1,mot2]=identifyFxFyStage()
print(f,sprintf('figures/currstep_%d',obj.id),'-depsc');
end
function tf2=identify_tf2(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;
@@ -101,6 +145,9 @@ function [mot1,mot2]=identifyFxFyStage()
ob='not ';%not observable
end
disp([s,' is ',ct,'controlable and ',ob,'observable.']);
%tf(ss) % display all transfer functions
end
function y=myNorm(y)
@@ -109,206 +156,220 @@ function [mot1,mot2]=identifyFxFyStage()
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);
figure()
h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_c1(3,1),'b',mot.ss_d1(3,1),'m',mot.ss_1(2,1),'c',mot.ss_0(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','no res','no cur + 1 res'});
legend(ax(1),'Location','sw',{'real','plant','c1','d1','1','0'});
print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc');
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=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid);
function mot=fyStage(mot)
mot.id=motid;
mot.tfc=currstep(mot);
%current loop iqCmd->iqMeas
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
opt=tfestOptions;
opt.Display='off';
opt.initializeMethod='iv';
opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion
%simplified current loop iqCmd->iqMeas (first order tf)
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
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
%resonance iqMeas->iqForce
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqForce');
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');
%current to position iqForce->actPos
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
ssLst=["tfc","tf1","tf2","tfc*tf1","tf1*tf2","tfc*tf1*tf2"];
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
%check observable/controlable of transfer functions
ssLst=["tfc","tfd","tf0","tf1","tf2","tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2"];
sys=[];
for s = ssLst
eval('sys=ss('+s+');')
%t=tf(sys);
%disp(evalc('t'))
chkCtrlObsv(sys,char(s));
end
end
% sample code:
%tfc iqCmd-> iqMeas
%tf2 resonance iqMeas->iqForce
%tf1 iqForce->(actVel,actPos)
%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,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');
%without resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage');
tf1.InputName{1}=s;%restore
%simplified current, without resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage');
tf1.InputName{1}=s;%restore
%no current loop, no resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos
% +-----------+
mot.ss_1=ss(tf1);
chkCtrlObsv(mot.ss_1,'ss_1 fyStage');
%simplified mechanics, no current loop, no resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos
% +-----------+
mot.ss_0=ss(tf0);
chkCtrlObsv(mot.ss_0,'ss_0 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()
motid=2;
%mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid);
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid);
mot.id=motid;
currstep(mot);
function mot=fxStage(mot)
%current loop iqCmd->iqMeas
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
opt=tfestOptions;
opt.Display='off';
opt.initializeMethod='iv';
opt.WeightingFilter=[1,4;10,670]*(2*pi); % Hz->rad/s conversion
%simplified current loop iqCmd->iqMeas (first order tf)
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
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');
%resonance iqMeas->iqForce
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqF1');
%resonance iqMeas->iqForce
tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF1','OutputName','iqF2');
%resonance iqMeas->iqForce
tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF2','OutputName','iqF3');
%resonance iqMeas->iqForce
tf5=tf(mot.mdl.num5,mot.mdl.den5,'InputName','iqF3','OutputName','iqForce');
%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');
%current to position iqForce->actPos
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
ssLst=["tfc","tf1","tf2","tf3","tf4","tf5","tfc*tf1","tf1*tf2","tf1*tf2*tf3","tfc*tf1*tf2"];
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
%check observable/controlable of transfer functions
ssLst=["tfc","tfd","tf0","tf1","tf2","tf3","tf4","tf5",...
"tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2","tf1*tf2*tf3"];
sys=[];
for s = ssLst
eval('sys=ss('+s+');')
%t=tf(sys);
%disp(evalc('t'))
chkCtrlObsv(sys,char(s));
end
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);
% best plant approximation
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
mot.ss_plt=connect(tfc,tf1,tf2,tf3,tf4,tf5,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_plt,'ss_plt fxStage');
%without resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_c1,'ss_c1 fxStage');
tf1.InputName{1}=s;%restore
%simplified current, without resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_d1,'ss_d1 fxStage');
tf1.InputName{1}=s;%restore
%no current loop, no resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos
% +-----------+
mot.ss_1=ss(tf1);
chkCtrlObsv(mot.ss_1,'ss_1 fxStage');
%simplified mechanics, no current loop, no resonance
% u +-----------+ y
%iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos
% +-----------+
mot.ss_0=ss(tf0);
chkCtrlObsv(mot.ss_0,'ss_0 fxStage');
plotBode(mot)
end
close all
mot1=fyStage();
mot2=fxStage();
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,4)
%identification of second order transfer function out of the current step recorded data.
identify_currstep(mot{motid});
end
if bitand(mode,8)
%identification of second order transfer function out of the position recorded data.
identify_tf2(mot);
end
if motid==1 && bitand(mode,1)
mot=fyStage(mot);
end
if motid==2 && bitand(mode,2)
mot=fxStage(mot);
end
motCell{motid}=mot;
end
%controlSystemDesigner('bode',1,mot1.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited