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
2018-11-19 15:54:16 +01:00
parent 7e867b9316
commit 0a5ec9b004
22 changed files with 76507 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
matlab/Readme.md
View File

@@ -26,35 +26,16 @@ clear;
clear global;
close all;
[mot1,mot2]=identifyFxFyStage();
[pb]=simFxFyStage(mot1);
[ssc]=StateSpaceControlDesign(mot1);
[pb]=simFxFyStage(mot2);
[ssc]=StateSpaceControlDesign(mot2);
Very good trajectory-filter motor2:
f=268;w0=f*2*pi;
num1=[1 20 w0^2];
den1=[1 500 w0^2];
bode(numV,denV)
[pb]=simFxFyStage(mot1);sim('stage_closed_loop');
close all;[ssc]=StateSpaceControlDesign(mot1);sim('observer');
[pb]=simFxFyStage(mot2);sim('stage_closed_loop');
close all;[ssc]=StateSpaceControlDesign(mot2);sim('observer');
f=140;w0=f*2*pi;
num2=[1 300 w0^2];
den2=[1 100 w0^2];
bode(numV,denV)
f=62;w0=f*2*pi;
num3=[1 40 w0^2];
den3=[1 20 w0^2];
bode(numV,denV)
numV=conv(num1,num2)
denV=conv(den1,den2)
%numV=conv(conv(num1,num2),num3)
%denV=conv(conv(den1,den2),den3)
mögliche tf:
iqCmd---->iqMeas
iqVolts-->iqMeas
iqMeas--->actPos
```

290
matlab/StateSpaceControlDesign.m
View File

@@ -1,11 +1,10 @@
function [ssc]=StateSpaceControlDesign(mot)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
%
% builds a state space controller designed for the plant.
% shows step answers of open and closed loop, also for the observer controller and the final discrete observer
%
% finally it opens a simulink observer file for testing
% the matchich simulink model is: 'observer'
%References:
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
@@ -16,31 +15,89 @@ function [ssc]=StateSpaceControlDesign(mot)
%
% https://www.youtube.com/watch?v=Lax3etc837U
ssPlt=mot.ssPlt;%ssPlt; %real plant (model of real plant)
%mPlt: mode to select plant
%0 real plant (model of real plant)
%1 current, mechanic, no resonance
%2 no current current, mechanic, no resonance
%3 no current current, mechanic, first resonance
%mMdl: mode to select model for observer
%0 real plant (NOT RECOMANDED, because not observab,econtrolable)
%1 current, mechanic, no resonance
%2 no current current, mechanic, no resonance
%3 no current current, mechanic, first resonance
%mPrefilt:prefilter mode
%0 no filter
%1 inverse resonance filter
%2 manual setup filter
%mShow: mode(bits) to plot/simulate
% 0: 1: bode plots of open loop
% 1: 2: step answer on open loop
% 2: 4: step answer on closed loop with space state controller
% 3: 8: step answer on closed loop with observer controller
% 4:16: step answer on closed loop with disctrete observer controller
% 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
% 6:64:
%use_lqr: use lqr instead of pole placement
mPlt=0;
mMdl=1;
mPrefilt=2;
mShow=32+64;
use_lqr=0;
switch mPlt
case 0
ssPlt=mot.ssPlt;%real plant (model of real plant)
case 1
ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance
case 2
ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance
case 3
ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance
end
ssPlt.Name='open loop plant';
ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
ssMdl.Name='open loop model';
switch mMdl
case 0
ssMdl=mot.ssPlt;%real plant (model of real plant)
case 1
ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance
case 2
ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance
case 3
ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance
end
ssMdl.Name='open loop model'; %model for observer
[Ap,Bp,Cp,Dp]=ssdata(ssPlt);
[Am,Bm,Cm,Dm]=ssdata(ssMdl);
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
if bitand(mShow,1)
figure();h=bodeplot(ssPlt,ssMdl);
setoptions(h,'IOGrouping','all')
end
figure();h=bodeplot(ssPlt,ssMdl);
setoptions(h,'IOGrouping','all')
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
xp0 = zeros(1,length(Ap));
xm0 = zeros(1,length(Am));
[yp,t,x] = lsim(ssPlt,u,t,xp0);
[ym,t,x] = lsim(ssMdl,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')
if bitand(mShow,2)
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
[yp,t,x] = lsim(ssPlt,u,t,xp0);
[ym,t,x] = lsim(ssMdl,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')
end
poles = eig(Am);
%w0=abs(poles);
%ang=angle(-poles);
@@ -50,7 +107,6 @@ function [ssc]=StateSpaceControlDesign(mot)
% *** space state controller ***
%
%place poles for the controller feedback
use_lqr=0;
if use_lqr %use the lqr controller
Q=eye(length(ssMdl.A));
R=1;
@@ -59,42 +115,41 @@ function [ssc]=StateSpaceControlDesign(mot)
if mot.id==1
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
p1=-6300+280i;
p2=-6200+150i;
P=[p1 p1' p2 p2'];
P=[-4100 -4000 -1500+10j -1500-10j];
else
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'];
switch mMdl
case 0
p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i;
P=[p1 p1' p2 p2' p3 p3'];
case 1
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-4100 -4000 -1500+10j -1500-10j];
case 2
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j];
case 3
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j -1400 -1300];
end
else
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
p1=-6300+2800i;
p2=-6200+1500i;
P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
else
p1=-3300+2800i;
p2=-1900+130i;
p3=-2900+80i;
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'];
switch mMdl
case 0
p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i;
p4=-2300+450i; p5=-2000+20i; p6=-1500+10i;
P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
case 1
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
case 2
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
end
end
%P=P*.1; % P was too aggressive
K = place(Am,Bm,P);
%K = acker(Am,Bm,Pm);
end %if lqr
@@ -104,19 +159,15 @@ function [ssc]=StateSpaceControlDesign(mot)
if length(V)>1
V=V(3); % only the position scaling needed
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:
t = 0:1E-4:.5;
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,xm0);
figure();plot(t,y);title('step on closed loop');
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.OutputName);
if bitand(mShow,4)
% step answer on closed loop with space state controller:
t = 0:1E-4:.5;
[y,t,x]=lsim(ss_cl,V*u,t,xm0);
figure();plot(t,y);title('step on closed loop');
end
% *** observer controller ***
%
%observer poles-> 5 times farther left than system poles
@@ -131,54 +182,83 @@ function [ssc]=StateSpaceControlDesign(mot)
Ct = [ Cm zeros(size(Cm)) ];
Dt=0;
% step answer on closed loop with observer controller:
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName);
if bitand(mShow,8)
% step answer on closed loop with observer controller:
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
end
% *** disctrete observer controller ***
%
Ts=1/5000; % 5kHz
ss_tz = c2d(ss_t,Ts);
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
ss_tz.Name='discrete obsvr ctrl';
% step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05;
figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
%plot all bode diagrams of desPos->actPos
figure();
h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
if bitand(mShow,16)
% step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05;
figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
end
figure();
h=pzplot(ss_cl(3),ss_t(3),ss_tz(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
if bitand(mShow,32)
%plot all bode diagrams of desPos->actPos
figure();
if mMdl==2 || mMdl==3
idx=1;
else
idx=3;
end
h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
figure();
h=pzplot(ss_cl(idx),ss_t(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
figure();
h=pzplot(ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%calculate matrices for the simulink system
Ao=Am-L*Cm;
Bo=[Bm L];
Co=K;
Do=zeros(size(Co,1),size(Bo,2));
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
if mMdl==2 || mMdl==3
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
else
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
end
%discrete plant
ssPltz = c2d(ssPlt,Ts);
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
%discrete observer controller
%discrete observer controller
ss_oz = c2d(ss_o,Ts);
[Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
mdlName='observer';
%mdlName='observer';
%open(mdlName);
%prefilter to compensate non observable resonance frequencies
prefilt=Prefilt(mot,mPrefilt);
numV=prefilt.Numerator{1};
denV=prefilt.Denominator{1};
%discrete prefilter
prefiltz=c2d(mot.prefilt,Ts);
prefiltz=c2d(prefilt,Ts);
numVz=prefiltz.Numerator{1};
denVz=prefiltz.Denominator{1};
if bitand(mShow,64)
h=bodeplot(prefilt,prefiltz);
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%state space controller
ssc=struct();
for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
@@ -187,6 +267,40 @@ function [ssc]=StateSpaceControlDesign(mot)
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
end
function pf=Prefilt(mot,mode)
switch mode
case 0 %no filter
pf=tf(1,1);
case 1 %inverse resonance
if mot.id==1
den=mot.mdl.num2;%num=1;
num=mot.mdl.den2;%den=[1 0 0];
pf=tf(num,den);
else
den=conv(conv(conv(mot.mdl.num2,mot.mdl.num3),mot.mdl.num4),mot.mdl.num5);%num=1;
num=conv(conv(conv(mot.mdl.den2,mot.mdl.den3),mot.mdl.den4),mot.mdl.den5);%den=[1 0 0];
pf=tf(num,den);
end
case 2
if mot.id==1
f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2];
numV=num1;
denV=den1;
pf=tf(numV,denV);
else
f=277;w0=f*2*pi; num1=[1 20 w0^2]; den1=[1 500 w0^2];
f=138;w0=f*2*pi; num2=[1 300 w0^2]; den2=[1 100 w0^2];
f=60;w0=f*2*pi; num3=[1 33 w0^2]; den3=[1 20 w0^2];
numV=conv(num1,num2);
denV=conv(den1,den2);
numV=conv(conv(num1,num2),num3);
denV=conv(conv(den1,den2),den3) ;
pf=tf(numV,denV);
end
end
%controlSystemDesigner('bode',1,pf); % <<<<<<<<< This opens a transferfunction that can be edited
end
%code snipplets from an example on youtube (see reference at top)
function SCRATCH()
@@ -200,16 +314,16 @@ function SCRATCH()
plot(pts(:,1),pts(:,2),'.');hold on;
plot(rec(:,5),rec(:,6),'-');%despos
plot(rec(:,2),rec(:,3),'-');%actPos
%sig.time = [0 1 1 5 5 8 8 10];
%sig.signals.values = [0 0 2 2 2 3 3 3]';
%sig.signals.dimensions = 1;
sig.time=0:2E-4:(length(rec)-1)*2E-4;
sig.signals.values=rec(:,5);
sig.signals.dimensions = 1;
sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2))
A = [ 0 1 0
980 0 -2.8
0 0 -100 ];

39
matlab/documentFunctions.m Normal file
View File

@@ -0,0 +1,39 @@
baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuningDoc/';
%plots bode of desPos_actPos
[mot1,mot2]=identifyFxFyStage();
close all;
[pb]=simFxFyStage(mot2);sim('stage_closed_loop');
t=desPos_actPos.Time;
u=desPos_actPos.Data(:,1);
y=desPos_actPos.Data(:,2);
figure(1);
clf;
plot(t,u,'b');hold on;
plot(t,y,'color',[0 .5 0]);
grid on;ylim([-4 4]);
grid on;ylim([-2 2]);
saveas(gca, baseDir+"m2_sim_pb.eps",'epsc');
saveas(gca, baseDir+"m1_sim_ss.eps",'epsc');
saveas(gca, baseDir+"m2_sim_ss.eps",'epsc');
saveas(gca, baseDir+"m1_sim_ss_pref.eps",'epsc');
saveas(gca, baseDir+"m2_sim_ss_pref.eps",'epsc');
saveas(gca, baseDir+"m2_mdl_bode.eps",'epsc');
minFrq=1;maxFrq=1000;
tSec=t(length(t));
f=(1:length(t))/tSec;
fu=fft(u);
fy=fft(y);
ph=phase(fy./fu)
mag=abs(fy)./abs(fu);
magDb=20*log10(mag);
figure(2);
subplot(2,1,1);
semilogx(f,magDb);
subplot(2,1,2);
semilogx(f,ph);
grid on;

238
matlab/identifyFxFyStage.m
View File

@@ -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

BIN
matlab/observer.slx
View File

Binary file not shown.

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.

4
matlab/testObserver.m
View File

@@ -4,11 +4,11 @@ function [ssc]=testObserver(mot)
if mdl==0
A = [ 0 1 0
980 0 -2.8
100
0 0 -100 ];
B = [ 0
0
100 ];
0];
C = [ 1 0 0 ];
D=0;