Add some analysis on Voice coil with high mass

org/control_cascade.org

@@ -60,9 +60,9 @@ The control architecture we wish here to study is shown in Figure [[fig:cascade_
     \node[block, left= of addF] (K) {$\bm{K}_{\mathcal{L}}$};
     \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
     \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
-    \node[block, left= of J] (Kx) {$\bm{K}_\mathcal{X}$};
+    \node[block, left= of J] (Kp) {$\bm{K}_\mathcal{X}$};
 
-    \node[block, align=center, left= of Kx] (Ex) {Compute\\Pos. Error};
+    \node[block, align=center, left= of Kp] (Ex) {Compute\\Pos. Error};
 
     % Connections and labels
     \draw[->] (outputF) -- ++(1.0, 0);
@@ -79,8 +79,8 @@ The control architecture we wish here to study is shown in Figure [[fig:cascade_
     \draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);
 
     \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
-    \draw[->] (Ex.east) -- (Kx.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
-    \draw[->] (Kx.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
+    \draw[->] (Ex.east) -- (Kp.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
+    \draw[->] (Kp.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
     \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
 
     \begin{scope}[on background layer]
@@ -169,7 +169,7 @@ We log the signals.
 #+end_src
 
 #+begin_src matlab
-  Kx = tf(zeros(6));
+  Kp = tf(zeros(6));
   Kl = tf(zeros(6));
   Kiff = tf(zeros(6));
 #+end_src
@@ -510,7 +510,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
 
   %% Input/Output definition
   clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kx'],  1, 'input'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
   io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror
 
   %% Run the linearization
@@ -603,10 +603,10 @@ As before, we take the minimum realization.
 
   h = 2; % Lead parameter
 
-  Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * (s + 2*pi*5)/s * 1/(1+s/2/pi/20);
+  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * (s + 2*pi*5)/s * 1/(1+s/2/pi/20);
 
   % Normalization of the gain of have a loop gain of 1 at frequency wc
-  Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
+  Kp = Kp.*diag(1./diag(abs(freqresp(Gx*Kp, wc))));
 #+end_src
 
 #+begin_src matlab :exports none
@@ -617,7 +617,7 @@ As before, we take the minimum realization.
   ax1 = subplot(2, 1, 1);
   hold on;
   for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kp(i,i), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -626,7 +626,7 @@ As before, we take the minimum realization.
   ax2 = subplot(2, 1, 2);
   hold on;
   for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kp(i,i), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');

org/control_voice_coil.org

@@ -44,15 +44,15 @@
 :END:
 
 * Introduction                                                        :ignore:
-The goal here is to study the use of a voice coil based nano-hexapod.
-That is to say a nano-hexapod with a very small stiffness.
+* HAC-LAC + Cascade Control Topology
+** Introduction                                                      :ignore:
 
 #+name: fig:cascade_control_architecture
 #+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs
 #+RESULTS:
 [[file:figs/cascade_control_architecture.png]]
 
-* Matlab Init                                                :noexport:ignore:
+** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
 <<matlab-dir>>
 #+end_src
@@ -69,7 +69,7 @@ That is to say a nano-hexapod with a very small stiffness.
   open('nass_model.slx')
 #+end_src
 
-* Initialization
+** Initialization
 We initialize all the stages with the default parameters.
 #+begin_src matlab
   initializeGround();
@@ -111,14 +111,14 @@ We log the signals.
 #+end_src
 
 #+begin_src matlab
-  Kx = tf(zeros(6));
+  Kp = tf(zeros(6));
   Kl = tf(zeros(6));
   Kiff = tf(zeros(6));
 #+end_src
 
-* Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
+** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
 <<sec:lac_iff>>
-** Identification
+*** Identification
 Let's first identify the plant for the IFF controller.
 #+begin_src matlab
   %% Name of the Simulink File
@@ -135,7 +135,7 @@ Let's first identify the plant for the IFF controller.
   G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
 #+end_src
 
-** Plant
+*** Plant
 The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].
 
 #+begin_src matlab :exports none
@@ -206,7 +206,7 @@ The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].
 #+caption: IFF Plant ([[./figs/cascade_vc_iff_plant.png][png]], [[./figs/cascade_vc_iff_plant.pdf][pdf]])
 [[file:figs/cascade_vc_iff_plant.png]]
 
-** Root Locus
+*** Root Locus
 As seen in the root locus (Figure [[fig:cascade_vc_iff_root_locus]], no damping can be added to modes corresponding to the resonance of the micro-station.
 
 However, critical damping can be achieve for the resonances of the nano-hexapod as shown in the zoomed part of the root (Figure [[fig:cascade_vc_iff_root_locus]], left part).
@@ -259,7 +259,7 @@ The maximum damping is obtained for a control gain of $\approx 70$.
 #+caption: Root Locus for the IFF control ([[./figs/cascade_vc_iff_root_locus.png][png]], [[./figs/cascade_vc_iff_root_locus.pdf][pdf]])
 [[file:figs/cascade_vc_iff_root_locus.png]]
 
-** Controller and Loop Gain
+*** Controller and Loop Gain
 We create the $6 \times 6$ diagonal Integral Force Feedback controller.
 The obtained loop gain is shown in Figure [[fig:cascade_vc_iff_loop_gain]].
 #+begin_src matlab
@@ -303,9 +303,9 @@ The obtained loop gain is shown in Figure [[fig:cascade_vc_iff_loop_gain]].
 #+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_vc_iff_loop_gain.png][png]], [[./figs/cascade_vc_iff_loop_gain.pdf][pdf]])
 [[file:figs/cascade_vc_iff_loop_gain.png]]
 
-* High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
+** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
 <<sec:hac_joint_space>>
-** Identification of the damped plant
+*** Identification of the damped plant
 Let's identify the dynamics from $\bm{\tau}^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure [[fig:cascade_control_architecture]].
 
 #+begin_src matlab
@@ -331,7 +331,7 @@ These unstable poles are probably not physical, and they disappear when taking t
   isstable(Gl)
 #+end_src
 
-** Obtained Plant
+*** Obtained Plant
 The obtained dynamics is shown in Figure [[fig:cascade_vc_hac_joint_plant]].
 
 Few things can be said on the dynamics:
@@ -407,7 +407,7 @@ Few things can be said on the dynamics:
 #+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_vc_hac_joint_plant.png][png]], [[./figs/cascade_vc_hac_joint_plant.pdf][pdf]])
 [[file:figs/cascade_vc_hac_joint_plant.png]]
 
-** Controller Design and Loop Gain
+*** Controller Design and Loop Gain
 As the plant is well decoupled, a diagonal plant is designed.
 
 #+begin_src matlab
@@ -415,15 +415,7 @@ As the plant is well decoupled, a diagonal plant is designed.
 
   h = 2; % Lead parameter
 
-  Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
-       (s + 2*pi*1)/s * ... % Weak Integrator
-       (s + 2*pi*10)/s;
-
-  % Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
-  %      (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
-  %      (s + 2*pi*1)/s * ... % Weak Integrator
-  %      (s + 2*pi*10)/s * ... % Weak Integrator
-  %      1/(1 + s/2/wc); % Low pass filter after crossover
+  Kl = (s + 2*pi*1)/s;
 
   % Normalization of the gain of have a loop gain of 1 at frequency wc
   Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
@@ -461,9 +453,9 @@ As the plant is well decoupled, a diagonal plant is designed.
   isstable(feedback(Gl*Kl, eye(6), -1))
 #+end_src
 
-* Primary Controller in the task space - $\bm{K}_\mathcal{X}$
+** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
 <<sec:primary_controller>>
-** Identification of the linearized plant
+*** Identification of the linearized plant
 We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.
 #+begin_src matlab
   %% Name of the Simulink File
@@ -471,15 +463,16 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
 
   %% Input/Output definition
   clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kx'],  1, 'input'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
   io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror
 
   %% Run the linearization
-  G = linearize(mdl, io, 0);
-  G.InputName  = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
-  G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+  Gp = linearize(mdl, io, 0);
+  Gp.InputName  = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
+  Gp.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
 #+end_src
 
+A minus sign is added because the minus sign is already included in the plant identification.
 #+begin_src matlab
   isstable(Gp)
   Gp = -minreal(Gp);
@@ -495,7 +488,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   Gpl.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
 #+end_src
 
-** Obtained Plant
+*** Obtained Plant
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
@@ -557,6 +550,16 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:primary_plant_voice_coil_X
+#+caption: Obtained Primary plant in the Task space ([[./figs/primary_plant_voice_coil_X.png][png]], [[./figs/primary_plant_voice_coil_X.pdf][pdf]])
+[[file:figs/primary_plant_voice_coil_X.png]]
+
+
 #+begin_src matlab :exports none
   freqs = logspace(0, 4, 1000);
 
@@ -618,21 +621,28 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
-** Controller Design
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:primary_plant_voice_coil_L
+#+caption: Obtained Primary plant in the frame of the legs ([[./figs/primary_plant_voice_coil_L.png][png]], [[./figs/primary_plant_voice_coil_L.pdf][pdf]])
+[[file:figs/primary_plant_voice_coil_L.png]]
+
+
+*** Controller Design
 #+begin_src matlab
   wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
 
   h = 2; % Lead parameter
 
-  Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
-       (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
-       (s + 2*pi*1)/s * ...
-       (s + 2*pi*10)/s * ...
-       1/(1+s/2/wc); % For Piezo
-  % Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil
+  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
+       (1/h) * (1 + s/wc*h)/(1 + s/wc/h); % For Piezo
+  % Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil
 
   % Normalization of the gain of have a loop gain of 1 at frequency wc
-  Kx = Kx.*diag(1./diag(abs(freqresp(Gpx*Kx, wc))));
+  Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, wc))));
 #+end_src
 
 #+begin_src matlab :exports none
@@ -643,7 +653,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   ax1 = subplot(2, 1, 1);
   hold on;
   for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kx(i,i), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -652,7 +662,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   ax2 = subplot(2, 1, 2);
   hold on;
   for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kx(i,i), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -663,11 +673,35 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
   linkaxes([ax1,ax2],'x');
 #+end_src
 
-#+begin_src matlab :exports none :tangle no
-  isstable(feedback(Gpx*Kx, eye(6), -1))
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/loop_gain_primary_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:loop_gain_primary_voice_coil_X
+#+caption: Obtained Loop gain for the primary controller in the Task space ([[./figs/loop_gain_primary_voice_coil_X.png][png]], [[./figs/loop_gain_primary_voice_coil_X.pdf][pdf]])
+[[file:figs/loop_gain_primary_voice_coil_X.png]]
+
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(Gpx*Kp, eye(6), -1))
+#+end_src
+
+And now we include the Jacobian inside the controller.
+#+begin_src matlab
+  Kp = inv(nano_hexapod.J')*Kp;
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(-Gp*Kp, eye(6), +1))
+#+end_src
+
+** Simulation
+Let's first save the 3 controllers for further analysis:
+#+begin_src matlab
+  save('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff', 'Kl', 'Kp')
 #+end_src
 
-* Simulation
 #+begin_src matlab
   load('mat/conf_simulink.mat');
   set_param(conf_simulink, 'StopTime', '2');
@@ -683,14 +717,14 @@ And we simulate the system.
   save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz', '-append');
 #+end_src
 
-* Results
-** Load the simulation results
+** Results
+*** Load the simulation results
 #+begin_src matlab
   load('./mat/experiment_tomography.mat', 'tomo_align_dist');
   load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac', 'cascade_hac_lac_lorentz');
 #+end_src
 
-** Control effort
+*** Control effort
 #+begin_src matlab :exports none
   load('mat/stages.mat', 'nano_hexapod');
 
@@ -747,7 +781,7 @@ And we simulate the system.
 #+caption: Actuator Action during a tomography experiment when using Voice Coil actuators ([[./figs/actuator_force_torques_tomography_voice_coil.png][png]], [[./figs/actuator_force_torques_tomography_voice_coil.pdf][pdf]])
 [[file:figs/actuator_force_torques_tomography_voice_coil.png]]
 
-** Load the simulation results
+*** Load the simulation results
 #+begin_src matlab
   n_av = 4;
   han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
@@ -946,7 +980,538 @@ And we simulate the system.
 [[file:figs/exp_tomography_voice_coil_time_domain.png]]
 
 
+** Robustness to Payload Variability
+*** Initialization
+Let's change the payload mass, and see if the controller design for a payload mass of 1 still gives good performance.
+#+begin_src matlab
+  initializeSample('mass', 50);
+#+end_src
+
+#+begin_src matlab
+  Kp = tf(zeros(6));
+  Kl = tf(zeros(6));
+  Kiff = tf(zeros(6));
+#+end_src
+
+*** Low Authority Control
+Let's first identify the transfer function for the Low Authority control.
+
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],    1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm');  io_i = io_i + 1; % Force Sensors
+
+  %% Run the linearization
+  G_iff_m = linearize(mdl, io, 0);
+  G_iff_m.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G_iff_m.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+#+end_src
+
+The obtained dynamics is compared when using a payload of 1Kg in Figure [[fig:voice_coil_variability_mass_iff]].
+
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G_iff_m(i, i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+  title('Diagonal elements of the Plant');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_iff
+#+caption: Dynamics of the LAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff.png][png]], [[./figs/voice_coil_variability_mass_iff.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_iff.png]]
+
+A gain of 50 will here suffice to obtain critical damping of the nano-hexapod modes.
+
+Let's load the IFF controller designed when the payload has a mass of 1Kg.
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_iff_loop_gain
+#+caption: Loop gain for the IFF Control when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff_loop_gain.png][png]], [[./figs/voice_coil_variability_mass_iff_loop_gain.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_iff_loop_gain.png]]
+
+*** High Authority Control
+We use the Integral Force Feedback developed with a mass of 1Kg and we identify the dynamics for the High Authority Controller in the case of the 50Kg payload
+
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],    1, 'input');               io_i = io_i + 1; % Actuator Inputs
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'output', [], 'Dnlm');  io_i = io_i + 1; % Leg Displacement
+
+  %% Run the linearization
+  Gl_m = linearize(mdl, io, 0);
+  Gl_m.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  Gl_m.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
+ 
+  isstable(Gl_m)
+  Gl_m = minreal(Gl_m);
+  isstable(Gl_m)
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gl_m(i, i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title('Diagonal elements of the Plant');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend();
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_hac_plant
+#+caption: Dynamics of the HAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_plant.png][png]], [[./figs/voice_coil_variability_mass_hac_plant.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_hac_plant.png]]
+
+We load the HAC controller design when the payload has a mass of 1Kg.
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_hac_lool_gain
+#+caption: Loop Gain of the HAC when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_hac_lool_gain.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_hac_lool_gain.png]]
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(Gl_m*Kl, eye(6), -1))
+#+end_src
+
+*** Primary Plant
+We use the Low Authority Controller developed with a mass of 1Kg and we identify the dynamics for the Primary controller in the case of the 50Kg payload.
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror
+
+  %% Run the linearization
+  Gp_m = linearize(mdl, io, 0);
+  Gp_m.InputName  = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
+  Gp_m.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+A minus sign is added to cancel the minus sign already included in the identified plant.
+#+begin_src matlab
+  isstable(Gp_m)
+  Gp_m = -minreal(Gp_m);
+  isstable(Gp_m)
+#+end_src
+
+#+begin_src matlab
+  load('mat/stages.mat', 'nano_hexapod');
+  Gpx_m = Gp_m*inv(nano_hexapod.J');
+  Gpx_m.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+
+  Gpl_m = nano_hexapod.J*Gp_m;
+  Gpl_m.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
+#+end_src
+
+#+begin_important
+There are two zeros with positive real part for the plant in the y direction at about 100Hz.
+This is problematic as it limits the bandwidth to be less than $\approx 50\ \text{Hz}$.
+
+It is important here to physically understand why such "positive" zero appears.
+
+If we make a "rigid" 50kg paylaod, the positive zero disappears.
+#+end_important
+
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title('Diagonal elements of the Plant');
+
+  ax2 = subplot(2, 2, 3);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend();
+
+  ax3 = subplot(2, 2, 2);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(Gpx_m(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Gpx_m(1, 1), freqs, 'Hz'))), '--');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title('Off-Diagonal elements of the Plant');
+
+  ax4 = subplot(2, 2, 4);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_primary_plant
+#+caption: Dynamics of the Primary plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_plant.png][png]], [[./figs/voice_coil_variability_mass_primary_plant.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_primary_plant.png]]
+
+We load the primary controller that was design when the payload has a mass of 1Kg.
+
+We load the HAC controller design when the payload has a mass of 1Kg.
+#+begin_src matlab
+  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
+  Kp_x = nano_hexapod.J'*Kp;
+#+end_src
+
+#+begin_src matlab
+  wc = 2*pi*50; % Bandwidth Bandwidth [rad/s]
+
+  h = 2; % Lead parameter
+
+  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
+       (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
+       (s + 2*pi*1)/s * ...
+       1/(1+s/2/wc); % For Piezo
+
+  % Normalization of the gain of have a loop gain of 1 at frequency wc
+  Kp = Kp.*diag(1./diag(abs(freqresp(Gpx_m*Kp, wc))));
+#+end_src
+
+#+begin_src matlab :exports none
+  labels = {'$L_{x}$', '$L_{y}$', '$L_{z}$', '$L_{R_x}$', '$L_{R_y}$', '$L_{R_z}$'};
+
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))));
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))), 'DisplayName', labels{i});
+  end
+  set(gca,'ColorOrderIndex',1);
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northwest')
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:voice_coil_variability_mass_primary_lool_gain
+#+caption: Loop Gain of the Primary loop when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_primary_lool_gain.pdf][pdf]])
+[[file:figs/voice_coil_variability_mass_primary_lool_gain.png]]
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(Gpx_m*Kp_x, eye(6), -1))
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(Gp_m*Kp, eye(6), -1))
+#+end_src
+
+*** Simulation
+#+begin_src matlab
+  load('mat/conf_simulink.mat');
+  set_param(conf_simulink, 'StopTime', '2');
+#+end_src
+
+And we simulate the system.
+#+begin_src matlab
+  sim('nass_model');
+#+end_src
+
+#+begin_src matlab
+  cascade_hac_lac_lorentz_high_mass = simout;
+  save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz_high_mass', '-append');
+#+end_src
+
+#+begin_src matlab
+  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  ax1 = subplot(2, 3, 1);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 1))
+  hold off;
+  ylabel('$D_x$, $D_y$, $D_z$ [m]');
+
+  ax2 = subplot(2, 3, 2);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 2))
+  hold off;
+
+  ax3 = subplot(2, 3, 3);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 3))
+  hold off;
+
+  ax4 = subplot(2, 3, 4);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 4))
+  hold off;
+  xlabel('Time [s]');
+  ylabel('$R_x$, $R_y$, $R_z$ [rad]');
+
+  ax5 = subplot(2, 3, 5);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 5))
+  hold off;
+  xlabel('Time [s]');
+
+  ax6 = subplot(2, 3, 6);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
+  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
+  hold off;
+  xlabel('Time [s]');
+  legend();
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+  xlim([0.5, inf]);
+#+end_src
+
 * Other analysis
+** Robustness to Payload Variability
+- [ ] For 3/masses (1kg, 10kg, 50kg), plot each of the 3 plants
+
 ** Direct HAC control in the task space - $\bm{K}_\mathcal{X}$
 *** Introduction                                                    :ignore: