167 lines
3.2 KiB
Matlab
167 lines
3.2 KiB
Matlab
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace
|
|
%zustandsregler:
|
|
% web(fullfile(docroot, 'simulink/examples.html'))
|
|
% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
|
|
% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
|
|
function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
|
|
sys=mot.ss;
|
|
sys=ss(sys.A,sys.B,sys.C(3,:),0); % $$$ only output position
|
|
|
|
%sys=ss(tf(mot1.mdl.num1,mot1.mdl.den1));
|
|
%A=sys.A;
|
|
%B=sys.B;
|
|
%C=sys.C;
|
|
%[A,B,C,D]=tf2ss(mot1.mdl.num1,mot1.mdl.den1)
|
|
%tf2ss(mot1.mdl.num1,mot1.mdl.den1)
|
|
|
|
figure();h=bodeplot(sys);
|
|
setoptions(h,'IOGrouping','all')
|
|
A=sys.A;
|
|
B=sys.B;
|
|
C=sys.C;
|
|
D=sys.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
|
|
|
|
|
|
t = 0:1E-4:.5;
|
|
u = ones(size(t)); %1000um
|
|
x0 = zeros(1,length(sys.A));
|
|
|
|
[y,t,x] = lsim(sys,u,t,x0);
|
|
figure();plot(t,y)
|
|
|
|
|
|
poles = eig(A);
|
|
|
|
if motid==1
|
|
p1=-3300+2800i;
|
|
p2=-1500+500i;
|
|
p3=-1200+10i;
|
|
P=[p1 p1' p2 p2' p3 p3'];
|
|
else
|
|
end
|
|
K = place(A,B,P);
|
|
%K = acker(A,B,P);
|
|
%K = acker(A,B,[p1 p1' p2 p2' p3 p3']);
|
|
%K = place(A,B,[p1 p1']);
|
|
%Nbar = rscale(sys,K);
|
|
%Nbar=1;
|
|
Nbar=-1./(C*(A-B*K)^-1*B); %from my notes)
|
|
%Nbar(2)=1; %the voltage stuff is crap for now
|
|
if length(Nbar)>1
|
|
Nbar=Nbar(3); % only the position scaling needed
|
|
end
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
[y,t,x]=lsim(sys_cl,Nbar*u,t,x0);
|
|
figure();plot(t,y)
|
|
|
|
%observer poles-> 5 times farther left than system poles
|
|
if motid==1
|
|
op1=(p1*5);
|
|
op2=(p2*5);
|
|
op3=(p3*5);
|
|
OP=[op1 op1' op2 op2' op3 op3'];
|
|
else
|
|
end
|
|
L=place(A',C',OP)';
|
|
|
|
At = [ A-B*K B*K
|
|
zeros(size(A)) A-L*C ];
|
|
Bt = [ B*Nbar
|
|
zeros(size(B)) ];
|
|
Ct = [ C zeros(size(C)) ];
|
|
sys = ss(At,Bt,Ct,0);
|
|
lsim(sys,ones(size(t)),t,[x0 x0]);
|
|
|
|
%https://www.youtube.com/watch?v=Lax3etc837U
|
|
Ao=A-L*C;
|
|
Bo=[B L];
|
|
Co=K;
|
|
Do=zeros(size(Co,1),size(Bo,2));
|
|
mdlName='observer';
|
|
open(mdlName)
|
|
end
|
|
|
|
function SCRATCH()
|
|
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
|
|
|
|
|
|
|