UP | HOME

# Matlab Computation
Author: Thomas Dehaeze

## Table of Contents

- 1. System Description and Analysis
  - 1.1. System description
  - 1.2. Equations
  - 1.3. Numerical Values
  - 1.4. Campbell Diagram
  - 1.5. Simscape Model
  - 1.6. Comparison of the Analytical Model and the Simscape Model
  - 1.7. Effect of the rotation speed
- 2. Problem with pure Integral Force Feedback
  - 2.1. Plant Parameters
  - 2.2. Equations
  - 2.3. Simscape Model
  - 2.4. Comparison of the Analytical Model and the Simscape Model
  - 2.5. Effect of the rotation speed
  - 2.6. Decentralized Integral Force Feedback
  - 2.7. Sort poles
- 3. Modified IFF (pseudo integrator)
  - 3.1. Plant Parameters
  - 3.2. Modified Integral Force Feedback Controller
  - 3.3. Root Locus
  - 3.4. What is the optimal \(\omega_i\) and \(g\)?
- 4. IFF with a stiffness in parallel with the force sensor
  - 4.1. Equations
  - 4.2. Plant Parameters
  - 4.3. Schematic
  - 4.4. Physical Explanation
  - 4.5. Effect of the parallel stiffness on the IFF plant
  - 4.6. IFF when adding a spring in parallel
  - 4.7. Effect of \(k_p\) on the attainable damping
  - 4.8. Optimal Gain
- 5. Direct Velocity Feedback
  - 5.1. Equations
  - 5.2. Plant Parameters
  - 5.3. Plant - Bode Plot
  - 5.4. Comparison of the Analytical Model and the Simscape Model
  - 5.5. Root Locus
    - - 5.5.0.1. Root Locus Plots
- 6. Comparison
  - 6.1. Plant Parameters
  - 6.2. Root Locus
  - 6.3. Controllers - Optimal Gains
  - 6.4. Transmissibility
  - 6.5. Compliance
- 7. Notations
- 8. Function
  - 8.1. Sort Poles for the Root Locus
    - Function description
    - Optional Parameters
    - Function
    - Remove useless poles
    - Sort poles

- Section 1
- Section 2
- Section 3
- Section 4
- Section 5
- Section 6
- Section 7

## 1 System Description and Analysis

### 1.1 System description

The system consists of one 2 degree of freedom translation stage on top of a spindle (figure 1).

Figure 1: Schematic of the studied system

The control inputs are the forces applied by the actuators of the translation stage (\(F_u\) and \(F_v\)). As the translation stage is rotating around the Z axis due to the spindle, the forces are applied along \(\vec{i}_u\) and \(\vec{i}_v\).

The measurement is either the \(x-y\) displacement of the object located on top of the translation stage or the \(u-v\) displacement of the sample with respect to a fixed reference frame.

### 1.2 Equations

Based on the Figure 1, the equations of motions are:

\begin{equation} \begin{bmatrix} d_u \\ d_v \end{bmatrix} = \bm{G}_d \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation} \begin{equation} \begin{bmatrix} d_u \\ d_v \end{bmatrix} = \frac{1}{k} \frac{1}{G_{dp}} \begin{bmatrix} G_{dz} & G_{dc} \\ -G_{dc} & G_{dz} \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation}

With:

\begin{align} G_{dp} &= \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2 \\ G_{dz} &= \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \\ G_{dc} &= 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \end{align}

Explain Coriolis and Centrifugal Forces (negative Stiffness) => First write the equations in terms of \(k\), \(m\) and \(c\) and explain the terms.

### 1.3 Numerical Values

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

### 1.4 Campbell Diagram

The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed.

It is shown in Figure 2, and one can see that the system becomes unstable for \(\Omega > \omega_0\) (the real part of one of the poles becomes positive).

Figure 2: Campbell Diagram

### 1.5 Simscape Model

Define the rotating speed for the Simscape Model.

    W = 0.1; % Rotation Speed [rad/s]

The transfer function from \([F_u, F_v]\) to \([d_u, d_v]\) is identified from the Simscape model.

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 3, 'openoutput'); io_i = io_i + 1;

    G = linearize(mdl, io, 0);

    %% Input/Output definition
    G.InputName  = {'Fu', 'Fv'};
    G.OutputName = {'du', 'dv'};

### 1.6 Comparison of the Analytical Model and the Simscape Model

The same transfer function from \([F_u, F_v]\) to \([d_u, d_v]\) is written down from the analytical model.

    Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
          [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
           -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];

Both transfer functions are compared in Figure 3 and are found to perfectly match.

Figure 3: Bode plot of the transfer function from \([F_u, F_v]\) to \([d_u, d_v]\) as identified from the Simscape model and from an analytical model

### 1.7 Effect of the rotation speed

The transfer functions from \([F_u, F_v]\) to \([d_u, d_v]\) are identified for the following rotating speeds.

    Ws = [0, 0.1, 0.5, 0.8, 1.1]*w0; % [rad/s]

    Gs = {zeros(2, 2, length(Ws))};

    for W_i = 1:length(Ws)
        W = Ws(W_i);

        Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
                         [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
                          -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]};
    end

They are compared in Figure 4.

Figure 4: Comparison of the transfer functions from \([F_u, F_v]\) to \([d_u, d_v]\) for several rotating speed

## 2 Problem with pure Integral Force Feedback

- Diagram with the controller
- Basic idea of IFF

### 2.1 Plant Parameters

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

### 2.2 Equations

The sensed forces are equal to:

\begin{equation} \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} - (c s + k) \begin{bmatrix} d_u \\ d_v \end{bmatrix} \end{equation}

Which then gives:

\begin{equation} \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} = \bm{G}_{f} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation} \begin{equation} \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} = \frac{1}{G_{fp}} \begin{bmatrix} G_{fz} & -G_{fc} \\ G_{fc} & G_{fz} \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation} \begin{align} G_{fp} &= \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2 \\ G_{fz} &= \left( \frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2} \right) \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right) + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2 \\ G_{fc} &= \left( 2 \xi \frac{s}{\omega_0} + 1 \right) \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right) \end{align}

### 2.3 Simscape Model

The rotation speed is set to \(\Omega = 0.1 \omega_0\).

    W = 0.1*w0; % [rad/s]

And the transfer function from \([F_u, F_v]\) to \([f_u, f_v]\) is identified using the Simscape model.

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;

    Giff = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff.InputName  = {'Fu', 'Fv'};
    Giff.OutputName = {'fu', 'fv'};

### 2.4 Comparison of the Analytical Model and the Simscape Model

The same transfer function from \([F_u, F_v]\) to \([f_u, f_v]\) is written down from the analytical model.

    Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
              [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
               (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];

The two are compared in Figure 5 and found to perfectly match.

Figure 5: Comparison of the transfer functions from \([F_u, F_v]\) to \([f_u, f_v]\) between the Simscape model and the analytical one

### 2.5 Effect of the rotation speed

The transfer functions from \([F_u, F_v]\) to \([f_u, f_v]\) are identified for the following rotating speeds.

    Ws = [0, 0.1, 0.5, 0.8, 1.1]*w0; % Rotating Speeds [rad/s]

    Gsiff = {zeros(2, 2, length(Ws))};

    for W_i = 1:length(Ws)
        W = Ws(W_i);

        Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
                          [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
                           (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]};
    end

The obtained transfer functions are shown in Figure 6.

Figure 6: Comparison of the transfer functions from \([F_u, F_v]\) to \([f_u, f_v]\) for several rotating speed

### 2.6 Decentralized Integral Force Feedback

Let’s take \(\Omega = \frac{\omega_0}{10}\).

    W = w0/10;

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
           [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
            (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];

The decentralized IFF controller consists of pure integrators:

\begin{equation} \bm{K}_{\text{IFF}}(s) = \frac{g}{s} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}

    g = 2;

    Kiff = g/s*tf(eye(2));

The Root Locus (evolution of the poles of the closed loop system in the complex plane as a function of \(g\)) is shown in Figure 7. It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable.

Figure 7: Root Locus for the Decentralized Integral Force Feedback controller. Several rotating speed are shown.

### 2.7 Sort poles

    W = 0.1;

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
           [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
            (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
    g = 1;

    gains = logspace(-2, 4, 100);

    poles = zeros(length(pole(minreal(feedback(Giff, g/s*eye(2))))), length(gains));
    poles(:, 1) = pole(minreal(feedback(Giff, gains(1)/s*eye(2))));
    poles(:, 2) = pole(minreal(feedback(Giff, gains(2)/s*eye(2))));
    % poles(:, 2) = poles(:, 1);

    for g_i = 3:length(gains)
        poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
        poles_est = poles(:, g_i-1);

        poles_gi = pole(minreal(feedback(Giff, gains(g_i)/s*eye(2))));

        % Array of distances between all the poles
        poles_dist = (poles_est-poles_gi.').*conj(poles_est-poles_gi.');
        [~, c] = sort(min(poles_dist));
        poles_dist = poles_dist(:, c);

        for p_i = 1:size(poles_dist, 1)
           [~, a_i] = min(poles_dist(:, p_i));

           poles(c(p_i), g_i) = poles_gi(a_i);
           poles_dist(a_i, :) = [];
           poles_gi(a_i, :) = [];
        end
    end

Working

    W = 0.1;

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
           [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
            (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];

    gains = logspace(-2, 4, 500);

    poles = zeros(length(pole(feedback(Giff, 1/s*eye(2)))), length(gains));
    poles(:, 1) = pole(feedback(Giff, gains(1)/s*eye(2)));
    poles(:, 2) = pole(feedback(Giff, gains(2)/s*eye(2)));
    % poles(:, 2) = poles(:, 1);

    for g_i = 3:length(gains)
        % poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
        poles_est = poles(:, g_i-1);
        poles_gi = pole(feedback(Giff, gains(g_i)/s*eye(2)));

        % Array of distances between all the poles
        poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));

        % Columns of distances are sorted from lowest to highest
        [~, c] = sort(min(poles_dist));
        poles_dist = poles_dist(:, c);

        % for each column of poles_dist corresponding to the i'th pole
        % with closest previous poles
        for p_i = 1:size(poles_dist, 2)
           % Get the indice a_i of the previous pole that is the closest
           % to pole c(p_i)
           [~, a_i] = min(poles_dist(:, p_i));

           poles(a_i, g_i) = poles_gi(c(p_i));

           % poles_dist(a_i, :) = [];
           % poles_gi(a_i, :) = [];
        end
    end

    W = 0.1;

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
           [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
            (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];

    gains = logspace(-2, 4, 500);

    poles = zeros(length(pole(feedback(Giff, 1/s*eye(2)))), length(gains));
    poles(:, 1) = pole(feedback(Giff, gains(1)/s*eye(2)));
    poles(:, 2) = pole(feedback(Giff, gains(2)/s*eye(2)));

    for g_i = 3:length(gains)
        % Estimated value of the poles
        poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));

        % New values for the poles
        poles_gi = pole(feedback(Giff, gains(g_i)/s*eye(2)));

        % Array of distances between all the poles
        poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));

        % Get indices corresponding to distances from lowest to highest
        [~, c] = sort(min(poles_dist));

        as = 1:length(poles_gi);

        % for each column of poles_dist corresponding to the i'th pole
        % with closest previous poles
        for p_i = c
           % Get the indice a_i of the previous pole that is the closest
           % to pole c(p_i)
           [~, a_i] = min(poles_dist(:, p_i));

           poles(as(a_i), g_i) = poles_gi(p_i);

           % Remove old poles that are already matched
           % poles_gi(as(a_i), :) = [];
           poles_dist(a_i, :) = [];
           as(a_i) = [];
        end
    end

How to remove poles that are not moving?

    poles_rl = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > 1e-8, :);
    poles_rl = poles_rl(1:end/2, :);

- works best without minreal
- `[ ]` create a function

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
           [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
            (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
    Kiff = 1/s*eye(2);
    gains = logspace(-2, 4, 500);

    [poles] = rootLocusPolesSorted(Giff, Kiff, gains, 'd_max', 1e-4);

## 3 Modified IFF (pseudo integrator)

- Classical modification of the IFF

### 3.1 Plant Parameters

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

### 3.2 Modified Integral Force Feedback Controller

Let’s modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used:

\begin{equation} K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}

where \(\omega_i\) characterize down to which frequency the signal is integrated.

Let’s arbitrary choose the following control parameters:

    g = 2;
    wi = 0.1*w0;

And the following rotating speed.

    Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
            [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
             (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];

The obtained Loop Gain is shown in Figure 8.

Figure 8: Loop Gain for the modified IFF controller

### 3.3 Root Locus

As shown in the Root Locus plot (Figure 9), for some value of the gain, the system remains stable.

Figure 9: Root Locus for the modified IFF controller

### 3.4 What is the optimal \(\omega_i\) and \(g\)?

In order to visualize the effect of \(\omega_i\) on the attainable damping, the Root Locus is displayed in Figure 10 for the following \(\omega_i\):

    wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s]

Figure 10: Root Locus for the modified IFF controller (zoomed plot on the left)

For the controller

\begin{equation} K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}

The gain at which the system becomes unstable is

\begin{equation} g_\text{max} = \omega_i \left( \frac{{\omega_0}^2}{\Omega^2} - 1 \right) \label{eq:iff_gmax} \end{equation}

While it seems that small \(\omega_i\) do allow more damping to be added to the system (Figure 10), the control gains may be limited to small values due to \eqref{eq:iff_gmax} thus reducing the attainable damping.

There must be an optimum for \(\omega_i\). To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of \(\omega_i\) (Figure 11).

    wis = logspace(-2, 1, 31)*w0; % [rad/s]

    opt_zeta = zeros(1, length(wis)); % Optimal simultaneous damping
    opt_gain = zeros(1, length(wis)); % Corresponding optimal gain

    for wi_i = 1:length(wis)
        wi = wis(wi_i);
        gains = linspace(0, (w0^2/W^2 - 1)*wi, 100);

        for g = gains
            Kiff = (g/(wi+s))*eye(2);

            [w, zeta] = damp(minreal(feedback(Giff, Kiff)));

            if min(zeta) > opt_zeta(wi_i) && all(zeta > 0)
                opt_zeta(wi_i) = min(zeta);
                opt_gain(wi_i) = g;
            end
        end
    end

Figure 11: Simultaneous attainable damping of the closed loop poles as a function of \(\omega_i\)

## 4 IFF with a stiffness in parallel with the force sensor

### 4.1 Equations

\begin{equation} \begin{bmatrix} f_u \\ f_v \end{bmatrix} = \bm{G}_k \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation} \begin{equation} \begin{bmatrix} f_u \\ f_v \end{bmatrix} = \frac{1}{G_{kp}} \begin{bmatrix} G_{kz} & -G_{kc} \\ G_{kc} & G_{kz} \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation}

With:

\begin{align} G_{kp} &= \left( \frac{s^2}{{\omega_0^\prime}^2} + 2\xi^\prime \frac{s}{{\omega_0^\prime}^2} + 1 - \frac{\Omega}{\omega_0^\prime} \right)^2 + \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right)^2 \\ G_{kz} &= \left( \frac{s^2}{{\omega_0^\prime}^2} + \frac{k_p}{k + k_p} - \frac{\Omega^2}{{\omega_0^\prime}^2} \right) \left( \frac{s^2}{{\omega_0^\prime}^2} + 2\xi^\prime \frac{s}{{\omega_0^\prime}^2} + 1 - \frac{\Omega}{\omega_0^\prime} \right) + \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right)^2 \\ G_{kc} &= \left( 2 \xi^\prime \frac{s}{\omega_0^\prime} + \frac{k}{k + k_p} \right) \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right) \end{align}

where:

- \(\omega_0^\prime = \frac{k + k_p}{m}\)
- \(\xi^\prime = \frac{c}{2 \sqrt{(k + k_p) m}}\)

    Giff_th = 1/( (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 )*[...
                       (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 - (c*s + k)*(m*s^2 + c*s + k + kp - m*W^2), -(c*s + k)*(2*m*s*W);
                       (c*s + k)*(2*m*s*W), (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 - (c*s + k)*(m*s^2 + c*s + k + kp - m*W^2)
                       ];

    w0p = sqrt((k + kp)/m);
    xip = c/(2*sqrt((k+kp)*m));

    Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 )*[...
                       (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
                       (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2
                       ];

    Giff_th_b = 1/( (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 )*[...
                       (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 - (c*s + k)*(m*s^2 + c*s + k + kp - m*W^2), -(c*s + k)*(2*m*s*W);
                       (c*s + k)*(2*m*s*W), (m*s^2 + c*s + k + kp - m*W^2)^2 + (2*m*s*W)^2 - (c*s + k)*(m*s^2 + c*s + k + kp - m*W^2)
                       ];

### 4.2 Plant Parameters

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

### 4.3 Schematic

Figure 12: Figure caption

### 4.4 Physical Explanation

- Negative stiffness induced by gyroscopic effects
- Zeros of the open-loop <=> Poles of the subsystem with the force sensors removes
- As the zeros are the poles of the closed loop system for high gains, we want them to be in the left-half plane
- Thus we want the zeros to be in the left half plant and thus the system with the force sensors stable
- This can be done by adding springs in parallel with the force sensors with a stiffness larger than the virtual negative stiffness added by the gyroscopic effects

The negative stiffness induced by the rotation is:

\begin{equation} k_{n} = - m \Omega^2 \end{equation}

And thus, the stiffness in parallel should be such that:

\begin{equation} k_{p} > m \Omega^2 \end{equation}

### 4.5 Effect of the parallel stiffness on the IFF plant

The rotation speed is set to \(\Omega = 0.1 \omega_0\).

    W = 0.1*w0; % [rad/s]

And the IFF plant (transfer function from \([F_u, F_v]\) to \([f_u, f_v]\)) is identified in three different cases:

- without parallel stiffness
- with a small parallel stiffness \(k_p < m \Omega^2\)
- with a large parallel stiffness \(k_p > m \Omega^2\)

The results are shown in Figure 13.

One can see that for \(k_p > m \Omega^2\), the systems shows alternating complex conjugate poles and zeros.

    kp = 0;
    cp = 0;

    Giff = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff.InputName  = {'Fu', 'Fv'};
    Giff.OutputName = {'fu', 'fv'};

    kp = 0.5*m*W^2;
    cp = 0;

    Giff_s = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff_s.InputName  = {'Fu', 'Fv'};
    Giff_s.OutputName = {'fu', 'fv'};

    kp = 1.5*m*W^2;
    cp = 0;

    Giff_l = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff_l.InputName  = {'Fu', 'Fv'};
    Giff_l.OutputName = {'fu', 'fv'};

Figure 13: Transfer function from \([F_u, F_v]\) to \([f_u, f_v]\) for \(k_p = 0\), \(k_p < m \Omega^2\) and \(k_p > m \Omega^2\)

### 4.6 IFF when adding a spring in parallel

In Figure 14 is displayed the Root Locus in the three considered cases with

\begin{equation} K_{\text{IFF}} = \frac{g}{s} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}

One can see that for \(k_p > m \Omega^2\), the root locus stays in the left half of the complex plane and thus the control system is unconditionally stable.

Thus, decentralized IFF controller with pure integrators can be used if:

\begin{equation} k_{p} > m \Omega^2 \end{equation}

Figure 14: Root Locus

### 4.7 Effect of \(k_p\) on the attainable damping

However, having large values of \(k_p\) may:

- decrease the actuator force authority
- decrease the attainable damping

To study the second point, Root Locus plots for the following values of \(k_p\) are shown in Figure 15.

    kps = [1, 5, 10, 50]*m*W^2;
    cp = 0.01;

It is shown that large values of \(k_p\) decreases the attainable damping.

Figure 15: Root Locus plot

### 4.8 Optimal Gain

Let’s take \(k_p = 5 m \Omega^2\) and find the optimal IFF control gain \(g\) such that maximum damping are added to the poles of the closed loop system.

    kp = 5*m*W^2;
    cp = 0.01;

    Giff = linearize(mdl, io, 0);

    opt_zeta = 0;
    opt_gain = 0;

    gains = logspace(-2, 4, 100);

    for g = gains
        Kiff = (g/s)*eye(2);

        [w, zeta] = damp(minreal(feedback(Giff, Kiff)));

        if min(zeta) > opt_zeta && all(zeta > 0)
            opt_zeta = min(zeta);
            opt_gain = min(g);
        end
    end

Figure 16: Root Locus for \(k_p = 5 m \Omega^2\) and the poles corresponding to the identified optimal gain

## 5 Direct Velocity Feedback

### 5.1 Equations

The sensed relative velocity are equal to:

\begin{equation} \begin{bmatrix} \dot{d}_u \\ \dot{d}_v \end{bmatrix} = \bm{G}_v \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation} \begin{equation} \begin{bmatrix} \dot{d}_u \\ \dot{d}_v \end{bmatrix} = \frac{s}{k} \frac{1}{G_{vp}} \begin{bmatrix} G_{vz} & G_{vc} \\ -G_{vc} & G_{vz} \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation}

With:

\begin{align} G_{vp} &= \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2 \\ G_{vz} &= \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \\ G_{vc} &= 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \end{align}

### 5.2 Plant Parameters

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

### 5.3 Plant - Bode Plot

The rotating speed is set to \(\Omega = 0.1 \omega_0\).

    W = 0.1*w0;

And the transfer function from \([F_u, F_v]\) to \([v_u, v_v]\) is identified using the Simscape model.

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;

    Gdvf = linearize(mdl, io, 0);

    %% Input/Output definition
    Gdvf.InputName  = {'Fu', 'Fv'};
    Gdvf.OutputName = {'Vu', 'Vv'};

### 5.4 Comparison of the Analytical Model and the Simscape Model

The same transfer function from \([F_u, F_v]\) to \([v_u, v_v]\) is written down from the analytical model.

    Gdvf_th = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
              [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
               -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];

    Gdvf_th.InputName  = {'Fu', 'Fv'};
    Gdvf_th.OutputName = {'vu', 'vv'};

The two are compared in Figure 5 and found to perfectly match.

Figure 17: Comparison of the transfer functions from \([F_u, F_v]\) to \([v_u, v_v]\) between the Simscape model and the analytical one

### 5.5 Root Locus

The Decentralized Direct Velocity Feedback controller consist of a pure gain on the diagonal:

\begin{equation} K_{\text{DVF}}(s) = g \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}

The corresponding Root Locus plots for the following rotating speeds are shown in Figure 18.

    Ws = [0, 0.1, 0.5, 0.8, 1.1]*w0; % Rotating Speeds [rad/s]

It is shown that for rotating speed \(\Omega < \omega_0\), the closed loop system is unconditionally stable and arbitrary damping can be added to the poles.

    gains = logspace(-2, 2, 100);

##### 5.5.0.1 Root Locus Plots

Figure 18: Root Locus for the Decentralized Direct Velocity Feedback controller. Several rotating speed are shown.

## 6 Comparison

### 6.1 Plant Parameters

Let’s define initial values for the model.

    k = 1;    % Actuator Stiffness [N/m]
    c = 0.05; % Actuator Damping [N/(m/s)]
    m = 1;    % Payload mass [kg]

    xi = c/(2*sqrt(k*m));
    w0 = sqrt(k/m); % [rad/s]

The rotating speed is set to \(\Omega = 0.1 \omega_0\).

    W = 0.1*w0;

### 6.2 Root Locus

    wi = 0.1*w0;

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;

    Giff = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff.InputName  = {'Fu', 'Fv'};
    Giff.OutputName = {'Fmu', 'Fmv'};

    kp = 5*m*W^2;
    cp = 0.01;

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;

    Giff_kp = linearize(mdl, io, 0);

    %% Input/Output definition
    Giff_kp.InputName  = {'Fu', 'Fv'};
    Giff_kp.OutputName = {'Fmu', 'Fmv'};

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;

    Gdvf = linearize(mdl, io, 0);

    %% Input/Output definition
    Gdvf.InputName  = {'Fu', 'Fv'};
    Gdvf.OutputName = {'Vu', 'Vv'};

Figure 19: Root Locus plot - Comparison of IFF with additional high pass filter, IFF with additional parallel stiffness and DVF

### 6.3 Controllers - Optimal Gains

In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case.

The obtained damping ratio and control are shown below.

| Obtained \(\xi\) | Control Gain
--- | --- | ---
Modified IFF | 0.83 | 2.0
IFF with \(k_p\) | 0.84 | 2.01
DVF | 0.85 | 1.67

### 6.4 Transmissibility

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/dw'], 1, 'input');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;

    Tol = linearize(mdl, io, 0);

    %% Input/Output definition
    Tol.InputName  = {'Dwx', 'Dwy'};
    Tol.OutputName = {'Dx', 'Dy'};

    Kiff = opt_gain_iff/(wi + s)*tf(eye(2));

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/dw'], 1, 'input');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;

    Tiff = linearize(mdl, io, 0);

    %% Input/Output definition
    Tiff.InputName  = {'Dwx', 'Dwy'};
    Tiff.OutputName = {'Dx', 'Dy'};

    kp = 5*m*W^2;
    cp = 0.01;

    Kiff = opt_gain_kp/s*tf(eye(2));

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/dw'], 1, 'input');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;

    Tiff_kp = linearize(mdl, io, 0);

    %% Input/Output definition
    Tiff_kp.InputName  = {'Dwx', 'Dwy'};
    Tiff_kp.OutputName = {'Dx', 'Dy'};

    Kdvf = opt_gain_kp*tf(eye(2));

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/dw'], 1, 'input');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;

    Tdvf = linearize(mdl, io, 0);

    %% Input/Output definition
    Tdvf.InputName  = {'Dwx', 'Dwy'};
    Tdvf.OutputName = {'Dx', 'Dy'};

Figure 20: Comparison of the transmissibility

### 6.5 Compliance

    %% Name of the Simulink File
    mdl = 'rotating_frame';

    %% Input/Output definition
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/fd'], 1, 'input');  io_i = io_i + 1;
    io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;

    Col = linearize(mdl, io, 0);

    %% Input/Output definition
    Col.InputName  = {'Fdx', 'Fdy'};
    Col.OutputName = {'Dx', 'Dy'};

    Kiff = opt_gain_iff/(wi + s)*tf(eye(2));

    Ciff = linearize(mdl, io, 0);

    %% Input/Output definition
    Ciff.InputName  = {'Fdx', 'Fdy'};
    Ciff.OutputName = {'Dx', 'Dy'};

    kp = 5*m*W^2;
    cp = 0.01;

    Kiff = opt_gain_kp/s*tf(eye(2));

    Ciff_kp = linearize(mdl, io, 0);

    %% Input/Output definition
    Ciff_kp.InputName  = {'Fdx', 'Fdy'};
    Ciff_kp.OutputName = {'Dx', 'Dy'};

    Kdvf = opt_gain_kp*tf(eye(2));

    Cdvf = linearize(mdl, io, 0);

    %% Input/Output definition
    Cdvf.InputName  = {'Fdx', 'Fdy'};
    Cdvf.OutputName = {'Dx', 'Dy'};

Figure 21: Comparison of the obtained Compliance

## 7 Notations

| Mathematical Notation | Matlab | Unit
--- | --- | --- | ---
Actuator Stiffness | \(k\) | `k` | N/m
Actuator Damping | \(c\) | `c` | N/(m/s)
Payload Mass | \(m\) | `m` | kg
Damping Ratio | \(\xi = \frac{c}{2\sqrt{km}}\) | `xi` |
Actuator Force | \(\bm{F}, F_u, F_v\) | `F` `Fu` `Fv` | N
Force Sensor signal | \(\bm{f}, f_u, f_v\) | `f` `fu` `fv` | N
Relative Displacement | \(\bm{d}, d_u, d_v\) | `d` `du` `dv` | m
Relative Velocity | \(\bm{v}, v_u, v_v\) | `v` `vu` `vv` | m/s
Resonance freq. when \(\Omega = 0\) | \(\omega_0\) | `w0` | rad/s
Rotation Speed | \(\Omega = \dot{\theta}\) | `W` | rad/s
Low Pass Filter corner frequency | \(\omega_i\) | `wi` | rad/s

| Mathematical Notation | Matlab | Unit
--- | --- | --- | ---
Laplace variable | \(s\) | `s` |
Complex number | \(j\) | `j` |
Frequency | \(\omega\) | `w` | [rad/s]

| Mathematical Notation | Matlab | Unit
--- | --- | --- | ---
IFF Plant | \(\bm{G}_\text{IFF}(s) = \frac{\bm{f}}{\bm{F}}\) | `Giff` | N/N
DVF Plant | \(\bm{G}_\text{DVF}(s) = \frac{\bm{v}}{\bm{F}}\) | `Gdvf` | (m/s)/N
IFF Controller | \(\bm{K}_\text{IFF}(s)\) | `Kiff` |
DVF Controller | \(\bm{K}_\text{DVF}(s)\) | `Kdvf` |

## 8 Function

### 8.1 Sort Poles for the Root Locus

This Matlab function is accessible here.

#### Function description

    function [poles] = rootLocusPolesSorted(G, K, gains, args)
    % rootLocusPolesSorted -
    %
    % Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
    %
    % Inputs:
    %    - G, K, gains, args -
    %
    % Outputs:
    %    - poles -

#### Optional Parameters

    arguments
        G
        K
        gains
        args.minreal double {mustBeNumericOrLogical} = false
        args.p_half  double {mustBeNumericOrLogical} = false
        args.d_max   double {mustBeNumeric} = -1
    end

#### Function

    if args.minreal
        p1 = pole(minreal(feedback(G, gains(1)*K)));
        [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
        p1 = p1(i_uniq);

        poles = zeros(length(p1), length(gains));
        poles(:, 1) = p1;
    else
        p1 = pole(feedback(G, gains(1)*K));
        [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
        p1 = p1(i_uniq);

        poles = zeros(length(p1), length(gains));
        poles(:, 1) = p1;
    end

    if args.minreal
        p2 = pole(minreal(feedback(G, gains(2)*K)));
        [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
        p2 = p2(i_uniq);
        poles(:, 2) = p2;
    else
        p2 = pole(feedback(G, gains(2)*K));
        [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
        p2 = p2(i_uniq);
        poles(:, 2) = p2;
    end

    for g_i = 3:length(gains)
        % Estimated value of the poles
        poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));

        % New values for the poles
        poles_gi = pole(feedback(G, gains(g_i)*K));
        [~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
        poles_gi = poles_gi(i_uniq);

        % Array of distances between all the poles
        poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));

        % Get indices corresponding to distances from lowest to highest
        [~, c] = sort(min(poles_dist));

        as = 1:length(poles_gi);

        % for each column of poles_dist corresponding to the i'th pole
        % with closest previous poles
        for p_i = c
            % Get the indice a_i of the previous pole that is the closest
            % to pole c(p_i)
            [~, a_i] = min(poles_dist(:, p_i));

            poles(as(a_i), g_i) = poles_gi(p_i);

            % Remove old poles that are already matched
            % poles_gi(as(a_i), :) = [];
            poles_dist(a_i, :) = [];
            as(a_i) = [];
        end
    end

#### Remove useless poles

    if args.d_max > 0
        poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
    end

    if args.p_half
        poles = poles(1:round(end/2), :);
    end

#### Sort poles

    [~, s_p] = sort(imag(poles(:,1)), 'descend');
    poles = poles(s_p, :);

Author: Thomas Dehaeze

Created: 2020-06-18 jeu. 11:51