%function [Ac,Bc,Cc,Dc,gammin] = coprimeunc(a,b,c,d,gamrel)
%
% Finds the controller which optimally robustifies a given
% shaped plant in terms of tolerating maximum coprime uncertainty.
% Used in the McFarlane-Glover H-infinity loopshaping procedure.
% --- Uses the robust control toolbox ---
%
%    a,b,c,d:     State-space description of (shaped) plant  
%    gamrel:      Final gamma used is gamrel*gammin [default: gamrel=1.1]
%    Ac,Bc,Cc,Dc: State-space description of "robustifying" controller
%                 assuming positive feedback

function [Ac,Bc,Cc,Dc,gammin] = coprimeunc(a,b,c,d,gamrel)
if nargin <4, 
   disp('usage:  [Ac,Bc,Cc,Dc] = coprimeunc(a,b,c,d,gamrel)'); return; end
if nargin <5, gamrel=1.1; end

% Find Normalized Coprime factors of the shaped plant
S=eye(size(d'*d))+d'*d;
R=eye(size(d*d'))+d*d';
Rinv=inv(R);Sinv=inv(S);

A1 = a-b*Sinv*d'*c; Q1 = c'*Rinv*c; R1 = b*Sinv*b'; 
[X1,X2,XAMP,XERR,XWELLPOSED,X]=aresolv(A1,Q1,R1); XWELLPOSED
% Alt. Mu toolbox: 
%[x1,x2, fail, reig_min] = ric_schr([A1 -R1; -Q1 -A1']); X = x2/x1;

A2 = A1'; Q2 = R1; R2 = Q1;
[Z1,Z2,ZAMP,ZERR,ZWELLPOSED,Z]=aresolv(A2,Q2,R2); ZWELLPOSED
% Alt. Mu toolbox: 
%[x1, x2, fail, reig_min] = ric_schr([A2 -R2; -Q2 -A2']); Z = x2/x1; 

% display optimal gamma
XZ = X*Z; gammin=sqrt(1+max(eig(XZ)))

% Use higher gamma 
gam=gamrel*gammin; gam2 = gam*gam; gamconst = (1-gam2)*eye(size(XZ)); 
Lc = gamconst + XZ; Li = inv(Lc'); Fc = -Sinv*(d'*c+b'*X);
Ac = a + b*Fc + gam2*Li*Z*c'*(c+d*Fc);
Bc = gam2*Li*Z*c';
Cc = b'*X; 
Dc = -d';
%---------------------------------------------------------------------
