Commit bffc055b authored by Kristian Soltesz's avatar Kristian Soltesz
Browse files

Added example and cleaned up code

parent 48724d85
......@@ -2,57 +2,80 @@
%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
% Process model (continuous time LTI SISO system)
s = tf('s');
P = 5/(s+1)/(3*s+1)*exp(-2*s);
%% Setup
clear all
close all
clc
% Note: While defaults in the optimization code work for many processes,
% you may need to change grids and tolerances for some.
%
% Note: The optimization scripts were primarily written with stable process
% models in mind. If applied to unstable process models, they may need to
% be modified.
%
% Note: The optimization scripts assume contunuous time process models.
% Modifying them to accomodate discrete time process models is
% straightforward.
%
% Note: If a filter has been designed for the process, simply incorprate it
% into the process model (that is, as a series connected component of P).
% Process model (continuous time LTI SISO system)
s = zpk('s');
P = 1/(s^2+2*.7*s+1)*exp(-0.5*s);
Ms = 1.5; % Maximum allowed sensitivity function magnitude
Mt = 1.5; % Maximum allowed complementary sensitivity function magnitude
Mks = 10; % Maximum allowed magnitude of transfer function from process
% 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
[K,p3] = pidfIAE(P,Ms,Mt,Mks,p2) % Hot-start with K2 parameters
[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);
% Bode magnitudes (to verify robustness constraints)
wMin = w(1);
wMax = w(end);
figure(1)
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(2)
% FIXME: plot temporal responses
% Note: The IE-optimal design method relies on convex-concave optimization.
% It guarantees robustness but not stability. So if you choose the grid in
% an unfortunate way, you might end up with a robustly unstable design.
% Note: Behaviour of the methods is undefined (or at least unexplained
% here) if the constraint combinatioon Ms,Mt,Mks is infeasible for P.
%
% Note: Neither the IE nor IAE (with or without filter) guarantee
% optimality. However, both methods have been verified to find the true
% optimum in a large number of cases, such as ...
%% Evaluation
\ No newline at end of file
% The relative dead time tau = L/(L+T) and arrest time Tar = T+L are
% defined for first order time delayed SISO LTI systems
%P(s) = K/(s*T+1)*exp(-s*L)
% defined for first order time SISO LTI systems K/(s*T+1)*exp(-s*L).
%
% This code computes generalizations of tau and Tar for arbitrary SISO LTI
% systems, which can be either discrete or continuous.
% Written by kristian@control.lth.se
%
% P - LTI SISO system.
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
function [tau,Tar,L] = getTauTar(P)
......
function [K,p,w] =pidIAE(P,Ms,Mt,Mks,p,wN,N)
function [K,p,w] =pidIAE(P,Ms,Mt,Mks,p)
% Computes PID controller that maximizes the integral gain subject to
% constraints on the magnitude of the sensitivity, complementary
% sensitivity and noise sensitivity fu nctions.
% The length of p decides if the controller is of PI or PID type.
%
% K(s) = kp + ki/s + kd*s
% K(s) = kp + ki/s + kd*s
%
% P - LTI process model.
% Ms - Maximum allowed sensitivity.
% Mt - Maximum allowed complementary sensitivity.
% Mks - Maxium allowed noise sensitivity.
% p - vector of controller parameters. Length must be 2 (PI)
% and 3 (PID). Kontroller must stabilize plant.
% wN - intended Nyquist frequency.
% N - number of points in frequency grid. Optional, use [].
% P - LTI process model.
% Ms - Maximum allowed sensitivity.
% Mt - Maximum allowed complementary sensitivity.
% Mks - Maxium allowed noise sensitivity.
% p - Vector of initial controller parameters. Length must be 2 (PI)
% or 3 (PID). Controller must stabilize plant.
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
% optional argument defaults
if N == []
N=500; % number of grid points
end
% See the git repo for further documentation.
% regularize
if p(1)~=0
k0=p(1);
p=p/k0;
P=P*k0;
if p(1) ~= 0
k0 = p(1);
p = p/k0;
P = P*k0;
else
k0=1;
k0 = 1;
end
% frequency grid (heuristic)
N=500; % number of grid points
[~,~,w]=bode(P);
wMax = min(wN,w(end));
wMin = min(w(1),wMax/100);
w=logspace(log10(wMin),log10(wMax),N)';
% Frequency grid (heuristic) - change as needed
N = 500; % Number of grid points
[~,~,w] = bode(P);
w = logspace(log10(w(1)),log10(w(end)),N)';
% helper functions
mdot=@(x,y)(bsxfun(@times,x,y)); % (row vector)-matrix column-wise dot
fr=@(x)permute(freqresp(x,w),[3 1 2]); % frequency response over grid
% Helper functions
mdot = @(x,y)(bsxfun(@times,x,y)); % (row vector)-matrix column-wise dot
fr = @(x)permute(freqresp(x,w),[3 1 2]); % frequency response over grid
% transfer functions
s=zpk('s');
K1=[1;1/s;s];
K1=K1(1:numel(p));
K=@(p)p.'*K1;
K_p=K1; % dK/dp
S=@(p)1/(1+P*K(p));
% Transfer functions
s = zpk('s');
K1 = [1;1/s;s];
K1 = K1(1:numel(p));
K = @(p)p.'*K1;
K_p = K1; % dK/dp
S = @(p)feedback(1,P*K(p));
% frequency responses
Pw=fr(P);
K_pw=fr(K_p);
% Frequency responses
Pw = fr(P);
K_pw = fr(K_p);
% optimization
cfg=optimset('algorithm','active-set','GradConstr','on','GradObj','on',...
% Optimization
cfg = optimset('algorithm','active-set','GradConstr','on','GradObj','on',...
'Display','off');
[p,IAE]=fmincon(@(p)J(p),p,[],[],[],[],eps+0*p,[],@(p)cNL(p),cfg);
[p,IAE] = fmincon(@(p)J(p),p,[],[],[],[],eps+0*p,[],@(p)cNL(p),cfg);
p = p*k0;
K = K(p);
% objective and gradient
function [J,J_p]=J(p)
% objective
[e,t]=step(P*S(p));
[e,t]=resample(e,t);
J=trapz(t,abs(e));
% Objective and gradient
function [J,J_p] = J(p)
% Objective
[e,t] = step(P*S(p));
[e,t] = resample(e,t);
J = trapz(t,abs(e));
% gradient
e_p=step(-P^2*K_p*S(p)^2,t);
J_p=trapz(t,mdot(sign(e),e_p));
% Gradient
e_p = step(-P^2*K_p*S(p)^2,t);
J_p = trapz(t,mdot(sign(e),e_p));
end
% constraints and gradients
% Constraints and gradients
function [c,cEq,c_p,cEq_p]=cNL(p)
% Ms
Sw=fr(S(p));
Sm=abs(Sw);
Sw = fr(S(p));
Sm = abs(Sw);
% Mt
Tw=1-Sw;
Tm=abs(Tw);
Tw = 1-Sw;
Tm = abs(Tw);
% Mks
Kw=fr(K(p));
Km=abs(Kw);
KSm=Km.*Sm*k0; % don't forget the regularization
Kw = fr(K(p));
Km = abs(Kw);
KSm = Km.*Sm*k0; % Undo regularization
% constraints
c=[Sm-Ms,Tm-Mt,KSm-Mks];
cEq=[];
% Constraints
c = [Sm-Ms,Tm-Mt,KSm-Mks];
cEq = [];
% gradients
SPdotK_pw=fr(P*S(p)*K_p);
Sm_p=-mdot(Sm,real(mdot(Sw.*Pw,K_pw)));
Tm_p=mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,K_pw)));
KSm_p=mdot(Sm./Km,real(mdot(conj(Kw).*Sw,K_pw)));
c_p=[Sm_p;Tm_p;KSm_p].';
cEq_p=[];
% Gradients
SPdotK_pw = fr(P*S(p)*K_p);
Sm_p = -mdot(Sm,real(mdot(Sw.*Pw,K_pw)));
Tm_p = mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,K_pw)));
KSm_p = mdot(Sm./Km,real(mdot(conj(Kw).*Sw,K_pw)));
c_p = [Sm_p;Tm_p;KSm_p].';
cEq_p = [];
end
end
\ No newline at end of file
function [K,p,w] = pidIE(P,Ms,Mt,Mks,p,wN,N,tol)
function [K,p,w] = pidIE(P,Ms,Mt,Mks,p)
%Computes PID parameters
% Computes PID controller that maximizes the integral gain subject to
% constraints on the magnitude of the sensitivity, complementary
......@@ -11,36 +11,27 @@ function [K,p,w] = pidIE(P,Ms,Mt,Mks,p,wN,N,tol)
% Ms - Maximum allowed sensitivity.
% Mt - Maximum allowed complementary sensitivity.
% Mks - Maxium allowed noise sensitivity.
% p - vector of controller parameters. Length must be 2 (PI).
% and 3 (PID). Controller must stabilize plant.
% wN - intended Nyquist frequency when implementing controller
% N - number of points in frequency grid. Optional, use [].
% tol - relative stopping tolerance. Optional, use [].
% p - Vector of controller parameters. Length must be 2 (PI)
% or 3 (PID). Controller must stabilize plant.
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
% See the git repo for further documentation.
% optional argument defaults
if N == []
N=500; % number of grid points
end
if tol == []
tol = 1e-3;
end
% frequency grid (heuristic)
% Frequency grid (heuristic) - change as needed
N = 500; % Number of grid points
[~,~,w]=bode(P);
wMax = min(wN,w(end));
wMax = w(end);
wMin = min(w(1),wMax/100);
w=logspace(log10(wMin),log10(wMax),N)';
% frequency response
% Frequency response
P = squeeze(freqresp(balreal(P),w));
% Constraint represented as circles with centresa and radii :
cs = -1; % Ms center
rs = 1./Ms; % Ms radius
cs = -1; % Ms center
rs = 1./Ms; % Ms radius
ct = -Mt.^2./(Mt.^2-1); % Mt center
rt = Mt./(Mt.^2-1); % Mt radius
rt = Mt./(Mt.^2-1); % Mt radius
% controller structure
K1=[1+0*w 1./(1i*w) 1i*w];
......@@ -49,10 +40,10 @@ K1=K1(:,1:n);
K=K1*p;
% Optimization routine
prev_obj = -inf; % Used for the stopping criterion
max_iters = 10; % max number of iterations (heuristic)
prev_obj = -inf; % Used for the stopping criterion
max_iters = 10; % Max number of iterations (heuristic)
for iter = 1:max_iters
Lc = P.*K; % Loop transfer funtion
Lc = P.*K; % Loop transfer funtion
cvx_begin
cvx_quiet(true)
variable p(n)
......@@ -68,7 +59,7 @@ for iter = 1:max_iters
p(3) = NaN;
end
% Stopping criterion (heuristic):
% Stopping criterion (heuristic) - change as needed
if (p(2)-prev_obj) < 1e-3*prev_obj
break;
end
......
function [KF,p,w]=getK2F(P,Ms,Mt,Mks,pK)
% Computes PID parameters, that minimize load step IAE, and filter for
% noise attenuation.
% Computes PID parameters, that minimize load step IAE, and filter for
% noise attenuation.
%
% K(s) * F(s) = (kp + ki/s + kd*s) * 1/((s*T)^2+2*z*T*s+1)
% K(s) * F(s) = (kp + ki/s + kd*s) * 1/((s*T)^2+2*z*T*s+1)
%
% P - process (LTI object).
% Ms - maximum allowed sensitivity.
% Mt - maximum allowed complementary sensitivity.
% Mks - maximum gain from plant output (noise) to control signal.
% pK - inital controller parameters [kp ki kd].
% P - LTI process model.
% Ms - Maximum allowed sensitivity.
% Mt - Maximum allowed complementary sensitivity.
% Mks - Maxium allowed noise sensitivity. This is only meaningful if
% there is high-frequency roll-off (through e.g. a filter).
% pK - Vector of initial controller parameters [kp ki kd].
% Length must be 2 (PI)
% or 3 (PID). Controller must stabilize plant.
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
% See the git repo for further documentation.
% regularize
if pK(1)~=0
k0=pK(1);
pK=pK/k0;
P=P*k0;
if pK(1) ~= 0
k0 = pK(1);
pK = pK/k0;
P = P*k0;
else
k0=1;
k0 = 1;
end
% Frequency grid (heuristic) - change as needed
N=500; % Number of grid points
[~,~,w]=bode(P);
w=logspace(log10(w(1)),log10(w(end)),N)';
N = 500; % Number of grid points
[~,~,w] = bode(P);
w = logspace(log10(w(1)),log10(w(end)),N)';
% Ensure HF roll-off
Tmin=2/w(end);
% Ensure LP filter breakdown within grid
Tmin = 1/w(end);
pF = Tmin;
% Organize parameters parameters
p=[pK(:);pF(:)];
pK=@(p)p(1:3);
pF=@(p)p(4); % FIXME: rescale to fit Lund parametrization of filter
w=w(:);
p = [pK(:);pF(:)];
pK = @(p)p(1:3);
pF = @(p)p(4); % FIXME: rescale to fit Lund parametrization of filter
w = w(:);
p(4)=max(p(4),Tmin);
p(4) = max(p(4),Tmin);
% Helper functions
mdot=@(x,y)(bsxfun(@times,x,y)); % (row vector)-matrix column-wise dot
fr=@(x)permute(freqresp(x,w),[3 1 2]); % frequency response over grid
mdot = @(x,y)(bsxfun(@times,x,y)); % (row vector)-matrix column-wise dot
fr = @(x)permute(freqresp(x,w),[3 1 2]); % frequency response over grid
% Misc
z=1/sqrt(2); % Filter damping
Pw=fr(P); % Frequency response
z = 1/sqrt(2); % Filter damping
Pw = fr(P); % Frequency response
% transfer functions
s=zpk('s');
K1=[1;1/s;s];
K=@(p)pK(p).'*K1;
F=@(p)1/(1/2*(pF(p)*s)^2+2*z*pF(p)*s+1);
%F=@(p)1/((T(p)*s)^2+2*z*T(p)*s+1); % Alternative parametrization
S=@(p)1/(1+P*K(p)*F(p));
% Transfer functions
s = zpk('s');
K1 = [1;1/s;s];
K = @(p)pK(p).'*K1;
F = @(p)1/((pF(p)*s)^2+2*z*pF(p)*s+1); % Alternative parametrization
S = @(p)feedback(1,P*K(p)*F(p));
% sensitivities
K_pK=@(p)K1; % dK/dpK
F_pF=@(p)(-1)*F(p)^2*s*(pF(p)*s+2*z); % dF/dT
%F_T=@(p)(-2)*F(p)^2*s*(T(p)*s+z); % Alternative parametrization
KF_p=@(p)[F(p)*K_pK(p);K(p)*F_pF(p)]; % dKF/dp
% Sensitivities
K_pK = @(p)K1; % dK/dpK
F_pF = @(p)(-2)*F(p)^2*s*(s*pF(p)+z); % dF/dpF
KF_p = @(p)[F(p)*K_pK(p);K(p)*F_pF(p)]; % dKF/dp
% Optimization
cfg=optimset('algorithm','active-set','GradConstr','on','GradObj','on',...
'Display','off','maxIter',10);
p=fmincon(@(p)J(p),p,[],[],[],[],[eps 0 0 Tmin],[],@(p)cNL(p),cfg);
cfg = optimset('algorithm','active-set','GradConstr','on','GradObj',...
'on','Display','off','maxIter',10);
p = fmincon(@(p)J(p),p,[],[],[],[],[eps 0 0 Tmin],[],@(p)cNL(p),cfg);
p(1:end-1) = p(1:end-1)*k0;
KF=minreal(K(p)*F(p));
KF = minreal(K(p)*F(p));
% Objective and gradient
function [J,J_p]=J(p)
function [J,J_p] = J(p)
% Objective
[e,t]=step(P*S(p));
[e,t]=resample(e,t);
J=trapz(t,abs(e));
e_p=step(minreal(-P^2*KF_p(p))*S(p)^2,t); % step(P*dS/dp)
J_p=trapz(t,mdot(sign(e),e_p));
[e,t] = step(P*S(p));
[e,t] = resample(e,t);
J = trapz(t,abs(e));
e_p = step(minreal(-P^2*KF_p(p))*S(p)^2,t); % step(P*dS/dp)
J_p = trapz(t,mdot(sign(e),e_p));
end
% Constraints and gradients
function [c,cEq,c_p,cEq_p]=cNL(p)
% Ms
Sw=fr(S(p));
Sm=abs(Sw);
cS=Sm-Ms;
Sw = fr(S(p));
Sm = abs(Sw);
cS = Sm-Ms;
% Mt
Tw=1-Sw;
Tm=abs(Tw);
cT=Tm-Mt;
Tw = 1-Sw;
Tm = abs(Tw);
cT = Tm-Mt;
% Mks
KFw=fr(K(p)*F(p));
KFm=abs(KFw);
Qm=KFm.*Sm*k0; % Don't forget the regularization
cQ=Qm-Mks;
KFw = fr(K(p)*F(p));
KFm = abs(KFw);
KSm = KFm.*Sm*k0; % Undo regularization
cKS = KSm-Mks;
% Constraints
c=[cS cT cQ];
cEq=[];
c = [cS cT cKS];
cEq = [];
% Gradients
KF_pw=fr(KF_p(p));
Sm_p=-mdot(Sm,real(mdot(Sw.*Pw,KF_pw)));
Tm_p=mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,KF_pw)));
Qm_p=mdot(Sm./KFm,real(mdot(conj(KFw).*Sw,KF_pw)));
c_p=[Sm_p;Tm_p;Qm_p].';
cEq_p=[];
KF_pw = fr(KF_p(p));
Sm_p = -mdot(Sm,real(mdot(Sw.*Pw,KF_pw)));
Tm_p = mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,KF_pw)));
Qm_p = mdot(Sm./KFm,real(mdot(conj(KFw).*Sw,KF_pw)));
c_p = [Sm_p;Tm_p;Qm_p].';
cEq_p = [];
end
end
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment