towards simple prefilters

2019-02-08 16:46:38 +01:00
parent cc582d1bf8
commit af732b313e
4 changed files with 64 additions and 35 deletions
--- a/MXfastStageDoc/MXfastStage.tex
+++ b/MXfastStageDoc/MXfastStage.tex
@@ -466,11 +466,11 @@ Figures \ref{fig:mot_chirp_sim} showed similar response as the real stages (Figu
 \begin{figure}[h!]
 \center

-\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz.png} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_DTGz_1.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_DTGz_bode1.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_DTGz_2.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_DTGz_bode2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz.png}\\
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_1.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_bode1.eps}\\ 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_bode2.eps} 
 \caption{Motor 1 sim	Motor 2 sim}
 \label{fig:mot_chirp_sim}
 \end{figure}
@@ -562,27 +562,46 @@ end

 \begin{figure}[h!]
 \center
+\includegraphics[scale=.35]{../matlab/figures/sim_cl_observer.png} 
+\caption{Observer flow chart}
+\label{fig:mot_observer_sim}
+\end{figure}

-\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer.png} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_observer_1.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_observer_bode1.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_observer_2.eps} 
-\includegraphics[scale=.65]{../matlab/figures/sim_cl_observer_bode2.eps} 
-\caption{Motor 1 sim	Motor 2 sim}
-\label{fig:mot_chirp_sim}
+
+\begin{figure}[h!]
+\center
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_1.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode1.eps}\\
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps} 
+\caption{Observer sim: Motor 1 Motor 2}
+\label{fig:mot_observer_sim}
 \end{figure}



-
+Comparing figures \ref{fig:mot_chirp_sim} and \ref{fig:mot_observer_sim} shows the better behavious of the observer model.

 With an additional prefilter resonance and reductions can be suppressed a bit.\\

+\begin{figure}[h!]
+\center
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_1.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode1.eps}\\
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode2.eps} 
+\caption{Observer with prefilter sim: Motor 1 Motor 2}
+\label{fig:mot_observer_sim}
+\end{figure}
+
+
+
 The system was simulated with 5kHz servo loop frequency.
 Higher servo loop frequency does not help, because a continious state space controller behaves not relevantly better than a sampled state space controller at 5kHz.\\

 Now the controller must be implemented on deltatau to check the performance on the real stage.

+\FloatBarrier
 \subsection{Implementing state space controller on Deltatau}

 In MATLAB the function \verb|StateSpaceControlDesign()| produces files \verb%/tmp/ssc[1|2].mat%.\\
--- a/matlab/StateSpaceControlDesign.m
+++ b/matlab/StateSpaceControlDesign.m
@@ -75,8 +75,6 @@ function [ssc]=StateSpaceControlDesign(mot)
  end
  ss_mdl.Name='open loop model'; %model for observer

-
-  [Ap,Bp,Cp,Dp]=ssdata(ss_plt);
  [Am,Bm,Cm,Dm]=ssdata(ss_mdl);

  if bitand(mShow,1)
@@ -84,7 +82,7 @@ function [ssc]=StateSpaceControlDesign(mot)
    setoptions(h,'IOGrouping','all')
  end

-  xp0 = zeros(1,length(Ap));
+  xp0 = zeros(1,length(ss_plt.A));
  xm0 = zeros(1,length(Am));

  if bitand(mShow,2)
@@ -142,6 +140,7 @@ function [ssc]=StateSpaceControlDesign(mot)
          %p1=-6300+2800i;   p2=-6200+1500i;
          %P=[p1 p1' p2 p2'];
          P=[-2500 -2800 -1500+10j -1500-10j];
+          P=[-2500 -2800 -1100+10j -1100-10j];
        case 2
          %p1=-6300+2800i;   p2=-6200+1500i;
          %P=[p1 p1' p2 p2'];
@@ -235,21 +234,12 @@ function [ssc]=StateSpaceControlDesign(mot)

  %discrete observer controller
  ss_oz = c2d(ss_o,Ts);
-  [Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
-  %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(prefilt,Ts);
-  numVz=prefiltz.Numerator{1};
-  denVz=prefiltz.Denominator{1};

  if bitand(mShow,64)
    h=bodeplot(prefilt,prefiltz);
@@ -258,8 +248,6 @@ function [ssc]=StateSpaceControlDesign(mot)

  %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"]
-  %for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
  for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
    ssc=setfield(ssc,k,eval(k));
  end
@@ -287,14 +275,22 @@ function pf=Prefilt(mot,mode)
        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);
+
+        %Lag
+        f=[100 200]; w=f*2*pi; T=1./w;
+        tf1=tf([T(1) 1],[T(2) 1]);
+        %bo = bodeoptions;
+        %bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';
+        %bode(tf1,bo)
+        
+        %k=1.2; aa=2; f=[40 60];w=f*2*pi;   tf([1 33 w0^2];  den3=[1 20 w0^2];
+        %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(conv(num1,num2),num3);
+        %denV=conv(conv(den1,den2),den3) ;
+        %pf=tf(numV,denV);
+        pf=tf1;
      end
  end
  %controlSystemDesigner('bode',1,pf);    % <<<<<<<<< This opens a transferfunction that can be edited
--- a/matlab/documentFunctions.m
+++ b/matlab/documentFunctions.m
@@ -26,6 +26,20 @@ for k =1:2
 end
 disp('document figure generation done');close all;

+
+close all;disp('simulate observer with prefilter...');
+for k =1:2
+  [ssc]=StateSpaceControlDesign(mot{k});sim('observer');
+  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
+  set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
+  print(f,sprintf('figures/sim_cl_obs_pf_%d',mot{k}.id),'-depsc');
+  f=bodeSamples(desPos_actPos);
+  print(f,sprintf('figures/sim_cl_obs_pf_bode%d',mot{k}.id),'-depsc');
+end
+disp('document figure generation done');close all;
+
+
+
 function SCRATCH()
  %plots bode of desPos_actPos
  [mot1,mot2]=identifyFxFyStage();
--- a/matlab/observer.slx
+++ b/matlab/observer.slx