Commit 3e942316 authored by Kristian Soltesz's avatar Kristian Soltesz
Browse files

Added code for tuning IAE-optimal robust PID

parent 57ac81d1
function [K,p,w] =pidIAE(P,Ms,Mt,Mks,p,wN,N)
% 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
%
% 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 [].
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
% optional argument defaults
if N == []
N=500; % number of grid points
end
% regularize
if p(1)~=0
k0=p(1);
p=p/k0;
P=P*k0;
else
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)';
% 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));
% frequency responses
Pw=fr(P);
K_pw=fr(K_p);
% 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 = 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));
% 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
function [c,cEq,c_p,cEq_p]=cNL(p)
% Ms
Sw=fr(S(p));
Sm=abs(Sw);
% Mt
Tw=1-Sw;
Tm=abs(Tw);
% Mks
Kw=fr(K(p));
Km=abs(Kw);
KSm=Km.*Sm*k0; % don't forget the regularization
% 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=[];
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