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

code clean

parent 814213e3
function [KF,p,w]=pidfIAE(P,Ms,Mt,Mks,pK,w)
function [K,p,w]=pidfIAE(P,Ms,Mt,Mks,pC,w)
% Computes PID parameters, that minimize load step IAE, and filter for
% noise attenuation.
%
......@@ -17,12 +17,12 @@ function [KF,p,w]=pidfIAE(P,Ms,Mt,Mks,pK,w)
% See the git repo for further documentation.
% regularize
if pK(1) ~= 0
k0 = pK(1);
pK = pK/k0;
P = P*k0;
if pC(1) ~= 0
c0 = pC(1);
pC = pC/c0;
P = P*c0;
else
k0 = 1;
c0 = 1;
end
% Frequency grid (heuristic) - change as needed
......@@ -35,9 +35,9 @@ 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
p = [pC(:);pF(:)];
pC = @(p)p(1:3);
pF = @(p)p(4);
w = w(:);
p(4) = max(p(4),Tmin);
......@@ -52,22 +52,23 @@ Pw = fr(P); % Frequency response
% 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));
C1 = [1;1/s;s];
C = @(p)pC(p).'*C1;
F = @(p)1/((pF(p)*s)^2+2*z*pF(p)*s+1);
K = @(p)series(F(p),C(p));
S = @(p)feedback(1,series(P,K(p)));
% 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
C_pC = @(p)C1; % dK/dpC
F_pF = @(p)(-2)*F(p)^2*s*(s*pF(p)+z); % dF/dpF
K_p = @(p)[F(p)*C_pC(p);C(p)*F_pF(p)]; % dK/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));
p(1:end-1) = p(1:end-1)*c0;
K = minreal(K(p));
% Objective and gradient
function [J,J_p] = J(p)
......@@ -75,7 +76,7 @@ KF = minreal(K(p)*F(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)
e_p = step(minreal(-P^2*K_p(p))*S(p)^2,t); % step(P*dS/dp)
J_p = trapz(t,mdot(sign(e),e_p));
end
......@@ -92,9 +93,9 @@ KF = minreal(K(p)*F(p));
cT = Tm-Mt;
% Mks
KFw = fr(K(p)*F(p));
KFm = abs(KFw);
KSm = KFm.*Sm*k0; % Undo regularization
Kw = fr(C(p)*F(p));
Km = abs(Kw);
KSm = Km.*Sm*c0; % Undo regularization
cKS = KSm-Mks;
% Constraints
......@@ -102,10 +103,10 @@ KF = minreal(K(p)*F(p));
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)));
K_pw = fr(K_p(p));
Sm_p = -mdot(Sm,real(mdot(Sw.*Pw,K_pw)));
Tm_p = mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,K_pw)));
Qm_p = mdot(Sm./Km,real(mdot(conj(Kw).*Sw,K_pw)));
c_p = [Sm_p;Tm_p;Qm_p].';
cEq_p = [];
end
......
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