wip

Makefile

@@ -4,7 +4,7 @@ BUILDCLASSES = Linux
 ARCH_FILTER = eldk42% SL6-x86_64
 EXCLUDE_VERSIONS = 3.14.8
 #SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.py cfg/*.pbi python/*.py)
-SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.pbi)
+SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.py cfg/*.pbi)
 #SOURCES+=src/DHVSaSub.cpp
 #DBDS+=src/DHVSaSub.dbd
 USR_CXXFLAGS+= -fno-operator-names

matlab/Readme.md

@@ -26,35 +26,16 @@ clear;
 clear global;
 close all;
 [mot1,mot2]=identifyFxFyStage();
-[pb]=simFxFyStage(mot1);
-[ssc]=StateSpaceControlDesign(mot1);
-[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)
+[pb]=simFxFyStage(mot1);sim('stage_closed_loop');
+close all;[ssc]=StateSpaceControlDesign(mot1);sim('observer');
+[pb]=simFxFyStage(mot2);sim('stage_closed_loop');
+close all;[ssc]=StateSpaceControlDesign(mot2);sim('observer');
 
 
-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)
-
+mögliche tf:
+iqCmd---->iqMeas
+iqVolts-->iqMeas
+iqMeas--->actPos
 ```
 
 

matlab/StateSpaceControlDesign.m

@@ -1,11 +1,10 @@
 function [ssc]=StateSpaceControlDesign(mot)
-  % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
+  % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
   %
   % builds a state space controller designed for the plant.
   % shows step answers of open and closed loop, also for the observer controller and the final discrete observer
   %
-  % finally it opens a simulink observer file for testing
-
+  % the matchich simulink model is: 'observer'
 
   %References:
   %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
@@ -16,31 +15,89 @@ function [ssc]=StateSpaceControlDesign(mot)
   %
   % https://www.youtube.com/watch?v=Lax3etc837U
 
-  ssPlt=mot.ssPlt;%ssPlt; %real plant (model of real plant)
+  %mPlt: mode to select plant
+  %0 real plant (model of real plant)
+  %1 current, mechanic, no resonance
+  %2 no current current, mechanic, no resonance
+  %3 no current current, mechanic, first resonance
+
+
+
+  %mMdl: mode to select model for observer
+  %0 real plant (NOT RECOMANDED, because not observab,econtrolable)
+  %1 current, mechanic, no resonance
+  %2 no current current, mechanic, no resonance
+  %3 no current current, mechanic, first resonance
+
+  %mPrefilt:prefilter mode
+  %0 no filter
+  %1 inverse resonance filter
+  %2 manual setup filter
+
+
+  %mShow: mode(bits) to plot/simulate
+  % 0: 1: bode plots of open loop
+  % 1: 2: step answer on open loop
+  % 2: 4: step answer on closed loop with space state controller
+  % 3: 8: step answer on closed loop with observer controller
+  % 4:16: step answer on closed loop with disctrete observer controller
+  % 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
+  % 6:64:
+
+
+  %use_lqr: use lqr instead of pole placement
+
+  mPlt=0;
+  mMdl=1;
+  mPrefilt=2;
+  mShow=32+64;
+  use_lqr=0;
+
+  switch mPlt
+    case 0
+      ssPlt=mot.ssPlt;%real plant (model of real plant)
+    case 1
+      ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance
+    case 2
+      ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance
+    case 3
+      ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance
+  end
   ssPlt.Name='open loop plant';
-  ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
-  ssMdl.Name='open loop model';
+
+  switch mMdl
+    case 0
+      ssMdl=mot.ssPlt;%real plant (model of real plant)
+    case 1
+      ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance
+    case 2
+      ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance
+    case 3
+      ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance
+  end
+  ssMdl.Name='open loop model'; %model for observer
+
 
   [Ap,Bp,Cp,Dp]=ssdata(ssPlt);
   [Am,Bm,Cm,Dm]=ssdata(ssMdl);
 
-  
-  %sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
+  if bitand(mShow,1)
+    figure();h=bodeplot(ssPlt,ssMdl);
+    setoptions(h,'IOGrouping','all')
+  end
 
-  figure();h=bodeplot(ssPlt,ssMdl);
-  setoptions(h,'IOGrouping','all')
-
-  % step answer on open loop:
-  t = 0:1E-4:.5;
-  u = ones(size(t));
   xp0 = zeros(1,length(Ap));
   xm0 = zeros(1,length(Am));
 
-  [yp,t,x] = lsim(ssPlt,u,t,xp0);
-  [ym,t,x] = lsim(ssMdl,u,t,xm0);
-  figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
-  legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
-
+  if bitand(mShow,2)
+    % step answer on open loop:
+    t = 0:1E-4:.5;
+    u = ones(size(t));
+    [yp,t,x] = lsim(ssPlt,u,t,xp0);
+    [ym,t,x] = lsim(ssMdl,u,t,xm0);
+    figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
+    legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
+  end
   poles = eig(Am);
   %w0=abs(poles);
   %ang=angle(-poles);
@@ -50,7 +107,6 @@ function [ssc]=StateSpaceControlDesign(mot)
   % *** space state controller ***
   %
   %place poles for the controller feedback
-  use_lqr=0;
   if use_lqr %use the lqr controller
     Q=eye(length(ssMdl.A));
     R=1;
@@ -59,42 +115,41 @@ function [ssc]=StateSpaceControlDesign(mot)
     if mot.id==1
       %2500rad/s = 397Hz -> locate poles here
       %6300rad/s = 1027Hz -> locate poles here
-      if length(poles)==4
-        p1=-6300+280i;
-        p2=-6200+150i;   
-        P=[p1 p1' p2 p2'];
-        P=[-4100 -4000 -1500+10j -1500-10j];
-      else
-        p1=-3300+2800i;
-        p2=-2700+500i;
-        p3=-2500+10i;
-        %p1=-3300+2800i;
-        %p2=-1500+500i;
-        %p3=-1200+10i;
-        P=[p1 p1' p2 p2' p3 p3'];
+      switch mMdl
+        case 0
+          p1=-3300+2800i;  p2=-2700+500i;  p3=-2500+10i;
+          P=[p1 p1' p2 p2' p3 p3'];
+        case 1
+          %p1=-6300+280i;    p2=-6200+150i;
+          %P=[p1 p1' p2 p2'];
+          P=[-4100 -4000 -1500+10j -1500-10j];
+        case 2
+          %p1=-6300+280i;    p2=-6200+150i;
+          %P=[p1 p1' p2 p2'];
+          P=[-1500+10j -1500-10j];
+        case 3
+          %p1=-6300+280i;    p2=-6200+150i;
+          %P=[p1 p1' p2 p2'];
+          P=[-1500+10j -1500-10j -1400 -1300];
       end
     else
       %2500rad/s = 397Hz -> locate poles here
       %6300rad/s = 1027Hz -> locate poles here
-      if length(poles)==4
-        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;
-        p3=-2900+80i;
-        p4=-2300+450i;
-        p5=-2000+20i;
-        p6=-1500+10i;
-        %p1=-3300+2800i;
-        %p2=-1500+500i;
-        %p3=-1200+10i;
-        P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
+      switch mMdl
+        case 0
+          p1=-3300+2800i;    p2=-1900+130i;       p3=-2900+80i;
+          p4=-2300+450i;     p5=-2000+20i;        p6=-1500+10i;
+          P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
+        case 1
+          %p1=-6300+2800i;   p2=-6200+1500i;
+          %P=[p1 p1' p2 p2'];
+          P=[-2500 -2800 -1500+10j -1500-10j];
+        case 2
+          %p1=-6300+2800i;   p2=-6200+1500i;
+          %P=[p1 p1' p2 p2'];
+          P=[-2500 -2800 -1500+10j -1500-10j];
       end
     end
-    %P=P*.1; % P was too aggressive
     K = place(Am,Bm,P);
     %K = acker(Am,Bm,Pm);
   end %if lqr
@@ -104,19 +159,15 @@ function [ssc]=StateSpaceControlDesign(mot)
   if length(V)>1
     V=V(3); % only the position scaling needed
   end
-  %prefilter to compensate non observable resonance frequencies
-  numV=mot.prefilt.Numerator{1};
-  denV=mot.prefilt.Denominator{1};
 
-  
-  
-  % step answer on closed loop with space state controller:
-  t = 0:1E-4:.5;
-  ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',mot.ssMdl.InputName,'OutputName',mot.ssMdl.OutputName);
-  [y,t,x]=lsim(ss_cl,V*u,t,xm0);
-  figure();plot(t,y);title('step on closed loop');
-  
-  
+  ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.OutputName);
+  if bitand(mShow,4)
+    % step answer on closed loop with space state controller:
+    t = 0:1E-4:.5;
+    [y,t,x]=lsim(ss_cl,V*u,t,xm0);
+    figure();plot(t,y);title('step on closed loop');
+  end
+
   % *** observer controller ***
   %
   %observer poles-> 5 times farther left than system poles
@@ -131,54 +182,83 @@ function [ssc]=StateSpaceControlDesign(mot)
   Ct = [ Cm    zeros(size(Cm)) ];
 
   Dt=0;
-  % step answer on closed loop with observer controller:
-  ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
-  figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
+  ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName);
+  if bitand(mShow,8)
+    % step answer on closed loop with observer controller:
+    figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
+  end
 
-  
   % *** disctrete observer controller ***
   %
   Ts=1/5000; % 5kHz
   ss_tz = c2d(ss_t,Ts);
   [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
   ss_tz.Name='discrete obsvr ctrl';
-  % step answer on closed loop with disctrete observer controller:
-  t = 0:Ts:.05;
-  figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
 
-  %plot all bode diagrams of desPos->actPos
-  figure();
-  h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3));
-  setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+  if bitand(mShow,16)
+    % step answer on closed loop with disctrete observer controller:
+    t = 0:Ts:.05;
+    figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
+  end
 
-  figure();
-  h=pzplot(ss_cl(3),ss_t(3),ss_tz(3));
-  setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+  if bitand(mShow,32)
+    %plot all bode diagrams of desPos->actPos
+    figure();
+    if mMdl==2 || mMdl==3
+      idx=1;
+    else
+      idx=3;
+    end
+    h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
+    setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+
+    figure();
+    h=pzplot(ss_cl(idx),ss_t(idx));
+    setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+    figure();
+    h=pzplot(ss_tz(idx));
+    setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+  end
 
-  
   %calculate matrices for the simulink system
   Ao=Am-L*Cm;
   Bo=[Bm L];
   Co=K;
   Do=zeros(size(Co,1),size(Bo,2));
-
-  ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
+  
+  if mMdl==2 || mMdl==3
+    ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
+  else
+    ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
+  end
   
   %discrete plant
   ssPltz = c2d(ssPlt,Ts);
   [Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
-  
-  %discrete observer controller 
+
+  %discrete observer controller
   ss_oz = c2d(ss_o,Ts);
   [Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
-  mdlName='observer';
+  %mdlName='observer';
   %open(mdlName);
-  
+
+
+  %prefilter to compensate non observable resonance frequencies
+  prefilt=Prefilt(mot,mPrefilt);
+
+  numV=prefilt.Numerator{1};
+  denV=prefilt.Denominator{1};
+
   %discrete prefilter
-  prefiltz=c2d(mot.prefilt,Ts);
+  prefiltz=c2d(prefilt,Ts);
   numVz=prefiltz.Numerator{1};
   denVz=prefiltz.Denominator{1};
- 
+
+  if bitand(mShow,64)
+    h=bodeplot(prefilt,prefiltz);
+    setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
+  end
+
   %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","numVz","denVz"]
@@ -187,6 +267,40 @@ function [ssc]=StateSpaceControlDesign(mot)
   save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
 end
 
+function pf=Prefilt(mot,mode)
+  switch mode
+    case 0 %no filter
+        pf=tf(1,1);
+    case 1 %inverse resonance
+      if mot.id==1
+        den=mot.mdl.num2;%num=1;
+        num=mot.mdl.den2;%den=[1 0 0];
+        pf=tf(num,den);
+      else
+        den=conv(conv(conv(mot.mdl.num2,mot.mdl.num3),mot.mdl.num4),mot.mdl.num5);%num=1;
+        num=conv(conv(conv(mot.mdl.den2,mot.mdl.den3),mot.mdl.den4),mot.mdl.den5);%den=[1 0 0];
+        pf=tf(num,den);
+      end
+    case 2
+      if mot.id==1
+        f=200;w0=f*2*pi;  num1=[1 300 w0^2]; den1=[1 200 w0^2];
+        numV=num1;
+        denV=den1;
+        pf=tf(numV,denV);
+      else
+        f=277;w0=f*2*pi;  num1=[1 20 w0^2];  den1=[1 500 w0^2];
+        f=138;w0=f*2*pi;  num2=[1 300 w0^2]; den2=[1 100 w0^2];
+        f=60;w0=f*2*pi;   num3=[1 33 w0^2];  den3=[1 20 w0^2];
+        numV=conv(num1,num2);
+        denV=conv(den1,den2);
+        numV=conv(conv(num1,num2),num3);
+        denV=conv(conv(den1,den2),den3) ;
+        pf=tf(numV,denV);
+      end
+  end
+  %controlSystemDesigner('bode',1,pf);    % <<<<<<<<< This opens a transferfunction that can be edited
+end
+
 
 %code snipplets from an example on youtube (see reference at top)
 function SCRATCH()
@@ -200,16 +314,16 @@ function SCRATCH()
   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/documentFunctions.m

@@ -0,0 +1,39 @@
+
+baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuningDoc/';
+%plots bode of desPos_actPos
+[mot1,mot2]=identifyFxFyStage();
+close all;
+[pb]=simFxFyStage(mot2);sim('stage_closed_loop');
+t=desPos_actPos.Time;
+u=desPos_actPos.Data(:,1);
+y=desPos_actPos.Data(:,2);
+figure(1);
+clf;
+plot(t,u,'b');hold on;
+plot(t,y,'color',[0 .5 0]);
+grid on;ylim([-4 4]);
+grid on;ylim([-2 2]);
+saveas(gca, baseDir+"m2_sim_pb.eps",'epsc');
+saveas(gca, baseDir+"m1_sim_ss.eps",'epsc');
+saveas(gca, baseDir+"m2_sim_ss.eps",'epsc');
+saveas(gca, baseDir+"m1_sim_ss_pref.eps",'epsc');
+saveas(gca, baseDir+"m2_sim_ss_pref.eps",'epsc');
+
+
+saveas(gca, baseDir+"m2_mdl_bode.eps",'epsc');
+
+minFrq=1;maxFrq=1000;
+tSec=t(length(t));
+f=(1:length(t))/tSec;
+fu=fft(u);
+fy=fft(y);
+ph=phase(fy./fu)
+mag=abs(fy)./abs(fu);
+magDb=20*log10(mag);
+figure(2);
+subplot(2,1,1);
+semilogx(f,magDb);
+subplot(2,1,2);
+semilogx(f,ph);
+grid on;
+

matlab/identifyFxFyStage.m

@@ -19,6 +19,7 @@ function [mot1,mot2]=identifyFxFyStage()
   % tfc,tf_mdl  : various transfer functions
   % ssPlt       : the final continous state space model of the plant (not observable, not controlable)
   % ssMdl       : the simplified continous state space model for the observer  (observable, controlable)
+  % ssMdlNC     : model without resonance and current loop
   %
   % The used data files (generated from Python) are:
   % (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/ )
@@ -97,7 +98,19 @@ function [mot1,mot2]=identifyFxFyStage()
     else
       ob='not ';%not observable
     end
-    disp([s,' is ',ct,'controlable and ',ob,'observable.']); 
+    disp([s,' is ',ct,'controlable and ',ob,'observable.']);
+  end
+
+  function y=myNorm(y)
+    %normalizes num and den by factor 1000
+    %y.*10.^(3*(length(y):-1:1))
+  end
+
+  function plotBode(mot)
+    t1=tf(mot.ssPlt);t2=tf(mot.ssMdl_c1);t3=tf(mot.ssMdl_12);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',t3(1,1),'m',mot.w);
+    setoptions(h,'FreqUnits','Hz','Grid','on');
+    ax=h.getaxes();
+    legend(ax(1),'Location','sw',{'real','plant','no res','no cur + 1 res'});
   end
 
   function mot=fyStage()
@@ -116,60 +129,80 @@ function [mot1,mot2]=identifyFxFyStage()
     mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
     %ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected
 
-    numc=myNorm(mot.mdl.numc);
-    denc=myNorm(mot.mdl.denc);
-    num1=myNorm(mot.mdl.num1);
-    den1=myNorm(mot.mdl.den1);
-    num2=myNorm(mot.mdl.num2); %resonance
-    den2=myNorm(mot.mdl.den2);
-    g1=tf(numc,denc);  % iqCmd->iqMeas
-    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)
-    %tf(s1) % display all transfer functions
-    num=conv(num1,num2);%num=1;
-    den=conv(den1,den2);%den=[1 0 0];
-    g2=tf(num,den); %iqMeas->ActPos
-    s2=ss(g2);
-    s3=append(s1,s2);
-    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
-    mot.ssPlt=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
-    mot.ssPlt.InputName{1}='iqCmd';
-    mot.ssPlt.OutputName{1}='iqMeas';
-    mot.ssPlt.OutputName{2}='iqVolts';
-    mot.ssPlt.OutputName{3}='actPos';
-    chkCtrlObsv(mot.ssPlt,'ssPlt fyStage');
+    tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop  iqCmd->iqMeas
+    tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
+    tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
+    %state -space model:  ssc:current ssm:mechanics ssa:all (current+mechanics)
+
+    % plant
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> iqVolts
     %             |          3|-------> actPos
     %             +-----------+
+    ssc=ss(tfc);
+    ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
+    ssm=ss(tf1*tf2); %iqMeas->ActPos
+    ssa=append(ssc,ssm);
+    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
+    mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
+    mot.ssPlt.InputName{1}='iqCmd';
+    mot.ssPlt.OutputName{1}='iqMeas';
+    mot.ssPlt.OutputName{2}='iqVolts';
+    mot.ssPlt.OutputName{3}='actPos';
+    chkCtrlObsv(mot.ssPlt,'ssPlt fyStage');
+    %tf(ssa) % display all transfer functions
 
     %simplified model without resonance
-    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);
-    mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
-    mot.ssMdl.InputName{1}='iqCmd';
-    mot.ssMdl.OutputName{1}='iqMeas';
-    mot.ssMdl.OutputName{2}='iqVolts';
-    mot.ssMdl.OutputName{3}='actPos';
-    chkCtrlObsv(mot.ssMdl,'ssMdl fyStage');
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> iqVolts
+    %             |          3|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1); %iqMeas->ActPos
+    ssa=append(ssc,ssm);
+    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
+    mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
+    mot.ssMdl_c1.InputName{1}='iqCmd';
+    mot.ssMdl_c1.OutputName{1}='iqMeas';
+    mot.ssMdl_c1.OutputName{2}='iqVolts';
+    mot.ssMdl_c1.OutputName{3}='actPos';
+    chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fyStage');
 
-    %filter in front of plant to suppress resonances (inverse of reonance)
-    den=num2;%num=1;
-    num=den2;%den=[1 0 0];
-    mot.prefilt=tf(num,den);
+    %model without current loop, with one resonance
+    %this assumes that the iqCmd->iqMeas is not relevant for motion
+    %  u          +-----------+           y
+    %iqMeas------>|1         1|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1*tf2); %iqMeas->ActPos
+    mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
+    mot.ssMdl_12.InputName{1}='iqMeas';
+    mot.ssMdl_12.OutputName{1}='actPos';
+    chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fyStage');
+
+    %model without current loop, no resonance
+    %this assumes that the iqCmd->iqMeas is not relevant for motion
+    %  u          +-----------+           y
+    %iqMeas------>|1         1|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1); %iqMeas->ActPos
+    mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
+    mot.ssMdl_1.InputName{1}='iqMeas';
+    mot.ssMdl_1.OutputName{1}='actPos';
+    chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fyStage');
+    
+    ssLst=["tfc","tf1","tf2","tfc*tf1","tf1*tf2","tfc*tf1*tf2"];
+    sys=[];
+    for s = ssLst
+      eval('sys=ss('+s+');')
+      %t=tf(sys);
+      %disp(evalc('t'))
+      chkCtrlObsv(sys,char(s));
+    end
 
     %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
     %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
-    t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',mot.w);
-    setoptions(h,'FreqUnits','Hz','Grid','on');
-
-  end
-  function y=myNorm(y)
-    %normalizes num and den by factor 1000
-    %y.*10.^(3*(length(y):-1:1))
+    plotBode(mot)
   end
 
   function mot=fxStage()
@@ -188,73 +221,88 @@ function [mot1,mot2]=identifyFxFyStage()
     mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
     mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
 
-    numc=myNorm(mot.mdl.numc);
-    denc=myNorm(mot.mdl.denc);
-    num1=myNorm(mot.mdl.num1);
-    den1=myNorm(mot.mdl.den1);
-    num2=myNorm(mot.mdl.num2); %resonance
-    den2=myNorm(mot.mdl.den2);
-    num3=myNorm(mot.mdl.num3); %resonance
-    den3=myNorm(mot.mdl.den3);
-    num4=myNorm(mot.mdl.num4); %resonance
-    den4=myNorm(mot.mdl.den4);
-    num5=myNorm(mot.mdl.num5); %resonance
-    den5=myNorm(mot.mdl.den5);
-    %num=myNorm(mot.mdl.num);
-    %den=myNorm(mot.mdl.den);
-    g1=tf(numc,denc);  % iqCmd->iqMeas
-    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)
-    %tf(s1) % display all transfer functions
-    num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1;
-    den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0];
+    tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop  iqCmd->iqMeas
+    tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
+    tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
+    tf3=tf(mot.mdl.num3,mot.mdl.den3); %resonance
+    tf4=tf(mot.mdl.num4,mot.mdl.den4); %resonance
+    tf5=tf(mot.mdl.num5,mot.mdl.den5); %resonance
 
-    g2=tf(num,den); %iqMeas->ActPos
-    s2=ss(g2);
-    s3=append(s1,s2);
-    %t_=tf(s3);
-    %bode(g2);figure;bode(t_(3,2));
-    %connect iqMeas from s1 to iqMeas of s2
-    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
+    %state -space model:  ssc:current ssm:mechanics ssa:all (current+mechanics)
 
-    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
-    mot.ssPlt=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
-
-    mot.ssPlt.InputName{1}='iqCmd';
-    mot.ssPlt.OutputName{1}='iqMeas';
-    mot.ssPlt.OutputName{2}='iqVolts';
-    mot.ssPlt.OutputName{3}='actPos'  ;
-    chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');
+    % plant
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> iqVolts
     %             |          3|-------> actPos
     %             +-----------+
+    ssc=ss(tfc);
+    ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
+    ssm=ss(tf1*tf2*tf3*tf4*tf5); %iqMeas->ActPos
+    ssa=append(ssc,ssm);
+    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
+    mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
+    mot.ssPlt.InputName{1}='iqCmd';
+    mot.ssPlt.OutputName{1}='iqMeas';
+    mot.ssPlt.OutputName{2}='iqVolts';
+    mot.ssPlt.OutputName{3}='actPos'  ;
+    chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');
 
     %simplified model without resonance
-    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);
-    mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
-    mot.ssMdl.InputName=mot.ssPlt.InputName;
-    mot.ssMdl.OutputName=mot.ssPlt.OutputName;
-    chkCtrlObsv(mot.ssMdl,'ssMdl fxStage');
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> iqVolts
+    %             |          3|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1); %iqMeas->ActPos
+    ssa=append(ssc,ssm);
+    ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
+    mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
+    mot.ssMdl_c1.InputName{1}='iqCmd';
+    mot.ssMdl_c1.OutputName{1}='iqMeas';
+    mot.ssMdl_c1.OutputName{2}='iqVolts';
+    mot.ssMdl_c1.OutputName{3}='actPos';
+    chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fxStage');
 
-    %filter in front of plant to suppress resonances (inverse of reonance)
-    den=conv(conv(conv(num2,num3),num4),num5);%num=1;
-    num=conv(conv(conv(den2,den3),den4),den5);%den=[1 0 0];
-    mot.prefilt=tf(num,den);
+    %model without current loop, with one resonance
+    %this assumes that the iqCmd->iqMeas is not relevant for motion
+    %  u          +-----------+           y
+    %iqMeas------>|1         1|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1*tf2); %iqMeas->ActPos
+    mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
+    mot.ssMdl_12.InputName{1}='iqMeas';
+    mot.ssMdl_12.OutputName{1}='actPos';
+    chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fxStage');   
+    
+    %model without current loop, no resonance
+    %this assumes that the iqCmd->iqMeas is not relevant for motion
+    %  u          +-----------+           y
+    %iqMeas------>|1         1|-------> actPos
+    %             +-----------+
+    ssm=ss(tf1); %iqMeas->ActPos
+    mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
+    mot.ssMdl_1.InputName{1}='iqMeas';
+    mot.ssMdl_1.OutputName{1}='actPos';
+    chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fxStage');   
+    
+    ssLst=["tfc","tf1","tf2","tf3","tf4","tf5","tfc*tf1","tf1*tf2","tf1*tf2*tf3","tfc*tf1*tf2"];
+    sys=[];
+    for s = ssLst
+      eval('sys=ss('+s+');')
+      %t=tf(sys);
+      %disp(evalc('t'))
+      chkCtrlObsv(sys,char(s));
+    end
 
     %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b');
     %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
-    t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',mot.w);
-    setoptions(h,'FreqUnits','Hz','Grid','on');
-    %controlSystemDesigner('bode',1,mot.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited
-
+    plotBode(mot)
   end
   close all
   mot1=fyStage();
   mot2=fxStage();
+  %controlSystemDesigner('bode',1,mot1.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited
+
 
 end

matlab/testObserver.m

@@ -4,11 +4,11 @@ function [ssc]=testObserver(mot)
   if mdl==0
     A = [ 0   1   0
          980  0  -2.8
+          100 
           0   0  -100 ];
 
     B = [ 0
-          0
-          100 ];
+          0];
 
     C = [ 1 0 0 ];
     D=0;

python/MXTuning.py

@@ -238,21 +238,14 @@ class MXTuning(Tuning):
       s='  //input values\n'
       for i in range(len(u)):
         s+='  double {u}=Mptr->{u};\n'.format(u=u[i])
-      prog+=s+'''\n
+      prog+=s+'''
   if (Mptr->ClosedLoop)
   {
-    return iqCmd;
-  }
-  else
-  {
-    Mptr->Servo.Integrator=0.0;
-    return 0.0;
-  }
 '''
 
-      s='  //x[n+1]=A*x[n]+B*u;\n'
+      s='    //x[n+1]=A*x[n]+B*u;\n'
       for i in range(A.shape[0]):
-        s+='  x[%d]='%i
+        s+='    x[%d]='%i
         for j in range(A.shape[1]):
           s+='%+28.22g*_x[%d]'%(A[i,j],j)
         for j in range(B.shape[0]):
@@ -260,29 +253,37 @@ class MXTuning(Tuning):
         s+=';\n'
       prog+=s+'\n'
 
-      s='  //y=C*x[n]+D*x[n];\n'
+      s='    //y=C*x[n]+D*x[n];\n'
       for i in range(C.shape[0]):
-        s+='  %s='%y[i]
+        s+='    %s='%y[i]
         for j in range(C.shape[1]):
           s+='%+28.22g*_x[%d]'%(C[i,j],j)
         s+=';\n'
       prog+=s+'\n'
 
 
-      prog+='''  iqCmd=DesPos*{V}-{y};
-  if (iqCmd>maxDac)
-  {{
-    iqCmd=maxDac;
+      prog+='''    iqCmd=DesPos*{V}-{y};
+    if (iqCmd>maxDac)
+    {{
+      iqCmd=maxDac;
+    }}
+    else
+    {{
+      if (iqCmd<-maxDac)
+      {{
+        iqCmd=-maxDac;
+      }}
+    }}
+    //return iqCmd;
+    pshm->P[200{motid}]=iqCmd;  //lowpass of Position error
+    return pshm->ServoCtrl(Mptr);
   }}
   else
   {{
-    if (iqCmd<-maxDac)
-    {{
-      iqCmd=-maxDac;
-    }}
+    Mptr->Servo.Integrator=0.0;
+    return 0.0;
   }}
-  return iqCmd;
-}}'''.format(V=V[0,0],y=y[0])
+}}'''.format(V=V[0,0],y=y[0],motid=motid)
 
       hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr);
 EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
@@ -336,7 +337,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
 
     #plt.ion()
     #args.host='MOTTEST-CPPM-CRM0573'
-    #args.host=None
+    args.host=None
     if args.host is None:
       comm=gt=None
     else: