work on observer

matlab/testObserver.m

@@ -0,0 +1,143 @@
+function [ssc]=testObserver(mot)
+  %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
+
+  A = [ 0   1   0
+       980  0  -2.8
+        0   0  -100 ];
+
+  B = [ 0
+        0
+        100 ];
+
+  C = [ 1 0 0 ];
+  D=0;
+
+  %[A,B,C,D]=ssdata(mot.ssMdl)
+  %C=C(3,:);D=D(3);
+  
+  Ap=A;Am=A;
+  Bp=B;Bm=B;
+  Cp=C;Cm=C;
+  Dp=D;Dm=D;Dt=D;
+
+  poles = eig(A);
+  disp(poles);
+
+  
+  t = 0:0.01:2;
+  u = zeros(size(t));
+  x0 = [0.01 0 0];
+  %x0 = [0.1 0 0 0];
+
+  sys = ss(A,B,C,D);
+
+  [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])
+
+  V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+  lsim(sys_cl,V*u,t)
+  title('Linear Simulation Results (with Nbar)')
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+  axis([0 2 0 1.2*10^-3])  
+  op1 = -100;
+  op2 = -101;
+  op3 = -102;
+  
+  L = place(A',C',[op1 op2 op3])';
+  
+  At = [ A-B*K             B*K
+         zeros(size(A))    A-L*C ];
+  Bt = [    B*V
+         zeros(size(B)) ];
+  Ct = [ C    zeros(size(C)) ];
+
+  sys = ss(At,Bt,Ct,0);
+  lsim(sys,zeros(size(t)),t,[x0 x0]);
+
+  title('Linear Simulation Results (with observer)')
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+  
+  
+  t = 0:1E-6:0.1;
+  x0 = [0.01 0.5 -5];
+  [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]);
+
+  n = 3;
+  e = x(:,n+1:end);
+  x = x(:,1:n);
+  x_est = x - e;
+
+  % Save state variables explicitly to aid in plotting
+  h = x(:,1); h_dot = x(:,2); i = x(:,3);
+  h_est = x_est(:,1); h_dot_est = x_est(:,2); i_est = x_est(:,3);
+
+  plot(t,h,'-r',t,h_est,':r',t,h_dot,'-b',t,h_dot_est,':b',t,i,'-g',t,i_est,':g')
+  legend('h','h_{est}','hdot','hdot_{est}','i','i_{est}')
+  xlabel('Time (sec)')
+
+  
+  
+  %discrete
+  Ts=1/50; % deltatau std. frq. is 5kHz
+  %The discrete system works with sampling >100Hz,. the bigger the frequency the better the result
+  %With sampling = 80Hz hte system already becomes instable.
+  ss_t=ss(At,Bt,Ct,Dt);
+  figure();pzplot(ss_t)
+  ss_tz=c2d(ss_t,Ts);
+  figure();pzplot(ss_tz)
+  [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz);
+
+  
+  [Apz,Bpz,Cpz,Dpz]=ssdata(c2d(ss(Ap,Bp,Cp,Dp),Ts));
+  [Amz,Bmz,Cmz,Dmz]=ssdata(c2d(ss(Am,Bm,Cm,Dm),Ts));
+  
+
+  Ao=Am-L*Cm;
+  Bo=[Bm L];
+  Co=K;
+  Do=zeros(size(Co,1),size(Bo,2));
+  
+  [Aoz,Boz,Coz,Doz]=ssdata(c2d(ss(Ao,Bo,Co,Do),Ts));
+  
+  
+  %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"]
+    ssc=setfield(ssc,k,eval(k));
+  end
+  mdlName='testObserverSim';
+  open(mdlName);
+  
+  
+  
+end