#+TITLE: Cascade Control applied on the Simscape Model :DRAWER: #+STARTUP: overview #+LANGUAGE: en #+EMAIL: dehaeze.thomas@gmail.com #+AUTHOR: Dehaeze Thomas #+HTML_LINK_HOME: ./index.html #+HTML_LINK_UP: ./index.html #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_MATHJAX: align: center tagside: right font: TeX #+PROPERTY: header-args:matlab :session *MATLAB* #+PROPERTY: header-args:matlab+ :comments org #+PROPERTY: header-args:matlab+ :results none #+PROPERTY: header-args:matlab+ :exports both #+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :tangle no #+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:shell :eval no-export #+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}") #+PROPERTY: header-args:latex+ :imagemagick t :fit yes #+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 #+PROPERTY: header-args:latex+ :imoutoptions -quality 100 #+PROPERTY: header-args:latex+ :results file raw replace #+PROPERTY: header-args:latex+ :buffer no #+PROPERTY: header-args:latex+ :eval no-export #+PROPERTY: header-args:latex+ :exports results #+PROPERTY: header-args:latex+ :mkdirp yes #+PROPERTY: header-args:latex+ :output-dir figs #+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") :END: * Introduction :ignore: The control architecture we wish here to study is shown in Figure [[fig:cascade_control_architecture]]. #+begin_src latex :file cascade_control_architecture.pdf \begin{tikzpicture} % Blocs \node[block={3.0cm}{3.0cm}] (P) {Plant}; \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {}; \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, align=center, left= of Kx] (Ex) {Compute\\Pos. Error}; % Connections and labels \draw[->] (outputF) -- ++(1.0, 0); \draw[->] ($(outputF) + (0.6, 0)$)node[branch](taum){} node[below]{$\bm{\tau}_m$} |- (Kiff.east); \draw[->] (Kiff.west) -| (addF.north); \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$}; \draw[->] (outputL) -- ++(1.8, 0); \draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south); \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; \draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$}; \draw[->] (outputX) -- ++(2.6, 0); \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[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; \begin{scope}[on background layer] \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {}; \node[fit={(Pdamped.north-|J.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {}; \node[anchor={north west}] at (Plin.north west){$P_\text{lin}$}; \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {}; \node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$}; \end{scope} \end{tikzpicture} #+end_src #+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]] This cascade control is designed in three steps: - In section [[sec:lac_iff]]: an active damping controller is designed. This is based on the Integral Force Feedback and applied in a decentralized way - In section [[sec:hac_joint_space]]: a decentralized tracking control is designed in the frame of the legs. This controller is based on the displacement of each of the legs - In section [[sec:primary_controller]]: a controller is designed in the task space in order to follow the wanted reference path corresponding to the sample position with respect to the granite * Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no simulinkproject('../'); #+end_src #+begin_src matlab open('nass_model.slx') #+end_src * Initialization We initialize all the stages with the default parameters. #+begin_src matlab initializeGround(); initializeGranite(); initializeTy(); initializeRy(); initializeRz(); initializeMicroHexapod(); initializeAxisc(); initializeMirror(); #+end_src The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg. #+begin_src matlab initializeNanoHexapod('actuator', 'piezo'); initializeSample('mass', 1); #+end_src We set the references that corresponds to a tomography experiment. #+begin_src matlab initializeReferences('Rz_type', 'rotating', 'Rz_period', 1); #+end_src #+begin_src matlab initializeDisturbances(); #+end_src Open Loop. #+begin_src matlab initializeController('type', 'cascade-hac-lac'); #+end_src And we put some gravity. #+begin_src matlab initializeSimscapeConfiguration('gravity', true); #+end_src We log the signals. #+begin_src matlab initializeLoggingConfiguration('log', 'all'); #+end_src #+begin_src matlab Kx = tf(zeros(6)); Kl = tf(zeros(6)); Kiff = tf(zeros(6)); #+end_src * Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$ <> ** Identification Let's first identify the plant for the IFF controller. #+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 = linearize(mdl, io, 0); G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}; #+end_src ** Plant #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; ax1 = subplot(2, 2, 1); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(G_iff(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, 2, 3); 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 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'); ax3 = subplot(2, 2, 2); hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/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(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(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/cascade_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_iff_plant #+caption: IFF Plant ([[./figs/cascade_iff_plant.png][png]], [[./figs/cascade_iff_plant.pdf][pdf]]) [[file:figs/cascade_iff_plant.png]] ** Root Locus #+begin_src matlab :exports none gains = logspace(0, 4, 500); figure; hold on; plot(real(pole(G_iff)), imag(pole(G_iff)), 'x'); set(gca,'ColorOrderIndex',1); plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o'); for i = 1:length(gains) set(gca,'ColorOrderIndex',1); cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6))); plot(real(cl_poles), imag(cl_poles), '.'); end ylim([0, 2*pi*500]); xlim([-2*pi*500,0]); xlabel('Real Part') ylabel('Imaginary Part') axis square #+end_src #+header: :tangle no :exports results :results none :noweb yes #+begin_src matlab :var filepath="figs/cascade_iff_root_locus.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_iff_root_locus #+caption: Root Locus for the IFF control ([[./figs/cascade_iff_root_locus.png][png]], [[./figs/cascade_iff_root_locus.pdf][pdf]]) [[file:figs/cascade_iff_root_locus.png]] The maximum damping is obtained for a control gain of $\approx 3000$. ** 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_iff_loop_gain]]. #+begin_src matlab w0 = 2*pi*50; Kiff = -3000/s*eye(6); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 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 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 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/cascade_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_iff_loop_gain #+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_iff_loop_gain.png][png]], [[./figs/cascade_iff_loop_gain.pdf][pdf]]) [[file:figs/cascade_iff_loop_gain.png]] * High Authority Control in the joint space - $\bm{K}_\mathcal{L}$ <> ** Identification of the damped plant We now identify the transfer function from $\tau^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure [[fig:cascade_control_architecture]]. #+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 = linearize(mdl, io, 0); Gl.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; Gl.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}; #+end_src There are some unstable poles in the Plant with very small imaginary parts. These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant. #+begin_src matlab isstable(Gl) Gl = minreal(Gl); isstable(Gl) #+end_src ** Obtained Plant The obtain plant is shown in Figure [[fig:cascade_hac_joint_plant]]. We can see that the plant is quite well decoupled. #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; ax1 = subplot(2, 2, 1); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gl(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(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i)); 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(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gl(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(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(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/cascade_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_hac_joint_plant #+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_hac_joint_plant.png][png]], [[./figs/cascade_hac_joint_plant.pdf][pdf]]) [[file:figs/cascade_hac_joint_plant.png]] ** Controller Design and Loop Gain The controller consists of: - A pure integrator - A Second integrator up to half the wanted bandwidth - A Lead around the cross-over frequency - A low pass filter with a cut-off equal to two times the wanted bandwidth #+begin_src matlab wc = 2*pi*400; % Bandwidth Bandwidth [rad/s] h = 2; % Lead parameter % Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2)); Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/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)))); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 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 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 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/cascade_hac_joint_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_hac_joint_loop_gain #+caption: Loop Gain for the High Autority Control in the joint space ([[./figs/cascade_hac_joint_loop_gain.png][png]], [[./figs/cascade_hac_joint_loop_gain.pdf][pdf]]) [[file:figs/cascade_hac_joint_loop_gain.png]] #+begin_src matlab :exports none :tangle no isstable(feedback(Gl*Kl, eye(6), -1)) #+end_src * Primary Controller in the task space - $\bm{K}_\mathcal{X}$ <> ** 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 mdl = 'nass_model'; %% 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, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror %% Run the linearization Gx = linearize(mdl, io, 0); Gx.InputName = {'rL1', 'rL2', 'rL3', 'rL4', 'rL5', 'rL6'}; Gx.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; #+end_src As before, we take the minimum realization. #+begin_src matlab isstable(Gx) Gx = minreal(Gx); isstable(Gx) #+end_src ** Obtained Plant #+begin_src matlab :exports none freqs = logspace(0, 4, 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(Gx(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(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i}); 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(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gx(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(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(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/cascade_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_primary_plant #+caption: Plant for the Primary Controller ([[./figs/cascade_primary_plant.png][png]], [[./figs/cascade_primary_plant.pdf][pdf]]) [[file:figs/cascade_primary_plant.png]] ** Controller Design #+begin_src matlab wc = 2*pi*10; % Bandwidth Bandwidth [rad/s] 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); % Normalization of the gain of have a loop gain of 1 at frequency wc Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc)))); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(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(Gx(i, i)*Kx(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/cascade_primary_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_primary_loop_gain #+caption: Loop Gain for the primary controller (outer loop) ([[./figs/cascade_primary_loop_gain.png][png]], [[./figs/cascade_primary_loop_gain.pdf][pdf]]) [[file:figs/cascade_primary_loop_gain.png]] * 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 = simout; save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac'); #+end_src * Results #+begin_src matlab load('./mat/experiment_tomography.mat', 'tomo_align_dist'); load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac'); #+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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 1)) hold off; xlabel('Time [s]'); ylabel('Dx [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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 2)) hold off; xlabel('Time [s]'); ylabel('Dy [m]'); 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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 3)) hold off; xlabel('Time [s]'); ylabel('Dz [m]'); 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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 4)) hold off; xlabel('Time [s]'); ylabel('Rx [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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 5)) hold off; xlabel('Time [s]'); ylabel('Ry [rad]'); 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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 6), 'DisplayName', 'Cascade') hold off; xlabel('Time [s]'); ylabel('Rz [rad]'); legend(); linkaxes([ax1,ax2,ax3,ax4],'x'); xlim([0.5, inf]); #+end_src #+header: :tangle no :exports results :results none :noweb yes #+begin_src matlab :var filepath="figs/cascade_hac_lac_tomography.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src #+name: fig:cascade_hac_lac_tomography #+caption: Results of the Tomography Experiment ([[./figs/cascade_hac_lac_tomography.png][png]], [[./figs/cascade_hac_lac_tomography.pdf][pdf]]) [[file:figs/cascade_hac_lac_tomography.png]]