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

first result with observer controller

Browse Source
This commit is contained in:
zamofing_t
2018-10-31 16:54:09 +01:00
parent dd408e6956
commit 8807ccbd85
4 changed files with 169 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
matlab/StateSpaceControlDesign.m
View File

@@ -16,43 +16,35 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
% %
% https://www.youtube.com/watch?v=Lax3etc837U % https://www.youtube.com/watch?v=Lax3etc837U
ss_ol=mot.ss; %ss_ol=mot.ssPlt;
ss_ol.Name='open loop'; ss_ol_plt=mot.ssPlt; %real plant (model of real plant)
ss_ol_plt.Name='open loop plant';
ss_ol_mdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
ss_ol_mdl.Name='open loop model';
[Ap,Bp,Cp,Dp]=ssdata(ss_ol_plt);
[Am,Bm,Cm,Dm]=ssdata(ss_ol_mdl);
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only %sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
figure();h=bodeplot(ss_ol); figure();h=bodeplot(ss_ol_plt,ss_ol_mdl);
setoptions(h,'IOGrouping','all') setoptions(h,'IOGrouping','all')
A=ss_ol.A;
B=ss_ol.B;
C=ss_ol.C;
D=ss_ol.D;
P=ctrb(A,B);
if rank(A)==rank(P)
disp('sys controlable')
else
disp('sys not controlable')
end
Q=obsv(A,C);
if rank(A)==rank(Q)
disp('sys observable')
else
disp('sys not observable')
end
% step answer on open loop: % step answer on open loop:
t = 0:1E-4:.5; t = 0:1E-4:.5;
u = ones(size(t)); u = ones(size(t));
x0 = zeros(1,length(ss_ol.A)); xp0 = zeros(1,length(Ap));
xm0 = zeros(1,length(Am));
[y,t,x] = lsim(ss_ol,u,t,x0); [yp,t,x] = lsim(ss_ol_plt,u,t,xp0);
figure();plot(t,y);title('step on open loop'); [ym,t,x] = lsim(ss_ol_mdl,u,t,xm0);
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
poles = eig(A); poles = eig(Am);
w0=abs(poles); %w0=abs(poles);
ang=angle(-poles); %ang=angle(-poles);
%------------------- %-------------------
%p=w0.*exp(j.*ang) %p=w0.*exp(j.*ang)
@@ -61,38 +53,57 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%place poles for the controller feedback %place poles for the controller feedback
if motid==1 if motid==1
%2500rad/s = 397Hz -> locate poles here %2500rad/s = 397Hz -> locate poles here
p1=-3300+2800i; %6300rad/s = 1027Hz -> locate poles here
p2=-2700+500i; if length(poles)==4
p3=-2500+10i; p1=-6300+2800i;
%p1=-3300+2800i; p2=-6200+1500i;
%p2=-1500+500i; P=[p1 p1' p2 p2'];
%p3=-1200+10i; else
P=[p1 p1' p2 p2' p3 p3']; p1=-3300+2800i;
p2=-2700+500i;
p3=-2500+10i;
%p1=-3300+2800i;
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];
end
else else
%2500rad/s = 397Hz -> locate poles here %2500rad/s = 397Hz -> locate poles here
p1=-3300+2800i; %6300rad/s = 1027Hz -> locate poles here
p2=-1900+130i; if length(poles)==4
p3=-2900+80i; p1=-6300+2800i;
p4=-2300+450i; p2=-6200+1500i;
p5=-2000+20i; P=[p1 p1' p2 p2'];
p6=-1500+10i; else
%p1=-3300+2800i; p1=-3300+2800i;
%p2=-1500+500i; p2=-1900+130i;
%p3=-1200+10i; p3=-2900+80i;
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6']; p4=-2300+450i;
p5=-2000+20i;
p6=-1500+10i;
%p1=-3300+2800i;
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
end
end end
K = place(A,B,P); K = place(Am,Bm,P);
%K = acker(A,B,P); %K = acker(Am,Bm,Pm);
V=-1./(C*(A-B*K)^-1*B); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
%Nbar(2)=1; %the voltage stuff is crap for now %Nbar(2)=1; %the voltage stuff is crap for now
if length(V)>1 if length(V)>1
V=V(3); % only the position scaling needed V=V(3); % only the position scaling needed
end end
%prefilter to compensate non observable resonance frequencies
numV=mot.prefilt.Numerator{1};
denV=mot.prefilt.Denominator{1};
% step answer on closed loop with space state controller: % step answer on closed loop with space state controller:
t = 0:1E-4:.5; t = 0:1E-4:.5;
ss_cl = ss(A-B*K,B*V,C,0,'Name','space state controller','InputName',mot.ss.InputName,'OutputName',mot.ss.OutputName); ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',mot.ssMdl.InputName,'OutputName',mot.ssMdl.OutputName);
[y,t,x]=lsim(ss_cl,V*u,t,x0); [y,t,x]=lsim(ss_cl,V*u,t,xm0);
figure();plot(t,y);title('step on closed loop'); figure();plot(t,y);title('step on closed loop');
@@ -102,29 +113,38 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
if motid==1 if motid==1
op1=(p1*5); op1=(p1*5);
op2=(p2*5); op2=(p2*5);
op3=(p3*5); if length(poles)>4
OP=[op1 op1' op2 op2' op3 op3']; op3=(p3*5);
OP=[op1 op1' op2 op2' op3 op3'];
else
OP=[op1 op1' op2 op2'];
end
else else
op1=(p1*2); op1=(p1*2);
op2=(p2*2); op2=(p2*2);
op3=(p3*2); if length(poles)>4
op4=(p4*2); op3=(p3*2);
op5=(p5*2); op4=(p4*2);
op6=(p6*2); op5=(p5*2);
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6']; op6=(p6*2);
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6'];
else
OP=[op1 op1' op2 op2'];
end
end end
L=place(A',C',OP)'; L=place(Am',Cm',OP)';
%L=acker(A',C',OP)'; %L=acker(A',C',OP)';
At = [ A-B*K B*K At = [ Am-Bm*K Bm*K
zeros(size(A)) A-L*C ]; zeros(size(Am)) Am-L*Cm ];
Bt = [ B*V Bt = [ Bm*V
zeros(size(B)) ]; zeros(size(Bm)) ];
Ct = [ C zeros(size(C)) ]; Ct = [ Cm zeros(size(Cm)) ];
% step answer on closed loop with observer controller: % step answer on closed loop with observer controller:
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ss.OutputName); ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
figure();lsim(ss_o,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer'); figure();lsim(ss_o,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
% *** disctrete observer controller *** % *** disctrete observer controller ***
@@ -134,7 +154,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
ss_od .Name='discrete obsvr ctrl'; ss_od .Name='discrete obsvr ctrl';
% step answer on closed loop with disctrete observer controller: % step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05; t = 0:Ts:.05;
figure();lsim(ss_od ,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer discrete'); figure();lsim(ss_od ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
%plot all bode diagrams of desPos->actPos %plot all bode diagrams of desPos->actPos
@@ -148,8 +168,8 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%calculate matrices for the simulink system %calculate matrices for the simulink system
Ao=A-L*C; Ao=Am-L*Cm;
Bo=[B L]; Bo=[Bm L];
Co=K; Co=K;
Do=zeros(size(Co,1),size(Bo,2)); Do=zeros(size(Co,1),size(Bo,2));
mdlName='observer'; mdlName='observer';
@@ -157,7 +177,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%state space controller %state space controller
ssc=struct(); ssc=struct();
for k=["Ts","A","B","C","D","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od"] for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od","numV","denV"]
ssc=setfield(ssc,k,eval(k)); ssc=setfield(ssc,k,eval(k));
end end

101
matlab/identifyFxFyStage.m
View File

@@ -17,7 +17,8 @@ function [mot1,mot2]=identifyFxFyStage()
% meas : a MATLAB idfrd model with data w,mag,phase % meas : a MATLAB idfrd model with data w,mag,phase
% mdl : a structure with the python numerators and denominators for the transfer functions % mdl : a structure with the python numerators and denominators for the transfer functions
% tfc,tf_mdl : various transfer functions % tfc,tf_mdl : various transfer functions
% ss : the final continous state space model of the plant % 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)
% %
% The used data files (generated from Python) are: % 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/18_10_02/ )
@@ -51,7 +52,6 @@ function [mot1,mot2]=identifyFxFyStage()
fMdl=load(strcat(path,sprintf('model%d.mat',motid))); fMdl=load(strcat(path,sprintf('model%d.mat',motid)));
obj.mdl=fMdl; obj.mdl=fMdl;
end end
function tfc=currstep(obj) function tfc=currstep(obj)
@@ -61,7 +61,7 @@ function [mot1,mot2]=identifyFxFyStage()
s=str2ndOrd(tfc); s=str2ndOrd(tfc);
t=(0:199)*50E-6; t=(0:199)*50E-6;
[y,t]=step(tfc,t); [y,t]=step(tfc,t);
f=figure(); figure();
subplot(1,2,1); subplot(1,2,1);
plot(t*1000,obj.currstep.OutputData(11:210),'b',t*1000,y*1000,'r'); plot(t*1000,obj.currstep.OutputData(11:210),'b',t*1000,y*1000,'r');
xlabel('ms') xlabel('ms')
@@ -83,6 +83,22 @@ function [mot1,mot2]=identifyFxFyStage()
s=sprintf('k:%g w0:%g damp:%g',k,w0,damp); s=sprintf('k:%g w0:%g damp:%g',k,w0,damp);
end 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 mot=fyStage() function mot=fyStage()
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1); mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
@@ -102,7 +118,7 @@ function [mot1,mot2]=identifyFxFyStage()
denc=myNorm(mot.mdl.denc); denc=myNorm(mot.mdl.denc);
num1=myNorm(mot.mdl.num1); num1=myNorm(mot.mdl.num1);
den1=myNorm(mot.mdl.den1); den1=myNorm(mot.mdl.den1);
num2=myNorm(mot.mdl.num2); num2=myNorm(mot.mdl.num2); %resonance
den2=myNorm(mot.mdl.den2); den2=myNorm(mot.mdl.den2);
g1=tf(numc,denc); % iqCmd->iqMeas g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1); s1=ss(g1);
@@ -114,15 +130,38 @@ function [mot1,mot2]=identifyFxFyStage()
s2=ss(g2); s2=ss(g2);
s3=append(s1,s2); s3=append(s1,s2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas 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');
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts
% | 3|-------> actPos
% +-----------+
%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');
%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);
mot.ss.InputName{1}='iqCmd';
mot.ss.OutputName{1}='iqMeas';
mot.ss.OutputName{2}='iqVolts';
mot.ss.OutputName{3}='actPos';
%h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g'); %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); %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
tmp=tf(mot.ss);h=bodeplot(mot.meas,'r',tmp(3,1),'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'); setoptions(h,'FreqUnits','Hz','Grid','on');
end end
@@ -149,24 +188,22 @@ function [mot1,mot2]=identifyFxFyStage()
denc=myNorm(mot.mdl.denc); denc=myNorm(mot.mdl.denc);
num1=myNorm(mot.mdl.num1); num1=myNorm(mot.mdl.num1);
den1=myNorm(mot.mdl.den1); den1=myNorm(mot.mdl.den1);
num2=myNorm(mot.mdl.num2); num2=myNorm(mot.mdl.num2); %resonance
den2=myNorm(mot.mdl.den2); den2=myNorm(mot.mdl.den2);
num3=myNorm(mot.mdl.num3); num3=myNorm(mot.mdl.num3); %resonance
den3=myNorm(mot.mdl.den3); den3=myNorm(mot.mdl.den3);
num4=myNorm(mot.mdl.num4); num4=myNorm(mot.mdl.num4); %resonance
den4=myNorm(mot.mdl.den4); den4=myNorm(mot.mdl.den4);
num5=myNorm(mot.mdl.num5); num5=myNorm(mot.mdl.num5); %resonance
den5=myNorm(mot.mdl.den5); den5=myNorm(mot.mdl.den5);
num=myNorm(mot.mdl.num); %num=myNorm(mot.mdl.num);
den=myNorm(mot.mdl.den); %den=myNorm(mot.mdl.den);
g1=tf(numc,denc); % iqCmd->iqMeas g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1); 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) 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 %tf(s1) % display all transfer functions
num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1; 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]; den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0];
num=conv(num1,num2);%num=1;
den=conv(den1,den2);%den=[1 0 0];
g2=tf(num,den); %iqMeas->ActPos g2=tf(num,den); %iqMeas->ActPos
s2=ss(g2); s2=ss(g2);
@@ -177,21 +214,37 @@ function [mot1,mot2]=identifyFxFyStage()
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas mot.ssPlt=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
mot.ss.InputName{1}='iqCmd'; mot.ssPlt.InputName{1}='iqCmd';
mot.ss.OutputName{1}='iqMeas'; mot.ssPlt.OutputName{1}='iqMeas';
mot.ss.OutputName{2}='iqVolts'; mot.ssPlt.OutputName{2}='iqVolts';
mot.ss.OutputName{3}='actPos' ; mot.ssPlt.OutputName{3}='actPos' ;
chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts % | 2|-------> iqVolts
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
%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');
%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);
%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.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); %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
tmp=tf(mot.ss);h=bodeplot(mot.meas,'r',tmp(3,1),'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'); setoptions(h,'FreqUnits','Hz','Grid','on');
%controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited %controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited

BIN
matlab/observer.slx
View File

Binary file not shown.

6
python/MXTuning.py
View File

@@ -339,9 +339,9 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#tune.init_stage(); #tune.init_stage();
plt.close('all') plt.close('all')
#tune.custom_chirp() #tune.custom_chirp()
tune.custom_chirp(motor=1,minFrq=1,maxFrq=3000,tSec=5,mode=0,file='/tmp/cst_chirp0.npz') tune.custom_chirp(motor=1,minFrq=100,maxFrq=3000,amp=10,tSec=5,mode=0,file='/tmp/cst_chirp0.npz')
tune.custom_chirp(motor=2,minFrq=1,maxFrq=1000,tSec=5,mode=1,file='/tmp/cst_chirp1.npz') #tune.custom_chirp(motor=2,minFrq=1,maxFrq=1000,tSec=5,mode=1,file='/tmp/cst_chirp1.npz')
tune.custom_chirp(motor=1,minFrq=1,maxFrq=3000,tSec=5,mode=2,file='/tmp/cst_chirp2.npz') #tune.custom_chirp(motor=1,minFrq=1,maxFrq=3000,tSec=5,mode=2,file='/tmp/cst_chirp2.npz')
plt.show() plt.show()
#------------------ Main Code ---------------------------------- #------------------ Main Code ----------------------------------