% This example demonstrates how to invoke the scripts for obtaining
%IE-optimal and IAE optimal (with or without LP filter) PID controllers.
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
% See the git repo for further documentation.

%% Setup
clear all
clc

% Process model (continuous time LTI SISO system)
s = zpk('s');
P = 1/(s^2+2*.5*s+1)*exp(-0.5*s);

% Robustness consraints
Ms = 1.6; % Maximum allowed sensitivity function magnitude
Mt = 1.6; % Maximum allowed complementary sensitivity function magnitude
Mks = 2; % Maximum allowed magnitude of transfer function from process
          % output to control signal, sometimes referred to as noise
          % sensitivity.
          
%% Design
% IE-optimal design
p0 = [0 0 0]'; % Parameters of any PID controller, which stabilizes P.
             % See pidIE.m for details. Use p = [0 0] to synthesize PI
             % controller.
[K1,p1] = pidIE(P,Ms,Mt,Mks,p0)

% IAE-optimal design
[K2,p2] = pidIAE(P,Ms,Mt,Mks,p1) % Hot-start with K1 parameters

% IAE-optimal design with filter
[K3,p3,w] = pidfIAE(P,Ms,Mt,Mks,p2); % Hot-start with K2 parameters
K3
p3

%% Evaluation
% Closed-loop transfer functions
L = @(K)series(P,K);
S = @(K)feedback(1,L(K));
T = @(K)feedback(L(K),1);
KS = @(K)feedback(K,P);
PS = @(K)feedback(P,K)

wMin = w(1);
wMax = w(end);
close all

% Controller dynamics
figure(1)
bode(K1,{wMin,wMax},'b',K2,'r',K3,'g')
hold on
title('Controller')
legend('IE-optimal','IAE-optimal','IAE-optimal w. filter','Location',...
    'NorthWest')

% Bode magnitudes (to verify robustness constraints)
figure(3)
subplot(311)
bodemag(S(K1),{wMin,wMax},'b',S(K2),'r',S(K3),'g')
hold on
ylim([1e-1 2*Ms])
set(gca,'ytick',[1 Ms])
plot([wMin wMax],Ms*[1 1],'k')
title('Sensitivity magnitude')
legend('IE-optimal','IAE-optimal','IAE-optimal w. filter','Constraint',...
    'Location','NorthWest')

subplot(312)
bodemag(T(K1),{wMin,wMax},'b',T(K2),'r',T(K3),'g')
hold on
ylim([1e-1 2*Mt])
set(gca,'ytick',[1 Mt])
plot([wMin wMax],Mt*[1 1],'k')
title('Complementary sensitivity')

subplot(313)
bodemag(KS(K1),{wMin,wMax},'b',KS(K2),'r',KS(K3),'g')
hold on
ylim([1e-1 2*Mks])
set(gca,'ytick',[1 Mks])
plot([wMin wMax],Mks*[1 1],'k')
title('Noise sensitivity')

% Load disturbance response (to compute IE and IAE)
figure(4)
step(PS(K1),'b',PS(K2),'r',PS(K3),'g',P,'k')
hold on
title('Load step response')
legend('IE-optimal','IAE-optimal','IAE-optimal w. filter','Open-loop',...
    'Location','East')