wip
This commit is contained in:
@@ -19,6 +19,7 @@ function [mot1,mot2]=identifyFxFyStage()
|
||||
% 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/ )
|
||||
@@ -97,7 +98,19 @@ function [mot1,mot2]=identifyFxFyStage()
|
||||
else
|
||||
ob='not ';%not observable
|
||||
end
|
||||
disp([s,' is ',ct,'controlable and ',ob,'observable.']);
|
||||
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()
|
||||
@@ -116,60 +129,80 @@ function [mot1,mot2]=identifyFxFyStage()
|
||||
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
|
||||
%ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected
|
||||
|
||||
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); %resonance
|
||||
den2=myNorm(mot.mdl.den2);
|
||||
g1=tf(numc,denc); % iqCmd->iqMeas
|
||||
s1=ss(g1);
|
||||
s1.C=[s1.C; 1E5* 2.4E-3 1E-3*s1.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
|
||||
%tf(s1) % display all transfer functions
|
||||
num=conv(num1,num2);%num=1;
|
||||
den=conv(den1,den2);%den=[1 0 0];
|
||||
g2=tf(num,den); %iqMeas->ActPos
|
||||
s2=ss(g2);
|
||||
s3=append(s1,s2);
|
||||
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
|
||||
mot.ssPlt=ss(s3.A,s3.B(:,1),s3.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');
|
||||
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
|
||||
g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies
|
||||
s2=ss(g2);
|
||||
s3=append(s1,s2);
|
||||
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
|
||||
mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
|
||||
mot.ssMdl.InputName{1}='iqCmd';
|
||||
mot.ssMdl.OutputName{1}='iqMeas';
|
||||
mot.ssMdl.OutputName{2}='iqVolts';
|
||||
mot.ssMdl.OutputName{3}='actPos';
|
||||
chkCtrlObsv(mot.ssMdl,'ssMdl fyStage');
|
||||
% 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');
|
||||
|
||||
%filter in front of plant to suppress resonances (inverse of reonance)
|
||||
den=num2;%num=1;
|
||||
num=den2;%den=[1 0 0];
|
||||
mot.prefilt=tf(num,den);
|
||||
%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);
|
||||
t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',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))
|
||||
plotBode(mot)
|
||||
end
|
||||
|
||||
function mot=fxStage()
|
||||
@@ -188,73 +221,88 @@ function [mot1,mot2]=identifyFxFyStage()
|
||||
mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
|
||||
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
|
||||
|
||||
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); %resonance
|
||||
den2=myNorm(mot.mdl.den2);
|
||||
num3=myNorm(mot.mdl.num3); %resonance
|
||||
den3=myNorm(mot.mdl.den3);
|
||||
num4=myNorm(mot.mdl.num4); %resonance
|
||||
den4=myNorm(mot.mdl.den4);
|
||||
num5=myNorm(mot.mdl.num5); %resonance
|
||||
den5=myNorm(mot.mdl.den5);
|
||||
%num=myNorm(mot.mdl.num);
|
||||
%den=myNorm(mot.mdl.den);
|
||||
g1=tf(numc,denc); % iqCmd->iqMeas
|
||||
s1=ss(g1);
|
||||
s1.C=[s1.C; 1E5* 2.4E-3 1E-3*s1.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
|
||||
%tf(s1) % display all transfer functions
|
||||
num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1;
|
||||
den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0];
|
||||
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
|
||||
|
||||
g2=tf(num,den); %iqMeas->ActPos
|
||||
s2=ss(g2);
|
||||
s3=append(s1,s2);
|
||||
%t_=tf(s3);
|
||||
%bode(g2);figure;bode(t_(3,2));
|
||||
%connect iqMeas from s1 to iqMeas of s2
|
||||
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
|
||||
%state -space model: ssc:current ssm:mechanics ssa:all (current+mechanics)
|
||||
|
||||
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
|
||||
mot.ssPlt=ss(s3.A,s3.B(:,1),s3.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');
|
||||
% 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
|
||||
g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies
|
||||
s2=ss(g2);
|
||||
s3=append(s1,s2);
|
||||
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
|
||||
mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
|
||||
mot.ssMdl.InputName=mot.ssPlt.InputName;
|
||||
mot.ssMdl.OutputName=mot.ssPlt.OutputName;
|
||||
chkCtrlObsv(mot.ssMdl,'ssMdl fxStage');
|
||||
% 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');
|
||||
|
||||
%filter in front of plant to suppress resonances (inverse of reonance)
|
||||
den=conv(conv(conv(num2,num3),num4),num5);%num=1;
|
||||
num=conv(conv(conv(den2,den3),den4),den5);%den=[1 0 0];
|
||||
mot.prefilt=tf(num,den);
|
||||
%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);
|
||||
t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',mot.w);
|
||||
setoptions(h,'FreqUnits','Hz','Grid','on');
|
||||
%controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited
|
||||
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user