Added code for tuning IAE-optimal robust PID

pidIAE.m

+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