could be first stable observer of motor1

matlab/StateSpaceControlDesign.m

@@ -63,6 +63,7 @@ function [ssc]=StateSpaceControlDesign(mot)
         p1=-6300+280i;
         p2=-6200+150i;   
         P=[p1 p1' p2 p2'];
+        P=[-4000 -4100 -1500+10j -1500-10j];
       else
         p1=-3300+2800i;
         p2=-2700+500i;

matlab/testObserver.m

@@ -1,6 +1,7 @@
 function [ssc]=testObserver(mot)
   %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
-
+  mdl=1; % 0:hovering ball  1:fast stage
+  if mdl==0
     A = [ 0   1   0
          980  0  -2.8
           0   0  -100 ];
@@ -11,9 +12,39 @@ function [ssc]=testObserver(mot)
 
     C = [ 1 0 0 ];
     D=0;
+  else
+    [A,B,C,D]=ssdata(mot.ssMdl);
+    C=C(3,:);D=D(3);    
+    numc=(mot.mdl.numc);
+    denc=(mot.mdl.denc);
+    num1=(mot.mdl.num1);
+    den1=(mot.mdl.den1);
 
-  %[A,B,C,D]=ssdata(mot.ssMdl)
+    g1=tf(numc,denc);  % iqCmd->iqMeas
-  %C=C(3,:);D=D(3);
+    s1=ss(g1);
+    s1.C=[s1.C; 1E5* 2.4E-3 1E-3*s1.C(2)*8.8];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
+    g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies
+    s2=ss(g2);
+    s3=append(s1,s2);
+    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);    
+    
+    %d=.77;w0=9100; %1.44kHz -> T0=6.9046e-04
+    %numc=[w0^2];
+    %denc=[1 d*2*w0 w0^2];
+    %T0=6E-4;  %w=1/1E-3=1E3 -> f=w/(2pi)=1000/6.2=159Hz
+    %numc=[1];
+    %denc=[T0^2 d*2*T0 1];
+    %bode(numc,denc)
+    
+    num=conv(numc,num1);%num=1;
+    den=conv(denc,den1);%den=[1 0 0];
+    g4=tf(num,den); %iqMeas->ActPos
+    s4=ss(g4);  
+    mot.ssMdl
+    s3
+    s4
+    [A,B,C,D]=ssdata(s4);
+  end
   
   Ap=A;Am=A;
   Bp=B;Bm=B;
@@ -24,10 +55,10 @@ function [ssc]=testObserver(mot)
   disp(poles);
 
   
+  if mdl==0
     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);
 
@@ -45,13 +76,13 @@ function [ssc]=testObserver(mot)
     ylabel('Ball Position (m)')
 
     p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100;
-  K = place(A,B,[p1 p2 p3]);
+    P=[p1 p2 p3];
+    K = place(A,B,P);
     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));
 
@@ -68,12 +99,32 @@ function [ssc]=testObserver(mot)
     xlabel('Time (sec)')
     ylabel('Ball Position (m)')
     axis([0 2 0 1.2*10^-3])     
+  else
+    x0 = [0 0 0 0];
+    p1=-63+2.80i;
+    p2=-62+1.50i;   
+    P=[p1 p1' p2 p2'];
+    P=[-4000 -4100 -500+10j -500-10j];
+    K = place(A,B,P);
+    Ts=1/5000
+    t = 0:Ts:2;
+    u = 0.001*ones(size(t));
+    V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+    sys_cl = ss(A-B*K,B*V,C,0);
+
+    lsim(sys_cl,u,t,x0);
+    xlabel('Time (sec)')
+    ylabel('Motor Position (m)')
+    axis([0 2 0 1.2E-3])
+  end
+
+  if mdl==0
     op1 = -100;
     op2 = -101;
     op3 = -102;
-  
+    OP=[op1,op2,op3];
-  L = place(A',C',[op1 op2 op3])';
+    OP=P*2;
-  
+    L = place(A',C',OP)';
     At = [ A-B*K             B*K
            zeros(size(A))    A-L*C ];
     Bt = [    B*V
@@ -81,13 +132,29 @@ function [ssc]=testObserver(mot)
     Ct = [ C    zeros(size(C)) ];
 
     sys = ss(At,Bt,Ct,0);
-  lsim(sys,zeros(size(t)),t,[x0 x0]);
+    %lsim(sys,zeros(size(t)),t,[x0 x0]);
-
+    lsim(sys,u,t,[x0 x0]*0);
     title('Linear Simulation Results (with observer)')
     xlabel('Time (sec)')
     ylabel('Ball Position (m)')
+  else
+    OP=P*2;
+    L = place(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)) ];
 
+    sys = ss(At,Bt,Ct,0);
+    lsim(sys,u,t,[x0 x0]*0);
+    title('Linear Simulation Results (with observer)')
+    xlabel('Time (sec)')
+    ylabel('Motor Position (m)')
+    axis([0 2 0 1.2E-3])
+  end  
   
+  if mdl==0
     t = 0:1E-6:0.1;
     x0 = [0.01 0.5 -5];
     [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]);
@@ -104,17 +171,17 @@ function [ssc]=testObserver(mot)
     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)')
-
+  end
   
   
   %discrete
-  Ts=1/50; % deltatau std. frq. is 5kHz
+  Ts=1/5000; % 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)
+  %figure();pzplot(ss_t)
   ss_tz=c2d(ss_t,Ts);
-  figure();pzplot(ss_tz)
+  %figure();pzplot(ss_tz)
   [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz);