first rewrite of ovserver
This commit is contained in:
@@ -16,18 +16,12 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
% https://www.youtube.com/watch?v=Lax3etc837U
|
||||
|
||||
%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
|
||||
%0 ss_plt :real plant (model of real plant)
|
||||
%1 ss_c1 :current, mechanic, no resonance
|
||||
%2 ss_d1 :simpl. current, mechanic, no resonance
|
||||
%3 ss_1 :no current, mechanic, no resonance
|
||||
%4 ss_0 :no current, simpl. mechanic, no resonance
|
||||
|
||||
%mPrefilt:prefilter mode
|
||||
%0 no filter
|
||||
@@ -55,34 +49,38 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
|
||||
switch mPlt
|
||||
case 0
|
||||
ssPlt=mot.ssPlt;%real plant (model of real plant)
|
||||
ss_plt=mot.ss_plt;
|
||||
case 1
|
||||
ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance
|
||||
ss_plt=mot.ss_c1;
|
||||
case 2
|
||||
ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance
|
||||
ss_plt=mot.ss_d1;
|
||||
case 3
|
||||
ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance
|
||||
ss_plt=mot.ss_1;
|
||||
case 4
|
||||
ss_plt=mot.ss_0;
|
||||
end
|
||||
ssPlt.Name='open loop plant';
|
||||
ss_plt.Name='open loop plant';
|
||||
|
||||
switch mMdl
|
||||
case 0
|
||||
ssMdl=mot.ssPlt;%real plant (model of real plant)
|
||||
ss_mdl=mot.ss_plt;
|
||||
case 1
|
||||
ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance
|
||||
ss_mdl=mot.ss_c1;
|
||||
case 2
|
||||
ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance
|
||||
ss_mdl=mot.ss_d1;
|
||||
case 3
|
||||
ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance
|
||||
ss_mdl=mot.ss_1;
|
||||
case 4
|
||||
ss_mdl=mot.ss_0;
|
||||
end
|
||||
ssMdl.Name='open loop model'; %model for observer
|
||||
ss_mdl.Name='open loop model'; %model for observer
|
||||
|
||||
|
||||
[Ap,Bp,Cp,Dp]=ssdata(ssPlt);
|
||||
[Am,Bm,Cm,Dm]=ssdata(ssMdl);
|
||||
[Ap,Bp,Cp,Dp]=ssdata(ss_plt);
|
||||
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
|
||||
|
||||
if bitand(mShow,1)
|
||||
figure();h=bodeplot(ssPlt,ssMdl);
|
||||
figure();h=bodeplot(ss_plt,ss_mdl);
|
||||
setoptions(h,'IOGrouping','all')
|
||||
end
|
||||
|
||||
@@ -93,8 +91,8 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
% 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);
|
||||
[yp,t,x] = lsim(ss_plt,u,t,xp0);
|
||||
[ym,t,x] = lsim(ss_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')
|
||||
end
|
||||
@@ -108,9 +106,9 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
%
|
||||
%place poles for the controller feedback
|
||||
if use_lqr %use the lqr controller
|
||||
Q=eye(length(ssMdl.A));
|
||||
Q=eye(length(ss_mdl.A));
|
||||
R=1;
|
||||
[K,P,E]=lqr(ssMdl,Q,R,0);
|
||||
[K,P,E]=lqr(ss_mdl,Q,R,0);
|
||||
else
|
||||
if mot.id==1
|
||||
%2500rad/s = 397Hz -> locate poles here
|
||||
@@ -155,12 +153,11 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
end %if lqr
|
||||
|
||||
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
|
||||
|
||||
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.OutputName);
|
||||
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
|
||||
if bitand(mShow,4)
|
||||
% step answer on closed loop with space state controller:
|
||||
t = 0:1E-4:.5;
|
||||
@@ -182,7 +179,7 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
Ct = [ Cm zeros(size(Cm)) ];
|
||||
|
||||
Dt=0;
|
||||
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName);
|
||||
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.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');
|
||||
@@ -233,8 +230,8 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
end
|
||||
|
||||
%discrete plant
|
||||
ssPltz = c2d(ssPlt,Ts);
|
||||
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
|
||||
%ss_pltz = c2d(ss_plt,Ts);
|
||||
%[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
|
||||
|
||||
%discrete observer controller
|
||||
ss_oz = c2d(ss_o,Ts);
|
||||
@@ -261,7 +258,9 @@ function [ssc]=StateSpaceControlDesign(mot)
|
||||
|
||||
%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"]
|
||||
%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"]
|
||||
%for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
|
||||
for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
|
||||
ssc=setfield(ssc,k,eval(k));
|
||||
end
|
||||
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
|
||||
@@ -302,93 +301,3 @@ function pf=Prefilt(mot,mode)
|
||||
end
|
||||
|
||||
|
||||
%code snipplets from an example on youtube (see reference at top)
|
||||
function SCRATCH()
|
||||
%import numpy as np
|
||||
%fh=np.load('mode1.npz')
|
||||
%import scipy.io
|
||||
%scipy.io.savemat('mode1.mat',fh,do_compression=True)
|
||||
|
||||
%matlab:
|
||||
load('mode1.mat');
|
||||
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 ];
|
||||
|
||||
B = [ 0
|
||||
0
|
||||
100 ];
|
||||
|
||||
C = [ 1 0 0 ];
|
||||
|
||||
poles = eig(A)
|
||||
|
||||
t = 0:0.01:2;
|
||||
u = zeros(size(t));
|
||||
x0 = [0.01 0 0];
|
||||
|
||||
sys = ss(A,B,C,0);
|
||||
|
||||
[y,t,x] = lsim(sys,u,t,x0);
|
||||
plot(t,y)
|
||||
title('Open-Loop Response to Non-Zero Initial Condition')
|
||||
xlabel('Time (sec)')
|
||||
ylabel('Ball Position (m)')
|
||||
|
||||
p1 = -10 + 10i;
|
||||
p2 = -10 - 10i;
|
||||
p3 = -50;
|
||||
|
||||
K = place(A,B,[p1 p2 p3]);
|
||||
sys_cl = ss(A-B*K,B,C,0);
|
||||
|
||||
lsim(sys_cl,u,t,x0);
|
||||
xlabel('Time (sec)')
|
||||
ylabel('Ball Position (m)')
|
||||
|
||||
p1 = -20 + 20i;
|
||||
p2 = -20 - 20i;
|
||||
p3 = -100;
|
||||
|
||||
K = place(A,B,[p1 p2 p3]);
|
||||
sys_cl = ss(A-B*K,B,C,0);
|
||||
|
||||
lsim(sys_cl,u,t,x0);
|
||||
xlabel('Time (sec)')
|
||||
ylabel('Ball Position (m)')
|
||||
|
||||
|
||||
t = 0:0.01:2;
|
||||
u = 0.001*ones(size(t));
|
||||
|
||||
sys_cl = ss(A-B*K,B,C,0);
|
||||
|
||||
lsim(sys_cl,u,t);
|
||||
xlabel('Time (sec)')
|
||||
ylabel('Ball Position (m)')
|
||||
axis([0 2 -4E-6 0])
|
||||
|
||||
Nbar = rscale(sys,K)
|
||||
|
||||
lsim(sys_cl,Nbar*u,t)
|
||||
title('Linear Simulation Results (with Nbar)')
|
||||
xlabel('Time (sec)')
|
||||
ylabel('Ball Position (m)')
|
||||
axis([0 2 0 1.2*10^-3])
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user