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
ss_ol=mot.ss;
ss_ol.Name='open loop';
%ss_ol=mot.ssPlt;
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
figure();h=bodeplot(ss_ol);
figure();h=bodeplot(ss_ol_plt,ss_ol_mdl);
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:
t = 0:1E-4:.5;
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);
figure();plot(t,y);title('step on open loop');
[yp,t,x] = lsim(ss_ol_plt,u,t,xp0);
[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);
w0=abs(poles);
ang=angle(-poles);
poles = eig(Am);
%w0=abs(poles);
%ang=angle(-poles);
%-------------------
%p=w0.*exp(j.*ang)
@@ -61,38 +53,57 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%place poles for the controller feedback
if motid==1
%2500rad/s = 397Hz -> locate poles here
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'];
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
p1=-6300+2800i;
p2=-6200+1500i;
P=[p1 p1' p2 p2'];
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'];
end
else
%2500rad/s = 397Hz -> locate poles here
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'];
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
p1=-6300+2800i;
p2=-6200+1500i;
P=[p1 p1' p2 p2'];
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'];
end
end
K = place(A,B,P);
%K = acker(A,B,P);
V=-1./(C*(A-B*K)^-1*B); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
K = place(Am,Bm,P);
%K = acker(Am,Bm,Pm);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
%Nbar(2)=1; %the voltage stuff is crap for now
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(A-B*K,B*V,C,0,'Name','space state controller','InputName',mot.ss.InputName,'OutputName',mot.ss.OutputName);
[y,t,x]=lsim(ss_cl,V*u,t,x0);
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');
@@ -102,29 +113,38 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
if motid==1
op1=(p1*5);
op2=(p2*5);
op3=(p3*5);
OP=[op1 op1' op2 op2' op3 op3'];
if length(poles)>4
op3=(p3*5);
OP=[op1 op1' op2 op2' op3 op3'];
else
OP=[op1 op1' op2 op2'];
end
else
op1=(p1*2);
op2=(p2*2);
op3=(p3*2);
op4=(p4*2);
op5=(p5*2);
op6=(p6*2);
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6'];
if length(poles)>4
op3=(p3*2);
op4=(p4*2);
op5=(p5*2);
op6=(p6*2);
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6'];
else
OP=[op1 op1' op2 op2'];
end
end
L=place(A',C',OP)';
L=place(Am',Cm',OP)';
%L=acker(A',C',OP)';
At = [ A-B*K B*K
zeros(size(A)) A-L*C ];
Bt = [ B*V
zeros(size(B)) ];
Ct = [ C zeros(size(C)) ];
At = [ Am-Bm*K Bm*K
zeros(size(Am)) Am-L*Cm ];
Bt = [ Bm*V
zeros(size(Bm)) ];
Ct = [ Cm zeros(size(Cm)) ];
% step answer on closed loop with observer controller:
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ss.OutputName);
figure();lsim(ss_o,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer');
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
figure();lsim(ss_o,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
% *** disctrete observer controller ***
@@ -134,7 +154,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
ss_od .Name='discrete obsvr ctrl';
% step answer on closed loop with disctrete observer controller:
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
@@ -148,8 +168,8 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%calculate matrices for the simulink system
Ao=A-L*C;
Bo=[B L];
Ao=Am-L*Cm;
Bo=[Bm L];
Co=K;
Do=zeros(size(Co,1),size(Bo,2));
mdlName='observer';
@@ -157,7 +177,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
%state space controller
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));
end