diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex
index 2067ccc..27e66d8 100644
--- a/MXfastStageDoc/MXfastStage.tex
+++ b/MXfastStageDoc/MXfastStage.tex
@@ -128,6 +128,13 @@ Out of the bode plot we can read approx.:\\
 Motor 1: -33dB at 130Hz\\
 Motor 2: -33dB at 84Hz\\
 
+The chirp parameters for low frequencies bode measurements (yellow) are: \verb|minFrq=1, maxFrq=15, amp=1000|.\\
+In this regime of 0 to 10Hz the bode amplitude is flat, which is mainly the result of the friction:\\
+Motor 1 has 9.63dB($\approx$ factor 10) at 10 Hz. This means that an amplitude of 1000um needs roughly 100 \verb|curr_bits|. 
+The friction as measured in figure \ref{fig:mot1_frict} is around 100 \verb|curr_bits|. At lower frequencies than 10 Hz with an amplitude of 1000um, the friction free motion consumes much less current (-40dB) than the friction.
+Therefore the whole current is used to overcome the friction and the bode amplitude remains constant.
+
+
 %n times higher mass $\rightarrow$ n times lower frequency for same amplitude response\\
 %n times higher frequency $\rightarrow$ n times higher velocity $\rightarrow$ $n^2$ times more acceleration==current
 %1um at 12Hz with 1 mA $\rightarrow$ with 2000mA $\rightarrow$ sqrt(2000)*12Hz=540Hz
@@ -385,15 +392,15 @@ springs: $k=k_1+k_2+\ldots+k_n$\\
 
 \begin{equation}
 \begin{aligned}
-\dot{x} = Ax + Bu\\
-y = Cx + Du
+\mathbf{\dot{x} = Ax + Bu}\\
+\mathbf{y = Cx + Du}
 \end{aligned}\label{eq:mech2}
 \end{equation}
 
 \eqref{eq:mech2} are the general input output equations in matrix form with x and A defines as \eqref{eq:mech3}:
 
 \begin{equation}
-x=
+\mathbf{x}=
 \begin{bmatrix}
 x_1\\
 \dot{x}_1\\
@@ -404,7 +411,7 @@ x_3\\
 x_4\\
 \dot{x}_4\\
 \end{bmatrix},\quad
-\dot{x}=
+\mathbf{\dot{x}}=
 \begin{bmatrix}
 \dot{x}_1\\
 \ddot{x}_1\\
@@ -415,7 +422,7 @@ x_4\\
 \dot{x}_4\\
 \ddot{x}_4\\
 \end{bmatrix},\quad
-A=
+\mathbf{A}=
 \begin{bmatrix}
 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\
 -\frac{k}{m_1} & -\frac{d}{m_1} & \frac{k_2}{m_1} & \frac{d_2}{m_1} & \frac{k_3}{m_1} & \frac{d_3}{m_1} & \frac{k_4}{m_1} & \frac{d_4}{m_1}\\
@@ -440,7 +447,14 @@ B=\begin{bmatrix}
 \label{eq:mech3}
 \end{equation}
 
+\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
+$k_1=0$ The non linear friction has to be compensate outside of the control loop.\\
+$d_1$ seems to be negligable (section \ref{sec:stageData}).\\
+But the system is not observable. $\rightarrow$ Perhaps the integrators of the system can be removed. having only $\mathbf{x}=[ \dot{x}_1 \dot{x}_2 \dot{x}_3 \dot{x}_4]$ as the state vector
+\end{tcolorbox}
+
 \subsection{Stage data}
+\label{sec:stageData}
 
 This data comes from datasheets and construction information \cite{DynParker}.
 
@@ -469,16 +483,19 @@ Inductance &L& 2.4mH\\
 
 \vspace{1pc}
 
-The data in the data sheet are quite confusing. But lets check the motor Konstant Km.\\
+The data sheet for the current are quite confusing (sine, trap, rms). But lets check the motor Konstant Km.\\
 The data sheet says:\\
-Stall Current Continous 0.92A, Stall force Continous 4N
-
+Stall Current Continous 0.92A (trap, DC), Stall force Continous 4N
 \[
 U=R\cdot I \qquad P=U \cdot I \quad \rightarrow \quad  P=R \cdot I^2\\
 \]
-
 at a constant current of 0.92A we have $ 8.8 \cdot 0.92^2 = 7.44 $W the resulting force will be:\\
-$1.46N \cdot \sqrt{7.44} = 3.98 N$
+$1.46N \cdot \sqrt{7.44} = 3.98 N$\\
+
+Lets have a look at the viscose damping:\\
+our speed is roughly 50$\mu$m/10ms. this is 5mm/s. The damping force at that speed is: $0.5\frac{Ns}{m}\cdot 5\frac{mm}{s}=2\cdot10^{-3}N$.
+The used current for that force is negligable, and theerefore the viscose damopling is negligable.
+
 
 
 \subsection{identification of stages}
@@ -500,20 +517,26 @@ The approximated transfer functions can be tweaked and edited with: e.g. \verb|c
 The full transfer function is then split in individual parts that are put back into \verb|MXTuning.py|.\\
 
 the transfer functions for each two motors are separated atomic transfer function as stated in table \ref{tab:trfFunc1}\\
+tfc = transfer function current, tfp = transfer function position. Consecutive letters are simplified transfer functions. tf[1-9]=transfer function  resonance.
 
 \begin{table}[h!]
 \center
 \begin{tabular}{|l|l|l|l|}
 \hline
-key & description                          & motor1 (fy)                 & motor2 (fx)              \\
+key & description                          & motor1 (fy)                 & motor2 (fx)                 \\
 \hline
-tfc & curren loop tf                       & f=694 ? 1389 , d=0.75       & same                     \\
-tf1 & \vtop{\hbox{\strut main mechanical tf} \hbox{\strut with -40dB/dec }}
-                                           & mag=6dB f=7.96Hz d=0.6      & mag=12dB f=3.34 d=0.4    \\
-tf2 & mechanical resonance tf              & f=[197,199] d=[0.02,0.02]   & f=[55|61] d=[0.2|0.2]    \\
-tf3 & mechanical resonance tf              &                             & f=[128|137] d=[0.05|0.05]   \\
-tf4 & mechanical resonance tf              &                             & f=[410|417] d=[0.015|0.015] \\
-tf5 & mechanical resonance tf              &                             & f=[230|233] d=[0.04|0.04]   \\
+tfc & current loop tf                      & f=694 ? 1389 , d=0.75       & same                        \\
+tfd & simplified cur.loop tf $PT_1$        & f-3dB, 45\deg at 400Hz      & same                        \\
+\hline
+tfp & \vtop{\hbox{\strut main mechanical tf} \hbox{\strut with -40dB/dec }}
+                                           & mag=6dB f=7.96Hz d=0.6      & mag=12dB f=3.34 d=0.4       \\
+tfq & \vtop{\hbox{\strut simplified mechanical tf} \hbox{\strut 2 integrators with -40dB/dec }}
+                                           & 0dB at 19.8Hz       & 0dB at 11.84Hz\\
+\hline
+tf1 & mechanical resonance tf              & f=[197,199] d=[0.02,0.02]   & f=[55|61] d=[0.2|0.2]       \\
+tf2 & mechanical resonance tf              &                             & f=[128|137] d=[0.05|0.05]   \\
+tf3 & mechanical resonance tf              &                             & f=[410|417] d=[0.015|0.015] \\
+tf4 & mechanical resonance tf              &                             & f=[230|233] d=[0.04|0.04]   \\
 \hline
 \end{tabular}
 \caption{plant partial transfer functions}
@@ -521,7 +544,7 @@ tf5 & mechanical resonance tf              &                             & f=[23
 \end{table}
 %\vspace{1pc}
 
-The black line on figure \ref{fig:mot_open} shows the concatenate transfer function $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$ .
+The black line on figure \ref{fig:mot_open} shows the concatenate transfer function $tfc \cdot tfp \cdot tf1 \cdot tf2 \cdot tf3 \cdot tf4$.\\
 \vspace{1pc}
 
 \fbox{\parbox[t]{15cm}{The current loop frequency from the MATLAB identification looks different than the used one. Matlab identifies $w_0=8727rad/sec = f0=1389Hz$ but to match the bode plot a value of half frequency need to be taken:$f_0=694$. The reason could be discretization and time delay because the servo loop is processed after the phase loop.}}
@@ -584,29 +607,17 @@ opposite to the PID cpntroller which is a ‘black-box design’, the  state spa
 \end{figure}
 
 
-To implement a state space controller with observer the model has to be observable and controllable. The full model of the tranfer function $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$ is not controllable due to the various resonance frequencies. Therefore for the observer design the state space model has to be simplified.
+To implement a state space controller with observer the model has to be observable and controllable. The full model of the tranfer function $tfc \cdot tfp \cdot  tf1 \cdot tf2 \cdot tf3 \cdot tf4$ is not controllable due to the various resonance frequencies. Therefore for the observer design the state space model has to be simplified. (see simplified transfer function in table  \ref{tab:trfFunc1}\\
 
-In addition to the transferfunction in table  \ref{tab:trfFunc1} following simplified transfer functions have been added:\\
-
-\begin{tabular}{|l|l|l|l|}
-\hline
-key & description                          & motor1 (fy)                 & motor2 (fx)              \\
-\hline
-tfd & simplified loop tf  $PT_1$           & f-3dB, 45\deg at 400Hz       & same                     \\
-tf0 & \vtop{\hbox{\strut simplified mechanical tf} \hbox{\strut 2 integrators with -40dB/dec }}
-                                           & 0dB at 19.8Hz       & 0dB at 11.84Hz\\
-\hline
-\end{tabular}
-\vspace{1pc}
 
 Figure \ref{fig:mdl_bode1} shows the bode plots of the best model to the most simplified model:\\
 
 \begin{tabular}{ll}
-plant & best model tranfer function: $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$\\
-mdl1c & main mechanical with second order current loop: $tf1 \cdot tfc$\\
-mdl1d & main mechanical with $PT_1$ current loop (first order): $tf1 \cdot tfd$\\
-mdl1  & main mechanical (incl. 'viscose friction'): $tf1$\\
-mdl0  & only 2 integrators: $tf0$\\
+plant & best model tranfer function: $tfc \cdot tfp \cdot  tf1 \cdot tf2 \cdot tf3 \cdot tf4$\\
+mdlcp & main mechanical with second order current loop: $tfc \cdot tfp$\\
+mdldp & main mechanical with $PT_1$ current loop (first order): $tfd \cdot tfp$\\
+mdlp  & main mechanical (incl. 'viscose friction'): $tfp$\\
+mdlq  & only 2 integrators: $tfq$\\
 \end{tabular}
 \vspace{1pc}
 
@@ -627,10 +638,10 @@ The matlab models are:\\
 
 \begin{tabular}{ll}
 \texttt{ss\_plt:} &  best approach of the plant with mechanics, resonance, current loop etc.\\
-\texttt{ss\_c1:} & model without resonance (only current and main mechanical)\\
-\texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\
-\texttt{ss\_1:}  & model without current loop, no resonance (only main mechanical)\\
-\texttt{ss\_0:} & simplified mechanical, no current loop, no resonance\\
+\texttt{ss\_cp:} & model without resonance (only current and main mechanical)\\
+\texttt{ss\_dp:} & model without resonance (simplified current and main mechanical)\\
+\texttt{ss\_p:}  & model without current loop, no resonance (only main mechanical)\\
+\texttt{ss\_q:} & simplified mechanical, no current loop, no resonance\\
 \end{tabular}\\
 \vspace{1pc}
 
@@ -639,13 +650,18 @@ Following code calculates parameters for a observer controller, does a simulatio
 \begin{verbatim}
 clear;clear global;close all;
 mot=identifyFxFyStage(7);
-sscType=0
-for k =1:2
-  [ssc]=StateSpaceControlDesign(mot{k},sscType);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_observer_%d',mot{k}.id),'-depsc');
+for sscType=0:1
+  for k=1:2
+    [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
+    f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');grid on;
+    set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
+    print(f,sprintf('figures/sim_cl_obs_%d_%d',sscType,mot{k}.id),'-depsc');
+    f=bodeSamples(desPos_actPos);
+    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',sscType,mot{k}.id),'-depsc');
+  end
 end
+
+
 \end{verbatim}
 
 
@@ -659,10 +675,10 @@ end
 
 \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} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_1.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_1.eps}\\
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_2.eps} 
 \caption{Observer sim: Motor 1 Motor 2}
 \label{fig:mot_observer_sim}
 \end{figure}
@@ -675,10 +691,10 @@ 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} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_1.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_1.eps}\\
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_2.eps} 
+\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_2.eps} 
 \caption{Observer with prefilter sim: Motor 1 Motor 2}
 \label{fig:mot_observer_sim}
 \end{figure}
diff --git a/PBInspect1.pbi b/PBInspect1.pbi
index a94c14f..34aa3ce 100644
--- a/PBInspect1.pbi
+++ b/PBInspect1.pbi
@@ -1,4 +1,9 @@
 [
+  [
+    "StatusGblListCtrl",
+    [],
+    "name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=41;floaty=486;floatw=208;floath=377;notebookid=-1;transparent=255"
+  ],
   [
     "MotorListCtrl",
     [
@@ -24,160 +29,207 @@
   [
     "WatchListCtrl",
     [
+      [
+        "EncTable[9].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[10].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[11].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[12].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[13].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[14].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[15].PrevEnc",
+        null
+      ],
+      [
+        "EncTable[16].PrevEnc",
+        null
+      ],
       [
         "Motor[1].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[1].iqCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[1].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[2].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[2].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[2].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[3].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[3].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[3].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[4].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[4].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[4].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[5].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[5].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[5].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[6].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[6].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[6].ServoOut",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[7].iqCmd",
-        "lambda v: '%.8g'%float(v)"
+        null
       ],
       [
         "Motor[7].idCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "Motor[7].iqCmd",
-        "lambda v: '%.8g'%float(v)"
-      ],
-      [
-        "P1000",
         null
       ],
       [
-        "P2000",
+        "Motor[8].idCmd",
+        null
+      ],
+      [
+        "Motor[1].ServoOut",
+        null
+      ],
+      [
+        "Motor[2].ServoOut",
+        null
+      ],
+      [
+        "Motor[3].ServoOut",
+        null
+      ],
+      [
+        "Motor[4].ServoOut",
+        null
+      ],
+      [
+        "Motor[5].ServoOut",
+        null
+      ],
+      [
+        "Motor[6].ServoOut",
+        null
+      ],
+      [
+        "Motor[7].ServoOut",
+        null
+      ],
+      [
+        "Motor[8].ServoOut",
+        null
+      ],
+      [
+        "Gate3[1].Chan[0].UserFlag",
         null
       ]
     ],
-    "name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255"
+    "name=w;caption=watch;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255"
   ],
   [
     "StatusMotorListCtrl",
     [
-      3
+      1
     ],
-    "name=m3;caption=motor3;state=67377148;dir=4;layer=0;row=6;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=694;floaty=941;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusGblListCtrl",
-    [],
-    "name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2058;floaty=457;floatw=208;floath=377;notebookid=-1;transparent=255"
+    "name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=3;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=485;floaty=810;floatw=208;floath=677;notebookid=-1;transparent=255"
   ],
   [
     "StatusMotorListCtrl",
     [
       2
     ],
-    "name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=5;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2588;floaty=946;floatw=208;floath=677;notebookid=-1;transparent=255"
+    "name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=3;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=520;floaty=734;floatw=208;floath=677;notebookid=-1;transparent=255"
   ],
   [
-    "StatusMotorListCtrl",
+    "WatchListCtrl",
     [
-      4
+      [
+        "P2010",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2011",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2012",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2013",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2014",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2015",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2016",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2017",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2018",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2019",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2020",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2021",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2022",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2023",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2024",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2025",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2026",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2027",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2028",
+        "lambda v: '%.5g'%float(v)"
+      ],
+      [
+        "P2029",
+        "lambda v: '%.5g'%float(v)"
+      ]
     ],
-    "name=m4;caption=motor4;state=67377148;dir=4;layer=0;row=7;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=905;floaty=792;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusMotorListCtrl",
-    [
-      5
-    ],
-    "name=m5;caption=motor5;state=67377148;dir=4;layer=0;row=7;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1139;floaty=836;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusMotorListCtrl",
-    [
-      7
-    ],
-    "name=m7;caption=motor7;state=67377148;dir=4;layer=0;row=8;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1353;floaty=657;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusMotorListCtrl",
-    [
-      6
-    ],
-    "name=m6;caption=motor6;state=67377148;dir=4;layer=0;row=8;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1276;floaty=771;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusMotorListCtrl",
-    [
-      1
-    ],
-    "name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=5;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=669;floaty=892;floatw=208;floath=677;notebookid=-1;transparent=255"
-  ],
-  [
-    "StatusCoordListCtrl",
-    [
-      1
-    ],
-    "name=c1;caption=coord1;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=527;floaty=439;floatw=208;floath=677;notebookid=-1;transparent=255"
+    "name=w-1;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=129863;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=464;floaty=580;floatw=208;floath=377;notebookid=-1;transparent=255"
   ]
 ]
\ No newline at end of file
diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m
index 49010fd..09c14d3 100644
--- a/matlab/StateSpaceControlDesign.m
+++ b/matlab/StateSpaceControlDesign.m
@@ -39,43 +39,67 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
 
 
   %use_lqr: use lqr instead of pole placement
-  verb=1;
+  t=0:1E-4:.5; %time vector for simulations
+  verb=0;
   use_lqr=0;
   MaxDac=2011.968;
-  filt_pos_err=Prefilt(mot,2);
+  tfDesPos=tf(1,1);
+  tfPosErr=tf(1,1);
+  ssc=struct();
+  ssc.sel1={3,[3]};
   %locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
   switch mode
     case -1 %TESTING
+      
+    case 0 %for document: best observer without prefilter
       ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      ss_mdl=mot.ss_d1;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      if mot.id==1
-        pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
-      else
-        pl=[-2500 -900 -800];
-      end
-      plObs=2*pl;
-    case 0
-      ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      ss_mdl=mot.ss_c1;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      ss_mdl=mot.ss_cp;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
       if mot.id==1
         pl=[-3300 -3200 -2900 -2800];
       else
         pl=[-3300 -3200 -2700 -2600];
       end
       plObs=2*pl;
-    case 1
+    case 1 %for document: best observer without prefilter
       ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      ss_mdl=mot.ss_d1;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      ss_mdl=mot.ss_cp;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      if mot.id==1
+        pl=[-3300 -3200 -2900 -2800];
+      else
+        pl=[-3300 -3200 -2700 -2600];
+      end
+      plObs=2*pl;
+      tfDesPos=Prefilt(mot,2);%user designed envelope
+      tfPosErr=Prefilt(mot,1);%inverse resonance
+    case 2 %for document: best observer with prefilter
+      ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      ss_mdl=mot.ss_dp;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
       if mot.id==1
         pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
       else
         pl=[-2500 -900 -800];
       end
       plObs=2*pl;
-    case 2
+    case 3
       ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      ss_mdl=mot.ss_c1;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      use_lqr=1 
+      ss_mdl=mot.ss_dp;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      if mot.id==1
+        pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
+      else
+        pl=[-2500 -900 -800];
+      end
+      plObs=2*pl;
+    case 4
+      %this is the hovering ball model
+      A = [ 0   1   0; 980  0  -2.8;0   0  -100 ];
+      B = [ 0 0 100 ]';
+      C = [ 1 0 0 ];
+      D = 0;
+      ssBall=ss(A,B,C,D,'InputName','iCmd','OutputName','actPos');
+      ss_plt=ssBall; %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      ss_mdl=ssBall;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
+      pl=[-10+10i -10-10i -50];
+      plObs=[-100 -101 -102];
   end
 
   [Am,Bm,Cm,Dm]=ssdata(ss_mdl);
@@ -100,12 +124,14 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
 
   if bitand(verb,4)
     % step answer on open loop:
-    t = 0:1E-4:.5;
     u = ones(size(t));
     [yp,t,x] = lsim(ss_plt,u,t,xp0);
     [ym,t,x] = lsim(ss_mdl,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')
+    figure();hold on; plot(t,yp,'DisplayName',ss_plt.OutputName{1}) 
+    plot(t,ym,'--','DisplayName',ss_plt.OutputName{1});
+    title('step on open loop (plant and model)');
+    %legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
+    legend('location','best')
   end
   %w0=abs(poles);
   %ang=angle(-poles);
@@ -126,13 +152,13 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
 
   V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
   if length(V)>1
-    V=V(3); % only the position scaling needed
+    disp(['scaling (should be actPos): ' ss_mdl.OutputName{end}])
+    V=V(end); % only the position scaling needed
   end
 
   ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
   if bitand(verb,8)
     % 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
@@ -206,7 +232,8 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
   ss_oz = c2d(ss_o,Ts);
 
   %discrete prefilter
-  filt_pos_err_z=c2d(filt_pos_err,Ts);
+  tfDesPos_z=c2d(tfDesPos,Ts);
+  tfPosErr_z=c2d(tfPosErr,Ts);
 
   if bitand(verb,128)
     h=bodeplot(filt_pos_err,filt_pos_err_z);
@@ -214,11 +241,10 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
   end
 
   %state space controller
-  ssc=struct();
-  for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V","MaxDac"]
+  for k=["Ts","ss_plt","ss_o","ss_oz","tfDesPos","tfDesPos_z","tfPosErr","tfPosErr_z","V","MaxDac"]
     ssc=setfield(ssc,k,eval(k));
   end
-  
+
   mat2py=struct();
   %[ozA,ozB,ozC,ozD]=ssdata(ss_oz);
   %[pos_err_num,pos_err_den]=tfdata(filt_pos_err_z);
@@ -230,12 +256,9 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
   mat2py.ozB=ss_oz.B;
   mat2py.ozC=ss_oz.C;
   mat2py.ozD=ss_oz.D;
-  mat2py.ozInpName=ss_oz.InputName
-  mat2py.ozOutName=ss_oz.OutputName
-
-  mat2py.pos_err_num=filt_pos_err_z.Numerator{1};
-  mat2py.pos_err_den=filt_pos_err_z.Denominator{1};
-  
+  mat2py.ozInpName=ss_oz.InputName;
+  mat2py.ozOutName=ss_oz.OutputName;
+ 
   fn=[pwd '/' sprintf( 'ssc%d.mat',mot.id)];
   save(fn,'-struct','mat2py');
   disp(['saved ' fn]);
@@ -247,23 +270,26 @@ function pf=Prefilt(mot,mode)
         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];
+        den=mot.mdl.num1;%num=1;
+        num=mot.mdl.den1;%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];
+        den=conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4);%num=1;
+        num=conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4);%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);
+        %f=200;w0=f*2*pi;  num1=[1 300 w0^2]; den1=[1 200 w0^2];
+        %numV=num1;
+        %denV=den1;
+        %pf=tf(numV,denV);
+        %Lag
+        f=[200 300]; w=f*2*pi; T=1./w;
+        pf=tf([T(1) 1],[T(2) 1]);
       else
         %Lag
-        f=[100 200]; w=f*2*pi; T=1./w;
+        f=[200 400]; 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';
diff --git a/matlab/documentFunctions.m b/matlab/documentFunctions.m
index 670656c..5ccfa9b 100644
--- a/matlab/documentFunctions.m
+++ b/matlab/documentFunctions.m
@@ -8,7 +8,7 @@ mot=identifyFxFyStage(7);
 close all;disp('simulate stage closed loop...');
 for k =1:2 
   [pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
-  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
+  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');grid on;
   set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
   print(f,sprintf('figures/sim_cl_DTGz_%d',mot{k}.id),'-depsc');
   f=bodeSamples(desPos_actPos);
@@ -16,14 +16,14 @@ for k =1:2
 end
 
 close all;disp('simulate observer...');
-for sscType=0%0:1
+for sscType=1%0:1
   for k=1:2
     [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
-    f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
+    f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');grid on;
     set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
-    print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc');
+    print(f,sprintf('figures/sim_cl_obs_%d_%d',sscType,mot{k}.id),'-depsc');
     f=bodeSamples(desPos_actPos);
-    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',m,mot{k}.id),'-depsc');
+    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',sscType,mot{k}.id),'-depsc');
   end
 end
 disp('document figure generation done');close all;
diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m
index d6ffc1a..d4d3236 100644
--- a/matlab/identifyFxFyStage.m
+++ b/matlab/identifyFxFyStage.m
@@ -163,11 +163,11 @@ function motCell=identifyFxFyStage(mode)
     catch
       return
     end
-    h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_c1(3,1),'b',mot.ss_d1(3,1),'m',mot.ss_1(2,1),'c',mot.ss_0(2,1),'k',mot.w);
+    h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_cp(3,1),'b',mot.ss_dp(3,1),'m',mot.ss_p(2,1),'c',mot.ss_q(2,1),'k',mot.w);
     setoptions(h,'FreqUnits','Hz','Grid','on');
     p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p);
     ax=h.getaxes();
-    legend(ax(1),'Location','sw',{'real','plant','c1','d1','1','0'});
+    legend(ax(1),'Location','sw',{'real','plant','cp','dp','p','q'});
     print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc');
   end
 
@@ -175,24 +175,24 @@ function motCell=identifyFxFyStage(mode)
 
     %current loop  iqCmd->iqMeas
     tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); 
-
     %simplified current loop  iqCmd->iqMeas (first order tf)
     tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); 
 
+    %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
+    tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'});
+    %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
+    tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'});
+    %simplified force(=current) to velocity iqForce->(actVel)
+    tfv=tf(mot.mdl.numv,mot.mdl.denv,'InputName','iqForce','OutputName',{'actVel'});
+    
     %resonance iqMeas->iqForce
-    tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqForce');
+    tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqForce');
 
     %current to position iqForce->actPos
-    tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
-    
-    %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
-    tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
-
-    %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
-    tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
-    
+    %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
+        
     %check observable/controlable of transfer functions
-    ssLst=["tfc","tfd","tf0","tf1","tf2","tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2"];
+    ssLst=["tfc","tfd","tfp","tfq","tfv","tf1","tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfc*tfv","tfd*tfv","tfd*tfv*tf1"];
     sys=[];
     for s = ssLst
       eval('sys=ss('+s+');')
@@ -201,23 +201,32 @@ function motCell=identifyFxFyStage(mode)
     
     % sample code:
     %tfc  iqCmd-> iqMeas
-    %tf2  resonance iqMeas->iqForce
-    %tf1  iqForce->(actVel,actPos)
+    %tfp  iqForce->(actVel,actPos)
+    %tf1  resonance iqMeas->iqForce
     %connect(tfc,tf2,'iqCmd','iqForce');
     %connect(tfc,tf2,'iqCmd',{'iqMeas','iqForce'});
     %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','iqForce','actPos'});
     %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','actPos'});
 
-    
     % best plant approximation
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    mot.ss_plt=connect(tfc,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_plt=connect(tfc,tfp,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
     mot.ss_plt.Name='best plant approximation';
     chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');     
+
+    % best plant approximation without friction (always -40dB)
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> actVel
+    %             |          3|-------> actPos
+    %             +-----------+
+    mot.ss_cqr=connect(tfc,tfq,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_cqr.Name='plant no friction';
+    chkCtrlObsv(mot.ss_plt,'ss_cqr fyStage');     
     
     %without resonance
     %  u          +-----------+           y
@@ -225,44 +234,41 @@ function motCell=identifyFxFyStage(mode)
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
-    mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
-    mot.ss_c1.Name='without resonance';
-    chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage');
-    tf1.InputName{1}=s;%restore
+    s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
+    mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_cp.Name='without resonance';
+    chkCtrlObsv(mot.ss_cp,'ss_cp fyStage');
+    tfp.InputName{1}=s;%restore
 
-    
     %simplified current, without resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
-    mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
-    mot.ss_d1.Name='simplified current, without resonance';
-    chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage');
-    tf1.InputName{1}=s;%restore
-    
+    s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
+    mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_dp.Name='simplified current, without resonance';
+    chkCtrlObsv(mot.ss_dp,'ss_dp fyStage');
+    tfp.InputName{1}=s;%restore
     
     %no current loop, no resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> actVel
     %             |          2|-------> actPos
     %             +-----------+
-    mot.ss_1=ss(tf1);
-    mot.ss_1.Name='no current loop, no resonance';
-    chkCtrlObsv(mot.ss_1,'ss_1 fyStage');
-
+    mot.ss_p=ss(tfp);
+    mot.ss_p.Name='no current loop, no resonance';
+    chkCtrlObsv(mot.ss_p,'ss_p fyStage');
     
     %simplified mechanics, no current loop, no resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> actVel
     %             |          2|-------> actPos
     %             +-----------+
-    mot.ss_0=ss(tf0);
-    mot.ss_0.Name='simplified mechanics, no current loop, no resonance';
-    chkCtrlObsv(mot.ss_0,'ss_0 fyStage');
+    mot.ss_q=ss(tfq);
+    mot.ss_q.Name='simplified mechanics, no current loop, no resonance';
+    chkCtrlObsv(mot.ss_q,'ss_q fyStage');
 
     %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);
@@ -272,58 +278,64 @@ function motCell=identifyFxFyStage(mode)
   function mot=fxStage(mot)
     %current loop  iqCmd->iqMeas
     tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); 
-
     %simplified current loop  iqCmd->iqMeas (first order tf)
     tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); 
 
+    %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
+    tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'});
+    %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
+    tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'});
+    
     %resonance iqMeas->iqForce
-    tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqF1');
+    tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqF1');
     %resonance iqMeas->iqForce
-    tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF1','OutputName','iqF2');
+    tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqF1','OutputName','iqF2');
     %resonance iqMeas->iqForce
-    tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF2','OutputName','iqF3');
+    tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF2','OutputName','iqF3');
     %resonance iqMeas->iqForce
-    tf5=tf(mot.mdl.num5,mot.mdl.den5,'InputName','iqF3','OutputName','iqForce');
+    tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF3','OutputName','iqForce');
     
     %current to position iqForce->actPos
-    tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
-    
-    %force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
-    tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
-
-    %simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos 
-    tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
+    %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
     
     %check observable/controlable of transfer functions
-    ssLst=["tfc","tfd","tf0","tf1","tf2","tf3","tf4","tf5",...
-           "tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2","tf1*tf2*tf3"];
+    ssLst=["tfc","tfd","tfp","tfq","tf1","tf2","tf3","tf4",...
+           "tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfd*tfp*tf1","tfc*tf1","tfd*tf1","tfc*tf1*tf2","tfd*tf1*tf2"];
     sys=[];
     for s = ssLst
       eval('sys=ss('+s+');')
       chkCtrlObsv(sys,char(s));
     end   
 
-
     % best plant approximation
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    mot.ss_plt=connect(tfc,tf1,tf2,tf3,tf4,tf5,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_plt=connect(tfc,tfp,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'});
     chkCtrlObsv(mot.ss_plt,'ss_plt fxStage');     
     
+    % best plant approximation without friction (always -40dB)
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> actVel
+    %             |          3|-------> actPos
+    %             +-----------+
+    mot.ss_cqr=connect(tfc,tfq,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_cqr.Name='plant no friction';
+    chkCtrlObsv(mot.ss_cqr,'ss_plt0 fxStage');     
+    
     %without resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> iqMeas
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
-    mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
-    chkCtrlObsv(mot.ss_c1,'ss_c1 fxStage');
-    tf1.InputName{1}=s;%restore
-
+    s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
+    mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
+    chkCtrlObsv(mot.ss_cp,'ss_cp fxStage');
+    tfp.InputName{1}=s;%restore
     
     %simplified current, without resonance
     %  u          +-----------+           y
@@ -331,28 +343,26 @@ function motCell=identifyFxFyStage(mode)
     %             |          2|-------> actVel
     %             |          3|-------> actPos
     %             +-----------+
-    s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
-    mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
-    chkCtrlObsv(mot.ss_d1,'ss_d1 fxStage');
-    tf1.InputName{1}=s;%restore
-    
-    
+    s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
+    mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
+    chkCtrlObsv(mot.ss_dp,'ss_dp fxStage');
+    tfp.InputName{1}=s;%restore
+        
     %no current loop, no resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> actVel
     %             |          2|-------> actPos
     %             +-----------+
-    mot.ss_1=ss(tf1);
-    chkCtrlObsv(mot.ss_1,'ss_1 fxStage');
+    mot.ss_p=ss(tfp);
+    chkCtrlObsv(mot.ss_p,'ss_p fxStage');
 
-    
     %simplified mechanics, no current loop, no resonance
     %  u          +-----------+           y
     %iqCmd------->|1         1|-------> actVel
     %             |          2|-------> actPos
     %             +-----------+
-    mot.ss_0=ss(tf0);
-    chkCtrlObsv(mot.ss_0,'ss_0 fxStage');  
+    mot.ss_q=ss(tfq);
+    chkCtrlObsv(mot.ss_q,'ss_q fxStage');  
     
     plotBode(mot)
   end
diff --git a/matlab/observer.slx b/matlab/observer.slx
index a199d8b..97d7584 100644
Binary files a/matlab/observer.slx and b/matlab/observer.slx differ
diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m
index 0684aa5..b5d8f21 100644
--- a/matlab/simFxFyStage.m
+++ b/matlab/simFxFyStage.m
@@ -6,24 +6,27 @@ function [pb]=simFxFyStage(mot)
   %the returned variable pb contains varous parameters:
   %  Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D
 
-  Ts=2E-4; % 0.2ms=5kHz
-  MaxDac=2011.968;
-  MaxPosErr=10000;
+  pb=struct(...
+    'Ts', 2E-4, ... % 0.2ms=5kHz
+    'MaxDac' ,2011.968, ...
+    'MaxPosErr', 10000);
   if mot.id==1
     %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
     %!motor(mot=1,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=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
+    pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
   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=3500;MaxInt=1000;
-  end
-  ss_plt=mot.ss_plt;
-  pb=struct();
-  for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","Ts","MaxDac","MaxPosErr","ss_plt"]
-    pb=setfield(pb,k,eval(k));
+    pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
   end
+  pb.ss_plt=mot.ss_plt;  pb.sel={3,[3]};
+  %pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]};
+  %pb.ss_plt=mot.ss_c1; pb.sel={3,[3]};
+  %pb.ss_plt=mot.ss_d1; pb.sel={3,[3]};
+  %pb.ss_plt=mot.ss_1; pb.sel={2,[2]};
+  %pb.ss_plt=mot.ss_p; pb.sel={2,[2]};
+  
   %mdlName='stage_closed_loop';
   %open(mdlName)
   %sim(mdlName) 
diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx
index 956a085..1be8e99 100644
Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ
diff --git a/matlab/testObserver.m b/matlab/testObserver.m
index 93d97f0..777cb23 100644
--- a/matlab/testObserver.m
+++ b/matlab/testObserver.m
@@ -1,50 +1,16 @@
-function [ssc]=testObserver(mot)
+function [ssc]=testObserver()
   %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
-          100 
-          0   0  -100 ];
+  A = [ 0   1   0
+       980  0  -2.8
+        0   0  -100 ];
 
-    B = [ 0
-          0];
+  B = [ 0
+        0
+       100 ];
 
-    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);
-
-    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)
-    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
+  C = [ 1 0 0 ];
+  D=0;
   
   Ap=A;Am=A;
   Bp=B;Bm=B;
@@ -52,159 +18,79 @@ function [ssc]=testObserver(mot)
   Dp=D;Dm=D;Dt=D;
 
   poles = eig(A);
-  disp(poles);
+  poles
 
   
-  if mdl==0
-    t = 0:0.01:2;
-    u = zeros(size(t));
-    x0 = [0.01 0 0];
+  t = 0:0.01:2;
+  u = zeros(size(t));
+  x0 = [0.01 0 0];
 
-    sys = ss(A,B,C,D);
+  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)')
+  [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 = -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;
-    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));
+  t = 0:0.01:2;
+  u = 0.001*ones(size(t));
 
-    sys_cl = ss(A-B*K,B,C,0);
+  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])
+  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])     
-  else
-    x0 = [0 0 0 0];
-    p1=-63+2.80i;
-    p2=-62+1.50i;   
-    P=[p1 p1' p2 p2'];
-    P=[-4000+100j -4000-100j -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);
+  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])     
 
-    lsim(sys_cl,u,t,x0);
-    xlabel('Time (sec)')
-    ylabel('Motor Position (m)')
-    axis([0 2 0 1.2E-3])
-  end
+  op1 = -100;
+  op2 = -101;
+  op3 = -102;
+  OP=[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
+         zeros(size(B)) ];
+  Ct = [ C    zeros(size(C)) ];
 
-  if mdl==0
-    op1 = -100;
-    op2 = -101;
-    op3 = -102;
-    OP=[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
-           zeros(size(B)) ];
-    Ct = [ C    zeros(size(C)) ];
+  sys = ss(At,Bt,Ct,0);
+  %lsim(sys,zeros(size(t)),t,[x0 x0]);
+  lsim(sys,u,t,[x0 x0]);
+  title('Linear Simulation Results (with observer)')
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
 
-    sys = ss(At,Bt,Ct,0);
-    %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)) ];
+  t = 0:1E-6:0.1;
+  x0 = [0.01 0.5 -5];
+  [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]);
 
-    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]);
+  n = 3;
+  e = x(:,n+1:end);
+  x = x(:,1:n);
+  x_est = x - e;
 
-    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);
 
-    % 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)')
-  end
-  
-  
-  %discrete
-  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)
-  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);
-  
-  
-  
+  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)')
+  K,L,V
 end
diff --git a/python/MXTuning.py b/python/MXTuning.py
index e66fa84..ee889e2 100755
--- a/python/MXTuning.py
+++ b/python/MXTuning.py
@@ -95,31 +95,82 @@ class MXTuning(Tuning):
                 #  Hz    -> kHz
                 #  rad/s -> rad/ms
       if mot==1:
+        #current loop 2nd order approx
+        #identification with matlab: k=1, w0=8725, d=0.75
+        dc=.75        # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        wc=8725.*_N   # rad/sec
+        #...but phase lag seems to have earlier effect -> reduce wc
+        wc*=.5       # rad/sec
+        fc=wc/2/np.pi # ca 1388Hz
+        Tc=1/wc
+        numc = np.poly1d([1.])
+        denc = np.poly1d([Tc**2,2*Tc*dc,1])
+
+
+        #simplified current loop 1st order approx
+        wd=2*np.pi*400. #45deg at 360Hz
+        Td=1/wd
+        numd = np.poly1d([1.])
+        dend = np.poly1d([Td,1])
+
+        #force(idMeas)->position
         #identify matlab: k:0.671226 w0:134.705 damp:0.191004
-        mag1=6 #10**(db_mag1/20)
-        db_mag1=20*np.log10(mag1)#dB
-        w1=50.*_N #rad/sec
-        f1=w1/2/np.pi; # ca. 6.5Hz
-        T1=1/w1
-        d1=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
-        num1=np.poly1d([mag1])
-        den1 = np.poly1d([T1**2,2*T1*d1,1])
+        magp=6 #10**(db_mag1/20)
+        db_magp=20*np.log10(magp)#dB
+        wp=50.*_N #rad/sec
+        fp=wp/2/np.pi; # ca. 6.5Hz
+        Tp=1/wp
+        dp=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        nump=np.poly1d([magp])
+        denp = np.poly1d([Tp**2,2*Tp*dp,1])
 
+        #force->position (simplified)
+        #reiner integrator: 19.8Hz=0dB -> k=frq*2*pi=180
+        numq=np.poly1d([(19.8*2*np.pi)**2])
+        denq=np.poly1d([1,0,0])
 
-        #reiner integrator: 19.8Hz=0dB -> k=30*2*pi=180
-        num0=np.poly1d([(19.8*2*np.pi)**2])
-        den0=np.poly1d([1,0,0])
+        #force->velocity (simplified)
+        #reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
+        numv=np.poly1d([19.8*2*np.pi])
+        denv=np.poly1d([1,0])
 
 
         #first resonance frequency
-        f2=np.array([197,199])
-        d2=np.array([.02,.02])#daempfung
-        w2=f2*2*np.pi*_N #rad/sec
-        T2=1/w2
-        num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
-        den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1])
+        f1=np.array([197,199])
+        d1=np.array([.02,.02])#daempfung
+        w1=f1*2*np.pi*_N #rad/sec
+        T1=1/w1
+        num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
+        den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
 
 
+        num=numc*nump*num1#*num3
+        den=denc*denp*den1#*den3
+        mdl= signal.lti(num, den) #num denum
+        #print(num);print(den);print(mdl)
+
+        w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
+
+        mdlcp= signal.lti(numc*nump, denc*denp)
+        mdldp= signal.lti(numd*nump, dend*denp)
+        mdlp=  signal.lti(nump, denp)
+        mdlq=  signal.lti(numq, denq)
+
+        bode(((mdl,'plant'),
+              (mdlcp,'mdl_cp'),
+              (mdldp,'mdl_dp'),
+              (mdlp,'mdl_p'),
+              (mdlq,'mdl_q'),
+              ),w)
+
+        d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,
+           'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs}
+        fn=os.path.join(self.baseDir,'model%d.mat'%mot)
+        import scipy.io
+        scipy.io.savemat(fn, mdict=d)
+        print('save to matlab file:'+fn)
+
+      elif mot==2:
         #current loop 2nd order approx
         #identification with matlab: k=1, w0=8725, d=0.75
         dc=.75        # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
@@ -139,52 +190,39 @@ class MXTuning(Tuning):
         dend = np.poly1d([Td,1])
 
 
-        num=num1*num2*numc#*num3
-        den=den1*den2*denc#*den3
-        mdl= signal.lti(num, den) #num denum
-        #print(num);print(den);print(mdl)
-
-        w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
-
-        mdl1c= signal.lti(num1*numc, den1*denc)
-        mdl1d= signal.lti(num1*numd, den1*dend)
-        mdl1=  signal.lti(num1, den1)
-        mdl0=  signal.lti(num0, den0)
-
-        bode(((mdl,'plant'),
-              (mdl1c,'mdl1c'),
-              (mdl1d,'mdl1d'),
-              (mdl1,'mdl1'),
-              (mdl0,'mdl0'),
-              ),w)
-
-        d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
-           'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
-        fn=os.path.join(self.baseDir,'model%d.mat'%mot)
-        import scipy.io
-        scipy.io.savemat(fn, mdict=d)
-        print('save to matlab file:'+fn)
-
-      elif mot==2:
+        #force(idMeas)->position
         #identify matlab: k:1.7282 w0:51.069 damp:0.327613
-        mag1=12. #10**(db_mag1/20)
-        db_mag1=20*np.log10(mag1)#dB
-        w1=21.*_N #rad/sec
-        f1=w1/2/np.pi; # ca. 6.5Hz
-        T1=1/w1
-        d1=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
-        num1=np.poly1d([mag1])
-        den1 = np.poly1d([T1**2,2*T1*d1,1])
-
+        magp=12. #10**(db_mag1/20)
+        db_magp=20*np.log10(magp)#dB
+        wp=21.*_N #rad/sec
+        fp=wp/2/np.pi; # ca. 6.5Hz
+        Tp=1/wp
+        dp=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        nump=np.poly1d([magp])
+        denp = np.poly1d([Tp**2,2*Tp*dp,1])
 
         #reiner integrator: 11.84Hz=0dB -> k=30*2*pi=180
-        num0=np.poly1d([(11.84*2*np.pi)**2])
-        den0=np.poly1d([1,0,0])
+        numq=np.poly1d([(11.84*2*np.pi)**2])
+        denq=np.poly1d([1,0,0])
+
+        #force->velocity (simplified)
+        #reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
+        numv=np.poly1d([19.8*2*np.pi])
+        denv=np.poly1d([1,0])
 
 
         #resonance frequency
-        f2=np.array([55.,61.])
-        d2=np.array([.2,.2])#daempfung
+        f1=np.array([55.,61.])
+        d1=np.array([.2,.2])#daempfung
+        w1=f1*2*np.pi #rad/sec
+        T1=1/w1
+        num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
+        den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
+
+
+        #resonance frequency
+        f2=np.array([128,137])
+        d2=np.array([.05,.05])#daempfung
         w2=f2*2*np.pi #rad/sec
         T2=1/w2
         num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
@@ -192,8 +230,8 @@ class MXTuning(Tuning):
 
 
         #resonance frequency
-        f3=np.array([128,137])
-        d3=np.array([.05,.05])#daempfung
+        f3=np.array([410,417])
+        d3=np.array([.015,.015])#daempfung
         w3=f3*2*np.pi #rad/sec
         T3=1/w3
         num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1])
@@ -201,64 +239,36 @@ class MXTuning(Tuning):
 
 
         #resonance frequency
-        f4=np.array([410,417])
-        d4=np.array([.015,.015])#daempfung
+        f4=np.array([230,233])
+        d4=np.array([.04,.04])#daempfung
         w4=f4*2*np.pi #rad/sec
         T4=1/w4
         num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1])
         den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1])
 
 
-        #resonance frequency
-        f5=np.array([230,233])
-        d5=np.array([.04,.04])#daempfung
-        w5=f5*2*np.pi #rad/sec
-        T5=1/w5
-        num5 = np.poly1d([T5[0]**2,2*T5[0]*d5[0],1])
-        den5 = np.poly1d([T5[1]**2,2*T5[1]*d5[1],1])
-
-
-        #current loop 2nd order approx
-        #identification with matlab: k=1, w0=8725, d=0.75
-        dc=.75        # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
-        wc=8725.*_N   # rad/sec
-        #...but phase lag seems to have earlier effect -> reduce wc
-        wc*=.5       # rad/sec
-        fc=wc/2/np.pi # ca 1388Hz
-        Tc=1/wc
-        numc = np.poly1d([1.])
-        denc = np.poly1d([Tc**2,2*Tc*dc,1])
-
-
-        #simplified current loop 1st order approx
-        wd=2*np.pi*400. #45deg at 360Hz
-        Td=1/wd
-        numd = np.poly1d([1.])
-        dend = np.poly1d([Td,1])
-
-
-        num=num1*num2*num3*num4*num5*numc
-        den=den1*den2*den3*den4*den5*denc
+        num=numc*nump*num1*num2*num3*num4
+        den=denc*denp*den1*den2*den3*den4
         mdl= signal.lti(num, den) #num denum
         #print(num);print(den);print(mdl)
 
         w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
 
-        mdl1c= signal.lti(num1*numc, den1*denc)
-        mdl1d= signal.lti(num1*numd, den1*dend)
-        mdl1=  signal.lti(num1, den1)
-        mdl0=  signal.lti(num0, den0)
+        mdlcp= signal.lti(numc*nump, denc*denp)
+        mdldp= signal.lti(numd*nump, dend*denp)
+        mdlp=  signal.lti(nump, denp)
+        mdlq=  signal.lti(numq, denq)
 
         bode(((mdl,'plant'),
-              (mdl1c,'mdl1c'),
-              (mdl1d,'mdl1d'),
-              (mdl1,'mdl1'),
-              (mdl0,'mdl0'),
+              (mdlcp,'mdlcp'),
+              (mdldp,'mdldp'),
+              (mdlp,'mdlp'),
+              (mdlq,'mdlq'),
               ),w)
 
 
-        d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
-           'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
+        d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,
+           'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs}
         fn=os.path.join(self.baseDir,'model%d.mat'%mot)
         import scipy.io
         scipy.io.savemat(fn, mdict=d)
@@ -357,7 +367,16 @@ class MXTuning(Tuning):
       prog+='''    //set output
     iqCmd={u}*{V}-{y};
     //return iqCmd;
-    pshm->P[200{motid}]=iqCmd;
+    pshm->P[20{motid}0]=DesPos;
+    pshm->P[20{motid}1]=IqMeas;
+    pshm->P[20{motid}2]=ActVel;
+    pshm->P[20{motid}3]=ActPos;
+    pshm->P[20{motid}4]=_x[0];
+    pshm->P[20{motid}5]=_x[1];
+    pshm->P[20{motid}6]=_x[2];
+    pshm->P[20{motid}7]=_x[3];
+    pshm->P[20{motid}8]=iqCmd;
+
     return pshm->ServoCtrl(Mptr);
   }}
   else
@@ -723,7 +742,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
             bm+=32 #amplitude linear (not dB)
           self.bode_plot(data, mode=bm, kwargs=meta)
           plt.show(block=False)
-          save_figs(fn)
+          #save_figs(fn)
           #wait_key()
           raw_input('press return')
 
diff --git a/python/usr_code/Makefile b/python/usr_code/Makefile
index aa76b5a..398d002 100644
--- a/python/usr_code/Makefile
+++ b/python/usr_code/Makefile
@@ -112,7 +112,7 @@ tags:
 
 Debug: all
 
-../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m
+../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m Makefile
 	cd ../../matlab; /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab -nodisplay -nojvm -nosplash -nodesktop -r \
       "clear;clear global;close all;\
        mot=identifyFxFyStage(7);\