% generateDiagPidControl
%   :PROPERTIES:
%   :header-args:matlab+: :tangle src/generateDiagPidControl.m
%   :header-args:matlab+: :comments org :mkdirp yes
%   :header-args:matlab+: :eval no :results none
%   :END:
%   <<sec:generateDiagPidControl>>

% This Matlab function is accessible [[file:src/generateDiagPidControl.m][here]].


function [K] = generateDiagPidControl(G, fs)
    %%
    pid_opts = pidtuneOptions(...
        'PhaseMargin', 50, ...
        'DesignFocus', 'disturbance-rejection');

    %%
    K = tf(zeros(6));

    for i = 1:6
        input_name = G.InputName(i);
        output_name = G.OutputName(i);
        K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
    end

    K.InputName = G.OutputName;
    K.OutputName = G.InputName;
end