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

Added code for tuning IAE-optimal robust PID with filter

parent 3e942316
function [KF,p,w]=getK2F(P,Ms,Mt,Mks,pK)
% 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)
%
% 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].
% regularize
if pK(1)~=0
k0=pK(1);
pK=pK/k0;
P=P*k0;
else
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)';
% Ensure HF roll-off
Tmin=2/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(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
% Misc
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));
% 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
% 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);
p(1:end-1) = p(1:end-1)*k0;
KF=minreal(K(p)*F(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));
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;
% 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;
% Constraints
c=[cS cT cQ];
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=[];
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