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 % Computes PID parameters, that minimize load step IAE, and filter for
% noise attenuation. % noise attenuation.
% %
...@@ -17,12 +17,12 @@ function [KF,p,w]=pidfIAE(P,Ms,Mt,Mks,pK,w) ...@@ -17,12 +17,12 @@ function [KF,p,w]=pidfIAE(P,Ms,Mt,Mks,pK,w)
% See the git repo for further documentation. % See the git repo for further documentation.
% regularize % regularize
if pK(1) ~= 0 if pC(1) ~= 0
k0 = pK(1); c0 = pC(1);
pK = pK/k0; pC = pC/c0;
P = P*k0; P = P*c0;
else else
k0 = 1; c0 = 1;
end end
% Frequency grid (heuristic) - change as needed % Frequency grid (heuristic) - change as needed
...@@ -35,9 +35,9 @@ Tmin = 1/w(end); ...@@ -35,9 +35,9 @@ Tmin = 1/w(end);
pF = Tmin; pF = Tmin;
% Organize parameters parameters % Organize parameters parameters
p = [pK(:);pF(:)]; p = [pC(:);pF(:)];
pK = @(p)p(1:3); pC = @(p)p(1:3);
pF = @(p)p(4); % FIXME: rescale to fit Lund parametrization of filter pF = @(p)p(4);
w = w(:); w = w(:);
p(4) = max(p(4),Tmin); p(4) = max(p(4),Tmin);
...@@ -52,22 +52,23 @@ Pw = fr(P); % Frequency response ...@@ -52,22 +52,23 @@ Pw = fr(P); % Frequency response
% Transfer functions % Transfer functions
s = zpk('s'); s = zpk('s');
K1 = [1;1/s;s]; C1 = [1;1/s;s];
K = @(p)pK(p).'*K1; C = @(p)pC(p).'*C1;
F = @(p)1/((pF(p)*s)^2+2*z*pF(p)*s+1); % Alternative parametrization F = @(p)1/((pF(p)*s)^2+2*z*pF(p)*s+1);
S = @(p)feedback(1,P*K(p)*F(p)); K = @(p)series(F(p),C(p));
S = @(p)feedback(1,series(P,K(p)));
% Sensitivities % Sensitivities
K_pK = @(p)K1; % dK/dpK C_pC = @(p)C1; % dK/dpC
F_pF = @(p)(-2)*F(p)^2*s*(s*pF(p)+z); % dF/dpF 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 K_p = @(p)[F(p)*C_pC(p);C(p)*F_pF(p)]; % dK/dp
% Optimization % Optimization
cfg = optimset('algorithm','active-set','GradConstr','on','GradObj',... cfg = optimset('algorithm','active-set','GradConstr','on','GradObj',...
'on','Display','off','maxIter',10); 'on','Display','off','maxIter',10);
p = fmincon(@(p)J(p),p,[],[],[],[],[eps 0 0 Tmin],[],@(p)cNL(p),cfg); p = fmincon(@(p)J(p),p,[],[],[],[],[eps 0 0 Tmin],[],@(p)cNL(p),cfg);
p(1:end-1) = p(1:end-1)*k0; p(1:end-1) = p(1:end-1)*c0;
KF = minreal(K(p)*F(p)); K = minreal(K(p));
% Objective and gradient % Objective and gradient
function [J,J_p] = J(p) function [J,J_p] = J(p)
...@@ -75,7 +76,7 @@ KF = minreal(K(p)*F(p)); ...@@ -75,7 +76,7 @@ KF = minreal(K(p)*F(p));
[e,t] = step(P*S(p)); [e,t] = step(P*S(p));
[e,t] = resample(e,t); [e,t] = resample(e,t);
J = trapz(t,abs(e)); 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)); J_p = trapz(t,mdot(sign(e),e_p));
end end
...@@ -92,9 +93,9 @@ KF = minreal(K(p)*F(p)); ...@@ -92,9 +93,9 @@ KF = minreal(K(p)*F(p));
cT = Tm-Mt; cT = Tm-Mt;
% Mks % Mks
KFw = fr(K(p)*F(p)); Kw = fr(C(p)*F(p));
KFm = abs(KFw); Km = abs(Kw);
KSm = KFm.*Sm*k0; % Undo regularization KSm = Km.*Sm*c0; % Undo regularization
cKS = KSm-Mks; cKS = KSm-Mks;
% Constraints % Constraints
...@@ -102,10 +103,10 @@ KF = minreal(K(p)*F(p)); ...@@ -102,10 +103,10 @@ KF = minreal(K(p)*F(p));
cEq = []; cEq = [];
% Gradients % Gradients
KF_pw = fr(KF_p(p)); K_pw = fr(K_p(p));
Sm_p = -mdot(Sm,real(mdot(Sw.*Pw,KF_pw))); Sm_p = -mdot(Sm,real(mdot(Sw.*Pw,K_pw)));
Tm_p = mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,KF_pw))); Tm_p = mdot(Tm,real(mdot(Sw.^2.*Pw./Tw,K_pw)));
Qm_p = mdot(Sm./KFm,real(mdot(conj(KFw).*Sw,KF_pw))); Qm_p = mdot(Sm./Km,real(mdot(conj(Kw).*Sw,K_pw)));
c_p = [Sm_p;Tm_p;Qm_p].'; c_p = [Sm_p;Tm_p;Qm_p].';
cEq_p = []; cEq_p = [];
end 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