code clean

pidfIAE.m

-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
+C_pC = @(p)C1;                         % dK/dpC
 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
 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