towards matlab

2018-10-10 17:15:36 +02:00
parent 5543582557
commit f9ddf04fdc
11 changed files with 640 additions and 91 deletions
--- a/matlab/StateSpaceControlDesign.m
+++ b/matlab/StateSpaceControlDesign.m
@@ -0,0 +1,166 @@
+%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=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
+
+
+