better observer

matlab/Readme.md

@@ -3,7 +3,7 @@ Matlab Simulink start
 ```
 [gfa-lc6-64 ~]
 [-bash INSTBASE=/prod]$
-cd /afs/psi.ch/user/z/zamofing_t/ESB-MX/
+cd ~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab
 module load matlab/2018a
 matlab&
 or start directly: /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab&
@@ -31,6 +31,29 @@ close all;
 [pb]=simFxFyStage(mot2);
 [ssc]=StateSpaceControlDesign(mot2);
 
+Very good trajectory-filter motor2:
+f=268;w0=f*2*pi;
+num1=[1 20 w0^2];
+den1=[1 500 w0^2];
+bode(numV,denV)
+
+
+f=140;w0=f*2*pi;
+num2=[1 300 w0^2];
+den2=[1 100 w0^2];
+bode(numV,denV)
+
+
+f=62;w0=f*2*pi;
+num3=[1 40 w0^2];
+den3=[1 20 w0^2];
+bode(numV,denV)
+
+
+numV=conv(num1,num2)
+denV=conv(den1,den2)
+%numV=conv(conv(num1,num2),num3)
+%denV=conv(conv(den1,den2),den3)
 
 ```
 

matlab/StateSpaceControlDesign.m

@@ -16,7 +16,7 @@ function [ssc]=StateSpaceControlDesign(mot)
   %
   % https://www.youtube.com/watch?v=Lax3etc837U
 
-  ssPlt=mot.ssMdl;%ssPlt; %real plant (model of real plant)
+  ssPlt=mot.ssPlt;%ssPlt; %real plant (model of real plant)
   ssPlt.Name='open loop plant';
   ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
   ssMdl.Name='open loop model';
@@ -63,7 +63,7 @@ function [ssc]=StateSpaceControlDesign(mot)
         p1=-6300+280i;
         p2=-6200+150i;   
         P=[p1 p1' p2 p2'];
-        P=[-4000 -4100 -1500+10j -1500-10j];
+        P=[-4100 -4000 -1500+10j -1500-10j];
       else
         p1=-3300+2800i;
         p2=-2700+500i;
@@ -80,6 +80,7 @@ function [ssc]=StateSpaceControlDesign(mot)
         p1=-6300+2800i;
         p2=-6200+1500i;   
         P=[p1 p1' p2 p2'];
+        P=[-2500 -2800 -1500+10j -1500-10j];
       else
         p1=-3300+2800i;
         p2=-1900+130i;
@@ -171,14 +172,16 @@ function [ssc]=StateSpaceControlDesign(mot)
   ss_oz = c2d(ss_o,Ts);
   [Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
   mdlName='observer';
-  open(mdlName);
-  
-
+  %open(mdlName);
   
+  %discrete prefilter
+  prefiltz=c2d(mot.prefilt,Ts);
+  numVz=prefiltz.Numerator{1};
+  denVz=prefiltz.Denominator{1};
  
   %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"]
+  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"]
     ssc=setfield(ssc,k,eval(k));
   end
   save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -187,6 +190,26 @@ 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 ];

matlab/simFxFyStage.m

@@ -18,13 +18,14 @@ function [pb]=simFxFyStage(mot)
   else
     %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
     %!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
-    Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
+    %Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
+    Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=3500;MaxInt=1000;
     mot_num=mot.tf_mdl.Numerator;
     mot_den=mot.tf_mdl.Denominator;
   end
 
   mdlName='stage_closed_loop';
-  open(mdlName)
+  %open(mdlName)
   %ServoDeltaTau_z(motid)
   [A,B,C,D]=tf2ss(mot_num,mot_den);
 
@@ -32,4 +33,5 @@ function [pb]=simFxFyStage(mot)
   pb=struct();
   for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr","A","B","C","D"]
     pb=setfield(pb,k,eval(k));
+  end
 end

matlab/testObserver.m

@@ -104,7 +104,7 @@ function [ssc]=testObserver(mot)
     p1=-63+2.80i;
     p2=-62+1.50i;   
     P=[p1 p1' p2 p2'];
-    P=[-4000 -4100 -500+10j -500-10j];
+    P=[-4000+100j -4000-100j -500+10j -500-10j];
     K = place(A,B,P);
     Ts=1/5000
     t = 0:Ts:2;