%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');



% #+name: fig:classical_feedback_test_system_bis
% #+caption: Block diagram corresponding to the example system
% #+RESULTS:
% [[file:figs/ex_test_system_feedback.png]]

% The two transfer functions present in the system are derived and defined below:

k = 1e6; % Stiffness [N/m]
c = 4e2; % Damping [N/(m/s)]
m = 10; % Mass [kg]

% Control Plant
G = 1/(m*s^2 + c*s + k);
% Disturbance dynamics
Gd = (c*s + k)/(m*s^2 + c*s + k);



% We also define the inputs signals that will be used for time domain simulations.
% They are graphically shown in Figure [[fig:ex_inputs_signals]].

% Time Vector
t = 0:1e-4:0.5;

% Reference Input
r = zeros(size(t));
r(t>0.05 & t<=0.25) = 1e-4*(t(t>0.05 & t<=0.25)-0.05);
r(t>0.25) = 2e-5;

% Measurement Noise
Fs = 1e3; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
n = sqrt(Fs/2)*randn(1, length(t)); % Signal with an ASD equal to one
n = lsim(1e-6*(s + 2*pi*1e2)^2/(s + 2*pi*1e3)^2/(1+s/2/pi/500), n, t)'; % Shaped noise

% Disturbance
d = zeros(size(t));
d(t>0.3) = 5e-6;

figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');

nexttile;
plot(t, 1e6*r, 'k-')
xlabel('Time [s]');
ylabel('Reference Signal $[\mu m]$');

nexttile;
plot(t, 1e6*n, 'k-')
xlabel('Time [s]');
ylabel('Measurement Noise $[\mu m]$');

nexttile;
plot(t, 1e6*d, 'k-')
xlabel('Time [s]');
ylabel('Disturbance Motion $[\mu m]$');



% #+name: fig:ex_general_plant_sim
% #+caption: Generalized plant that will be used for simulations
% #+RESULTS:
% [[file:figs/ex_general_plant_sim.png]]

% The Generalized plant of Figure [[fig:ex_general_plant_sim]] is defined on Matlab as follows:

Psim = [0  0  Gd  G
        0  0  0   1
        1 -1 -Gd -G];

Psim.InputName = {'r', 'n', 'd', 'u'};
Psim.OutputName = {'y', 'u', 'e'};

% Modern Interpretation of control specifications
% <<sec:ex_specification_interpretation>>

% #+begin_exercice
% 1. Translate the control specifications into wanted shape of closed-loop transfer functions
% 2. Conclude and the closed-loop transfer functions to be shaped
% 3. Chose a general configuration architecture that allows to shape these transfer function
% 4. Using Matlab, define the generalized plant

% #+HTML: <details><summary>Hint</summary>
% #+LATEX: \tcbsubtitle{Hint}
% 1. Make use of Table [[tab:usual_shaping_gang_four]]
% 2. Make use of Table [[tab:usual_shaping_gang_four]]
% 3. See Section [[sec:shaping_multiple_tf]]
% 4. See Section [[sec:shaping_multiple_tf]]
% #+HTML: </details>
% #+end_exercice

% After converting the control specifications into wanted shape of closed-loop transfer functions, we might come up with the Table [[tab:ex_specification_shapes]].

% In such case, we want to shape $S$, $GS$ and $T$.

% #+name: tab:ex_specification_shapes
% #+caption: Control Specifications and associated wanted shape of the closed-loop transfer functions
% #+attr_latex: :environment tabularx :width 0.7\linewidth :align llX
% | <l>                      |    <c>    | <l>                                |
% | *Specification*          |   *TF*    | *Wanted Shape*                     |
% |--------------------------+-----------+------------------------------------|
% | Follow Step Reference    |    $S$    | +40dB of slope at low frequency    |
% | Reject Disturbances      | $S$, $GS$ | Small gain                         |
% | Reject measurement noise |    $T$    | Small high frequency (>100Hz) gain |
% | Robust System            |    $S$    | Small $\Vert S \Vert_\infty$       |

% To do so, we use to generalized plant shown in Figure [[fig:ex_general_plant]] for the synthesis where the three closed-loop tranfert functions from $w$ to $[z_1\,,z_2\,,z_3]$ are respectively $S$, $GS$ and $T$.

% This generalized plant is defined on Matlab as follows:

P = [1 -1
     G -G
     0  1
     G -G];

% Step 1 - Shaping of $S$
% <<sec:ex_shaping_S>>

% Let's first shape the Sensitivity function as it is usually the most important of the /Gang of four/ closed-loop transfer functions.
% To do so, we have to create a weight $W_1(s)$ that defines the wanted upper bound on $|S(j\omega)|$:
% - small low frequency gain: $|S(j\cdot 0)| = 10^{-3}$
% - minimum crossover frequency of $\approx 10Hz$: $|S(j2\pi 10)| < \frac{1}{\sqrt{2}}$
% - small maximum peak magnitude for robustness properties: $\|S\|_\infty < 2$

% The weighting function is design using the =generateWeigh= function and its inverse shape can be seen in Figure

W1 = generateWeight('G0', 1e3, ...
                    'G1', 1/2, ...
                    'Gc', sqrt(2), 'wc', 2*pi*10, ...
                    'n', 1);



% To not constrain $GS$ and $T$ for the shaping of $S$, $W_2$ and $W_3$ are first taken as very small gains:

W2 = tf(1e-8);
W3 = tf(1e-8);



% The $\mathcal{H}_\infty$ synthesis is performed and the obtained closed-loop transfer functions $S$, $GS$, and $T$ and compared with the upper bounds set by the weighting functions in Figure [[fig:ex_results_1]].


Pw = blkdiag(W1, W2, W3, 1)*P;
K1 = hinfsyn(Pw, 1, 1, 'Display', 'on');



% #+RESULTS:
% #+begin_example
%   Test bounds:  0.5 <=  gamma  <=  0.51

%    gamma        X>=0        Y>=0       rho(XY)<1    p/f
%   5.05e-01     0.0e+00     0.0e+00     5.511e-14     p
%   Limiting gains...
%   5.05e-01     0.0e+00     0.0e+00     1.867e-14     p

%   Best performance (actual): 0.502
% #+end_example


% Closed-loop tranfer functions are computed
Z1 = lft(P, K1);

freqs = logspace(-1, 3, 1000);

figure;
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1');
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W|^{-1}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('S');
legend('location', 'southeast');

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(2,1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('GS');

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(3,1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('T');

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(K1, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('K');

linkaxes([ax1, ax2, ax3, ax4], 'x')
xlim([freqs(1), freqs(end)]);



% #+name: fig:ex_results_1
% #+caption: Obtained Shape Closed-Loop transfer functions (dashed black lines indicate inverse magnitude of the weighting functions)
% #+RESULTS:
% [[file:figs/ex_results_1.png]]

% Time domain simulation is then performed and the obtained output displacement and control inputs are shown in Figure [[fig:ex_time_domain_1]].

% We can see:
% - we are not able to follow the ramp input. This have to be solved by modifying the weighting function $W_1(s)$
% - we have poor rejection of disturbances. This we be solve by shaping $GS$ in Section [[sec:ex_shaping_GS]]
% - we have quite large effect of the measurement noise. This will be solved by shaping $T$ in Section [[sec:ex_shaping_T]]


% Closed-loop simulation
Z1sim = lft(Psim, K1);
z = lsim(Z1sim, [r;n;d], t);
y1 = z(:,1);
u1 = z(:,2);

figure;
tiledlayout(1, 1, 'TileSpacing', 'None', 'Padding', 'None');

nexttile;
hold on;
plot(t, 1e6*y1, 'DisplayName', 'Step 1');
plot(t, 1e6*r, 'k--', 'DisplayName', '$r$');
hold off;
xlabel('Time [s]'); ylabel('Output $y$ [$\mu m$]');
legend('location', 'southeast');



% #+name: fig:ex_time_domain_1
% #+caption: Time domain simulation results
% #+RESULTS:
% [[file:figs/ex_time_domain_1.png]]

% Remember that in order to follow ramp inputs, the sensitivity function should have a slope of +40dB/dec at low frequency (Table [[tab:usual_shaping_gang_four]]).

% To do so, let's modify $W_1$ to impose a slope of +40dB/dec at low frequency.
% This can simple be done by using a second order weight:

W1 = generateWeight('G0', 1e3, ...
                    'G1', 1/2, ...
                    'Gc', sqrt(2), 'wc', 2*pi*15, ...
                    'n', 2);



% The $\mathcal{H}_\infty$ synthesis is performed using the new weights and the obtained closed-loop shaped are shown in figure [[fig:ex_results_1b]].

% The time domain signals are shown in Figure [[fig:ex_time_domain_1b]] and it is confirmed that the ramps are now follows without static errors.


Pw = blkdiag(W1, W2, W3, 1)*P;
K1b = hinfsyn(Pw, 1, 1, 'Display', 'on');

% Compute the closed-loop transfer function
Z1b = lft(P, K1b);

freqs = logspace(-1, 3, 1000);

figure;
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1');
plot(freqs, abs(squeeze(freqresp(Z1b(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1b');
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W^{-1}|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('S');
legend('location', 'southeast');

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(2,1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('GS');

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(3,1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('T');

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(K1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K1b, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('K');

linkaxes([ax1, ax2, ax3, ax4], 'x')
xlim([freqs(1), freqs(end)]);



% #+name: fig:ex_results_1b
% #+caption: Obtained Shape Closed-Loop transfer functions
% #+RESULTS:
% [[file:figs/ex_results_1b.png]]


% Closed-loop simulations
Z1bsim = lft(Psim, K1b);
z = lsim(Z1bsim, [r;n;d], t);
y1b = z(:,1);
u1b = z(:,2);

figure;
tiledlayout(1, 1, 'TileSpacing', 'None', 'Padding', 'None');
hold on;
plot(t, 1e6*y1, 'DisplayName', 'Step 1');
plot(t, 1e6*y1b, 'DisplayName', 'Step 1b');
plot(t, 1e6*r, 'k--', 'DisplayName', '$r$');
hold off;
xlabel('Time [s]'); ylabel('Output $y$ [$\mu m$]');
legend('location', 'southeast');

% Step 2 - Shaping of $GS$
% <<sec:ex_shaping_GS>>

% Looking at Figure [[fig:ex_results_2]], it is clear that the rejection of disturbances is not satisfactory.
% This can also be seen by the large peak of $GS$ in Figure [[fig:ex_results_1b]].

% This poor rejection of disturbances is actually due to the fact that the obtain controller has at notch at the resonance frequency of the plant.


% To overcome this issue, we can simply increase the magnitude of $W_2$ to limit the peak magnitude of $GS$
% Let's take $W_2$ as a simple constant gain:

W2 = tf(4e5);



% The $\mathcal{H}_\infty$ Synthesis is performed and the obtained closed-loop transfer functions are shown in Figure [[fig:ex_results_2]].

Pw = blkdiag(W1, W2, W3, 1)*P;
K2 = hinfsyn(Pw, 1, 1, 'Display', 'on');

Z2 = lft(P, K2);

freqs = logspace(-1, 3, 1000);

figure;
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1');
plot(freqs, abs(squeeze(freqresp(Z1b(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1b');
plot(freqs, abs(squeeze(freqresp(Z2(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 2');
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W^{-1}|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('S');
legend('location', 'southeast');

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z2(2,1), freqs, 'Hz'))));
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('GS');

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z2(3,1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('T');

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(K1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K1b, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('K');

linkaxes([ax1, ax2, ax3, ax4], 'x')
xlim([freqs(1), freqs(end)]);



% #+name: fig:ex_results_2
% #+caption: Obtained Shape Closed-Loop transfer functions
% #+RESULTS:
% [[file:figs/ex_results_2.png]]

% Time domain simulation results are shown in Figure [[fig:ex_time_domain_2]].
% If is shown that indeed, the disturbance rejection performance are much better and only very small oscillation is obtained.


Z2sim = lft(Psim, K2);

z = lsim(Z2sim, [r;n;d], t);
y2 = z(:,1);
u2 = z(:,2);

figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');

hold on;
plot(t, 1e6*y1, 'DisplayName', 'Step 1');
plot(t, 1e6*y1b, 'DisplayName', 'Step 1b');
plot(t, 1e6*y2, 'DisplayName', 'Step 2');
plot(t, 1e6*r, 'k--', 'DisplayName', '$r$');
hold off;
xlabel('Time [s]'); ylabel('Output $y$ [$\mu m$]');
legend('location', 'southeast');

% Step 3 - Shaping of $T$
% <<sec:ex_shaping_T>>

% Finally, we want to limit the effect of the noise on the displacement output.

% To do so, $T$ is shaped such that its high frequency gain is reduced.

% This is done by increasing the high frequency gain of the weighting function $W_3$ until the $\mathcal{H}_\infty$ synthesis gives $\gamma \approx 1$.

% The final weighting function $W_3$ is defined as follows:

W3 = generateWeight('G0', 1e-1, ...
                    'G1', 1e4, ...
                    'Gc', 1, 'wc', 2*pi*70, ...
                    'n', 2);



% The $\mathcal{H}_\infty$ synthesis is performed and $\gamma$ is closed to one.

Pw = blkdiag(W1, W2, W3, 1)*P;

K3 = hinfsyn(Pw, 1, 1, 'Display', 'on');



% #+RESULTS:
% #+begin_example
%   Test bounds:  0.622 <=  gamma  <=  1.62

%    gamma        X>=0        Y>=0       rho(XY)<1    p/f
%   1.01e+00    -1.3e+07 #  -6.4e-18     3.459e-04     f
%   1.28e+00     0.0e+00     0.0e+00     5.369e-02     p
%   1.13e+00     3.8e-15     0.0e+00     1.695e-01     p
%   1.07e+00    -5.0e-08    -1.2e-19     1.622e+00 #   f
%   1.10e+00     0.0e+00     0.0e+00     3.147e-01     p
%   1.08e+00     0.0e+00     0.0e+00     5.327e-01     p
%   1.08e+00     3.4e-15     0.0e+00     8.052e-01     p

%   Best performance (actual): 1.08
% #+end_example

% The obtained closed-loop transfer functions are shown in Figure [[fig:ex_results_3]] and we can obverse that:
% - The high frequency gain of $T$ is indeed reduced
% - This comes as the expense of a large magnitude both $GS$ and $S$.
%   This means we will probably have slightly less good disturbance rejection and tracking performances.


Z3 = lft(P, K3);

freqs = logspace(-1, 3, 1000);

figure;
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1');
plot(freqs, abs(squeeze(freqresp(Z1b(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 1b');
plot(freqs, abs(squeeze(freqresp(Z2(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 2');
plot(freqs, abs(squeeze(freqresp(Z3(1,1), freqs, 'Hz'))), 'DisplayName', 'Step 3');
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W^{-1}|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('S');
legend('location', 'southeast');

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z2(2,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z3(2,1), freqs, 'Hz'))));
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('GS');

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(Z1(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z1b(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z2(3,1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Z3(3,1), freqs, 'Hz'))));
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('T');

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(K1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K1b, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(K3, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
title('K');

linkaxes([ax1, ax2, ax3, ax4], 'x')
xlim([freqs(1), freqs(end)]);



% #+name: fig:ex_results_3
% #+caption: Obtained Shape Closed-Loop transfer functions
% #+RESULTS:
% [[file:figs/ex_results_3.png]]

% The time domain simulation signals are shown in Figure [[fig:ex_time_domain_3]].
% We can indeed see a slightly less good disturbance rejection.
% However, the vibrations induced by the sensor noise is well reduced.
% This can be seen when zooming on the output signal in Figure [[fig:ex_time_domaim_3_zoom]].


Z3sim = lft(Psim, K3);

z = lsim(Z3sim, [r;n;d], t);
y3 = z(:,1);
u3 = z(:,2);

figure;
tiledlayout(1, 1, 'TileSpacing', 'None', 'Padding', 'None');
hold on;
plot(t, 1e6*y1, 'DisplayName', 'Step 1');
plot(t, 1e6*y1b, 'DisplayName', 'Step 1b');
plot(t, 1e6*y2, 'DisplayName', 'Step 2');
plot(t, 1e6*y3, 'DisplayName', 'Step 3');
plot(t, 1e6*r, 'k--', 'DisplayName', '$r$');
hold off;
xlabel('Time [s]'); ylabel('Output $y$ [$\mu m$]');
legend('location', 'southeast');



% #+name: fig:ex_time_domain_3
% #+caption: Time domain simulation results
% #+RESULTS:
% [[file:figs/ex_time_domain_3.png]]


figure;
hold on;
plot(t, 1e6*y1, 'DisplayName', 'Step 1');
plot(t, 1e6*y1b, 'DisplayName', 'Step 1b');
plot(t, 1e6*y2, 'DisplayName', 'Step 2');
plot(t, 1e6*y3, 'DisplayName', 'Step 3');
plot(t, 1e6*r, 'k--', 'DisplayName', '$r$');
hold off;
xlabel('Time [s]'); ylabel('Output $y$ [$\mu m$]');
legend('location', 'southeast');
xlim([0.4, 0.5])