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

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

    for i = 1:5
        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