diff --git a/docs/figs/decentralized_L_position_errors.pdf b/docs/figs/decentralized_L_position_errors.pdf new file mode 100644 index 0000000..b23284a Binary files /dev/null and b/docs/figs/decentralized_L_position_errors.pdf differ diff --git a/docs/figs/decentralized_L_position_errors.png b/docs/figs/decentralized_L_position_errors.png new file mode 100644 index 0000000..a444cd8 Binary files /dev/null and b/docs/figs/decentralized_L_position_errors.png differ diff --git a/docs/figs/decentralized_control_L_loop_gain.pdf b/docs/figs/decentralized_control_L_loop_gain.pdf new file mode 100644 index 0000000..0cd2f3c Binary files /dev/null and b/docs/figs/decentralized_control_L_loop_gain.pdf differ diff --git a/docs/figs/decentralized_control_L_loop_gain.png b/docs/figs/decentralized_control_L_loop_gain.png new file mode 100644 index 0000000..42be73e Binary files /dev/null and b/docs/figs/decentralized_control_L_loop_gain.png differ diff --git a/docs/figs/decentralized_control_plant_L.pdf b/docs/figs/decentralized_control_plant_L.pdf new file mode 100644 index 0000000..07511f2 Binary files /dev/null and b/docs/figs/decentralized_control_plant_L.pdf differ diff --git a/docs/figs/decentralized_control_plant_L.png b/docs/figs/decentralized_control_plant_L.png new file mode 100644 index 0000000..28c084d Binary files /dev/null and b/docs/figs/decentralized_control_plant_L.png differ diff --git a/docs/figs/decentralized_reference_tracking_L.pdf b/docs/figs/decentralized_reference_tracking_L.pdf new file mode 100644 index 0000000..da20d1e Binary files /dev/null and b/docs/figs/decentralized_reference_tracking_L.pdf differ diff --git a/docs/figs/decentralized_reference_tracking_L.png b/docs/figs/decentralized_reference_tracking_L.png new file mode 100644 index 0000000..f1d4020 Binary files /dev/null and b/docs/figs/decentralized_reference_tracking_L.png differ diff --git a/org/control_decentralized.org b/org/control_decentralized.org new file mode 100644 index 0000000..eeca927 --- /dev/null +++ b/org/control_decentralized.org @@ -0,0 +1,888 @@ +#+TITLE: Control in the Frame of the Legs 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: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: +In this document, we apply some decentralized control to the NASS and see what level of performance can be obtained. + +* Decentralized Control +** Matlab Init :noexport: +#+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 + simulinkproject('../'); +#+end_src + +#+begin_src matlab + open('nass_model.slx'); +#+end_src + +** Control Schematic +The control architecture is shown in Figure [[fig:decentralized_reference_tracking_L]]. + +The signals are: +- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite +- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod +- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs +- $\bm{\tau}$: forces applied in each actuator +- $\bm{\mathcal{L}}$: measured displacement of each leg +- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite + +#+begin_src latex :file decentralized_reference_tracking_L.pdf + \begin{tikzpicture} + % Blocs + \node[block={2.0cm}{2.0cm}] (P) {}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputX) at ($(P.south east)!0.7!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.3!(P.north east)$); + + \node[block, left= of inputF] (K) {$\bm{K}_\mathcal{L}$}; + \node[addb={+}{}{}{}{-}, left= of K] (subr) {}; + \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics}; + + \node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error}; + + % Connections and labels + \draw[->] (outputL) -- ++(1, 0) node[above left]{$\bm{\mathcal{L}}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south); + \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$}; + \draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$}; + + \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$}; + \draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south); + + \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$}; + \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$}; + \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0); + + % \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$}; + % \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$}; + % \draw[->] (G.east) node[above right]{$\bm{\mathcal{L}}$} -| ($(G.east)+(1, -1)$) -| (subr.south); + \end{tikzpicture} +#+end_src + +#+name: fig:decentralized_reference_tracking_L +#+caption: Decentralized control for reference tracking +#+RESULTS: +[[file:figs/decentralized_reference_tracking_L.png]] + +** Initialize the Simscape Model +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', 'ref-track-L'); + Kl = tf(zeros(6)); +#+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 + +** Identification of the plant +Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{L}}$. +#+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, '/Controller/Reference-Tracking-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error + + %% Run the linearization + G = linearize(mdl, io, 0); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'}; +#+end_src + +** Plant Analysis +The diagonal and off-diagonal terms of the plant are shown in Figure [[fig:decentralized_control_plant_L]]. + +We can see that: +- the diagonal terms have similar dynamics +- the plant is decoupled at low frequency + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(G(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(G(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('location', 'northwest'); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(G(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(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(G(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/decentralized_control_plant_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:decentralized_control_plant_L +#+caption: Transfer Functions from forces applied in each actuator $\tau_i$ to the relative motion of each leg $d\mathcal{L}_i$ ([[./figs/decentralized_control_plant_L.png][png]], [[./figs/decentralized_control_plant_L.pdf][pdf]]) +[[file:figs/decentralized_control_plant_L.png]] + +** Controller Design +The controller consists of: +- A pure integrator +- An integrator up to little before the crossover +- A lead around the crossover +- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin + +The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:decentralized_control_L_loop_gain]]. + +#+begin_src matlab + wc = 2*pi*20; + h = 1.5; + + Kl = diag(1./diag(abs(freqresp(G, wc)))) * ... + wc/s * ... % Pure Integrator + ((s/wc*2 + 1)/(s/wc*2)) * ... % Integrator up to wc/2 + 1/h * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead + 1/(1 + s/3/wc) * ... % Low pass Filter + 1/(1 + s/3/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(Kl(i, i)*G(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(Kl(i, i)*G(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/decentralized_control_L_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:decentralized_control_L_loop_gain +#+caption: Obtained Loop Gain ([[./figs/decentralized_control_L_loop_gain.png][png]], [[./figs/decentralized_control_L_loop_gain.pdf][pdf]]) +[[file:figs/decentralized_control_L_loop_gain.png]] + +#+begin_src matlab :exports none :tangle no + isstable(feedback(G*Kl, eye(6), -1)) +#+end_src + +We add a minus sign to the controller as it is not included in the Simscape model. +#+begin_src matlab + Kl = -Kl; +#+end_src + +** Simulation +#+begin_src matlab + initializeController('type', 'ref-track-L'); +#+end_src + +#+begin_src matlab + load('mat/conf_simulink.mat'); + set_param(conf_simulink, 'StopTime', '2'); +#+end_src + +#+begin_src matlab + sim('nass_model'); +#+end_src + +#+begin_src matlab + decentralized_L = simout; + save('./mat/tomo_exp_decentalized.mat', 'decentralized_L', '-append'); +#+end_src + +** Results +The reference path and the position of the mobile platform are shown in Figure [[fig:decentralized_L_position_errors]]. + +#+begin_src matlab + load('./mat/experiment_tomography.mat', 'tomo_align_dist'); + load('./mat/tomo_exp_decentalized.mat', 'decentralized_L'); +#+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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.Em.En.Data(:, 6), 'DisplayName', 'HAC-DVF') + hold off; + xlabel('Time [s]'); + ylabel('Rz [rad]'); + legend(); + + linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); + xlim([0.5, inf]); +#+end_src + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/decentralized_L_position_errors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:decentralized_L_position_errors +#+caption: Position Errors when using the Decentralized Control Architecture ([[./figs/decentralized_L_position_errors.png][png]], [[./figs/decentralized_L_position_errors.pdf][pdf]]) +[[file:figs/decentralized_L_position_errors.png]] + +* HAC-LAC (IFF) Decentralized Control +** Introduction :ignore: +We here add an Active Damping Loop (Integral Force Feedback) prior to using the Decentralized control architecture using $\bm{\mathcal{L}}$. + +** Control Schematic +The control architecture is shown in Figure [[fig:decentralized_reference_tracking_L]]. + +The signals are: +- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite +- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod +- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs +- $\bm{\tau}$: forces applied in each actuator +- $\bm{\mathcal{L}}$: measured displacement of each leg +- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite + +#+begin_src latex :file decentralized_reference_tracking_iff_L.pdf + \begin{tikzpicture} + % Blocs + \node[block={3.0cm}{3.0cm}] (P) {}; + \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= 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, align=center, left= of J] (Ex) {Compute\\Pos. Error}; + + % Connections and labels + \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.north); + \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$}; + + \draw[->] (outputL) -- ++(1, 0) node[above left]{$\bm{\mathcal{L}}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south); + \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$}; + \draw[->] (K.east) -- (addF.west); + + \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$}; + \draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south); + + \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$}; + \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$}; + \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0); + \end{tikzpicture} +#+end_src + +#+name: fig:decentralized_reference_tracking_L +#+caption: Decentralized control for reference tracking +#+RESULTS: +[[file:figs/decentralized_reference_tracking_L.png]] + +** 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 + simulinkproject('../'); +#+end_src + +#+begin_src matlab + open('nass_model.slx'); +#+end_src + +** Initialize the Simscape Model +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', 'ref-track-L'); + Kl = tf(zeros(6)); +#+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 + +** Initialization +#+begin_src matlab + initializeController('type', 'ref-track-iff-L'); + K_iff = tf(zeros(6)); + Kl = tf(zeros(6)); +#+end_src + +** Identification for IFF +#+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 + +#+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(G_iff(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); + + 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 + 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', 'southwest'); + + linkaxes([ax1,ax2],'x'); +#+end_src + +** Integral Force Feedback Controller +#+begin_src matlab + w0 = 2*pi*50; + K_iff = -5000/s * (s/w0)/(1 + s/w0) * 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(K_iff(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(K_iff(i,i)*G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$L_{\\tau,%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', 'southwest'); + + linkaxes([ax1,ax2],'x'); +#+end_src + +#+begin_src matlab + K_iff = -K_iff; +#+end_src + +** Identification of the damped plant +#+begin_src matlab + %% Name of the Simulink DehaezeFile + 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, '/Controller/Reference-Tracking-IFF-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error + + %% Run the linearization + Gd = linearize(mdl, io, 0); + Gd.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gd.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'}; +#+end_src + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(G( i, i), freqs, 'Hz')))); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gd(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 + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(G( i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i)); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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'); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + plot(freqs, abs(squeeze(freqresp(Gd(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gd(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(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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 + + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(G(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(G(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('location', 'northwest'); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(G(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(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(G(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 + +** Controller Design +#+begin_src matlab + wc = 2*pi*300; + h = 3; + + Kl = diag(1./diag(abs(freqresp(Gd, wc)))) * ... + ((s/(2*pi*20) + 1)/(s/(2*pi*20))) * ... % Pure Integrator + ((s/(2*pi*50) + 1)/(s/(2*pi*50))) * ... % Integrator up to wc/2 + 1/h * (1 + s/wc*h)/(1 + s/wc/h) * ... + 1/(1 + s/(2*wc)) * ... + 1/(1 + s/(3*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(Kl(i, i)*Gd(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(Kl(i, i)*Gd(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 + +#+begin_src matlab + isstable(feedback(Gd*Kl, eye(6), -1)) +#+end_src + +#+begin_src matlab + Kl = -Kl; +#+end_src + + +** Simulation +#+begin_src matlab + initializeController('type', 'ref-track-iff-L'); +#+end_src + +#+begin_src matlab + load('mat/conf_simulink.mat'); + set_param(conf_simulink, 'StopTime', '2'); +#+end_src + +#+begin_src matlab + sim('nass_model'); +#+end_src + +#+begin_src matlab + decentralized_iff_L = simout; + save('./mat/tomo_exp_decentalized.mat', 'decentralized_iff_L', '-append'); +#+end_src + +** Results +#+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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.Em.En.Data(:, 6), 'DisplayName', 'IFF + Decentralized') + hold off; + xlabel('Time [s]'); + ylabel('Rz [rad]'); + legend(); + + linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); + xlim([0.5, inf]); +#+end_src +* Conclusion