257 lines
8.9 KiB
Matlab
257 lines
8.9 KiB
Matlab
|
|
function [mot1,mot2]=identifyFxFyStage()
|
|
%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|-------> iqVolts
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
%
|
|
% the returned motor objects mot1 and mot2 contains:
|
|
%
|
|
% w,mag,phase : (gathered data with Python)
|
|
% 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)
|
|
%
|
|
% 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/ )
|
|
% - curr_step[1|2].mat
|
|
% - full_bode_mot[1|2].mat
|
|
% - model[1|2].mat
|
|
|
|
|
|
%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
|
|
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);
|
|
obj.meas= idfrd(response,obj.w,0);
|
|
|
|
fMdl=load(strcat(path,sprintf('model%d.mat',motid)));
|
|
obj.mdl=fMdl;
|
|
end
|
|
|
|
function tfc=currstep(obj)
|
|
opt=tfestOptions;
|
|
opt.Display='off';
|
|
tfc = tfest(obj.currstep, 2, 0,opt);
|
|
s=str2ndOrd(tfc);
|
|
t=(0:199)*50E-6;
|
|
[y,t]=step(tfc,t);
|
|
figure();
|
|
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');
|
|
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.']);
|
|
end
|
|
|
|
function mot=fyStage()
|
|
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
|
|
mot.tfc=currstep(mot);
|
|
|
|
opt=tfestOptions;
|
|
opt.Display='off';
|
|
opt.initializeMethod='iv';
|
|
opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion
|
|
|
|
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
|
|
|
|
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');
|
|
% 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);
|
|
|
|
%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))
|
|
end
|
|
|
|
function mot=fxStage()
|
|
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2);
|
|
currstep(mot);
|
|
|
|
opt=tfestOptions;
|
|
opt.Display='off';
|
|
opt.initializeMethod='iv';
|
|
opt.WeightingFilter=[1,4;10,670]*(2*pi); % Hz->rad/s conversion
|
|
|
|
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);
|
|
|
|
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];
|
|
|
|
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);
|
|
|
|
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');
|
|
% 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=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.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
|
|
|
|
end
|
|
close all
|
|
mot1=fyStage();
|
|
mot2=fxStage();
|
|
|
|
end
|