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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/';

% Simulink Model name
mdl = 'nass_model';

%% Colors for the figures
colors = colororder;

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);

% IFF Plant
% <<ssec:nass_active_damping_plant>>

% Transfer functions from actuator forces $f_i$ to force sensor measurements $f_{mi}$ are computed using the multi-body model.
% Figure ref:fig:nass_iff_plant_effect_kp examines how parallel stiffness affects the plant dynamics, with identification performed at maximum spindle velocity $\Omega_z = 360\,\text{deg/s}$ and with a payload mass of 25 kg.

% Without parallel stiffness (Figure ref:fig:nass_iff_plant_no_kp), the dynamics exhibits non-minimum phase zeros at low frequency, confirming predictions from the three-degree-of-freedom rotating model.
% Adding parallel stiffness (Figure ref:fig:nass_iff_plant_kp) transforms these into minimum phase complex conjugate zeros, enabling unconditionally stable decentralized IFF implementation.

% Though both cases show significant coupling around resonances, stability is guaranteed by the collocated arrangement of actuators and sensors [[cite:&preumont08_trans_zeros_struc_contr_with]].


%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput');             io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/NASS'],       3, 'openoutput', [], 'fn');  io_i = io_i + 1; % Force Sensors [N]

% Identify for multi payload masses (no rotation)
initializeReferences(); % No Spindle Rotation
% 1kg Sample
initializeSample('type', 'cylindrical', 'm', 1);
G_iff_m1 = linearize(mdl, io);
G_iff_m1.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m1.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% 25kg Sample
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25 = linearize(mdl, io);
G_iff_m25.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% 50kg Sample
initializeSample('type', 'cylindrical', 'm', 50);
G_iff_m50 = linearize(mdl, io);
G_iff_m50.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m50.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% Effect of Rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25_Rz = linearize(mdl, io, 0.1);
G_iff_m25_Rz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

% Effect of Rotation - No added parallel stiffness
initializeSimplifiedNanoHexapod('actuator_kp', 0);
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s
initializeSample('type', 'cylindrical', 'm', 25);
G_iff_m25_Rz_no_kp = linearize(mdl, io, 0.1);
G_iff_m25_Rz_no_kp.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_iff_m25_Rz_no_kp.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};

%% IFF Plant - Without parallel stiffness
f = logspace(-1,3,1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz')))), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

linkaxes([ax1,ax2],'x');
xlim([f(1), f(end)]);

%% IFF Plant - With added parallel stiffness
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $k_p = 50N/mm$')
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$f_{ni}/f_j$ - $k_p = 50N/mm$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

linkaxes([ax1,ax2],'x');
xlim([f(1), f(end)]);



% #+name: fig:nass_iff_plant_effect_kp
% #+caption: Effect of stiffness parallel to the force sensor on the IFF plant with $\Omega_z = 360\,\text{deg/s}$ and payload mass of 25kg. The dynamics without parallel stiffness has non-minimum phase zeros at low frequency (\subref{fig:nass_iff_plant_no_kp}). The added parallel stiffness transforms the non-minimum phase zeros to complex conjugate zeros (\subref{fig:nass_iff_plant_kp})
% #+attr_latex: :options [h!tbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_no_kp}without parallel stiffness}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_iff_plant_no_kp.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_kp}with parallel stiffness}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_iff_plant_kp.png]]
% #+end_subfigure
% #+end_figure

% The effect of rotation, shown in Figure ref:fig:nass_iff_plant_effect_rotation, is negligible as the actuator stiffness ($k_a = 1\,N/\mu m$) is large compared to the negative stiffness induced by gyroscopic effects (estimated from the 3DoF rotating model).

% Figure ref:fig:nass_iff_plant_effect_payload illustrate the effect of payload mass on the plant dynamics.
% While the poles and zeros are shifting with payload mass, the alternating pattern of poles and zeros is maintained, ensuring that the phase remains bounded between 0 and 180 degrees, and thus good robustness properties.


%% Effect of spindle's rotation on the IFF Plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$ deg/s')
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
% plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
%      'DisplayName', '$f_{ni}/f_j$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:));
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

%% Effect of the payload's mass on the IFF Plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 1kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 25kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '$f_{ni}/f_i$ - 50kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([0:45:180]);

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

% Controller Design
% <<ssec:nass_active_damping_control>>

% Previous analysis using the 3DoF rotating model showed that decentralized Integral Force Feedback (IFF) with pure integrators is unstable due to gyroscopic effects caused by spindle rotation.
% This finding is also confirmed with the multi-body model of the NASS: the system is unstable when using pure integrators and without parallel stiffness.

% This instability can be mitigated by introducing sufficient stiffness in parallel with the force sensors.
% However, as illustrated in Figure ref:fig:nass_iff_plant_kp, adding parallel stiffness increases the low frequency gain.
% If using pure integrators, this would results in high loop gain at low frequencies, adversely affecting the damped plant dynamics, which is undesirable.
% To resolve this issue, a second-order high-pass filter is introduced to limit the low frequency gain, as shown in Equation eqref:eq:nass_kiff.

% \begin{equation}\label{eq:nass_kiff}
%   \bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix}
%       K_{\text{IFF}}(s) & & 0 \\
%       & \ddots & \\
%       0 & & K_{\text{IFF}}(s)
%   \end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} \cdot \frac{\frac{s^2}{\omega_z^2}}{\frac{s^2}{\omega_z^2} + 2 \xi_z \frac{s}{\omega_z} + 1}
% \end{equation}

% The cut-off frequency of the second-order high-pass filter is tuned to be below the frequency of the complex conjugate zero for the highest mass, which is at $5\,\text{Hz}$.
% The overall gain is then increased to have large loop gain around resonances to be damped, as illustrated in Figure ref:fig:nass_iff_loop_gain.


%% Verify that parallel stiffness permits to have a stable plant
Kiff_pure_int = -200/s*eye(6);
isstable(feedback(G_iff_m25_Rz,       Kiff_pure_int, 1))
isstable(feedback(G_iff_m25_Rz_no_kp, Kiff_pure_int, 1))

%% IFF Controller Design
% Second order high pass filter
wz = 2*pi*2;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);

Kiff = -200 * ...              % Gain
       1/(0.01*2*pi + s) * ... % LPF: provides integral action
       Ghpf * ...              % 2nd order HPF (limit low frequency gain)
       eye(6);                 % Diagonal 6x6 controller (i.e. decentralized)

Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};

% The designed IFF controller is saved
save('./mat/nass_K_iff.mat', 'Kiff');

%% Loop gain for the decentralized IFF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '1kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '25kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '50kg')
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-4, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-110, 200]);
yticks([-180, -90, 0, 90, 180]);

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



% #+name: fig:nass_iff_loop_gain
% #+caption: Loop gain for the decentralized IFF: $K_{\text{IFF}}(s) \cdot \frac{f_{mi}}{f_i}(s)$
% #+attr_latex: :options [h!tbp]
% #+RESULTS:
% [[file:figs/nass_iff_loop_gain.png]]

% To verify stability, root loci for the three payload configurations are computed and shown in Figure ref:fig:nass_iff_root_locus.
% The results demonstrate that the closed-loop poles remain within the left-half plane, indicating the robust stability properties of the applied decentralized IFF.


%% Root Locus for the Decentralized IFF controller - 1kg Payload
figure;
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m1)),  imag(pole(G_iff_m1)),  'x', 'color', colors(1,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m1)), imag(tzero(G_iff_m1)), 'o', 'color', colors(1,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m1, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m1, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

%% Root Locus for the Decentralized IFF controller - 25kg Payload
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m25)),  imag(pole(G_iff_m25)),  'x', 'color', colors(2,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m25)), imag(tzero(G_iff_m25)), 'o', 'color', colors(2,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m25, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m25, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

%% Root Locus for the Decentralized IFF controller - 50kg Payload
gains = logspace(-2, 1, 200);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;

plot(real(pole(G_iff_m50)),  imag(pole(G_iff_m50)),  'x', 'color', colors(3,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(G_iff_m50)), imag(tzero(G_iff_m50)), 'o', 'color', colors(3,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_m50, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(G_iff_m50, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');

xline(0);
yline(0);
hold off;
axis equal;
xlim([-900, 100]); ylim([-100, 900]);
xticks([-900:100:0]);
yticks([0:100:900]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');