Code covered by the BSD License

# Optimal reduced-order discrete-time LQG design

### Gerard Van Willigenburg (view profile)

16 May 2008 (Updated )

Solution of the SDOPE by repeated forward and backward iteration

[tau,ga,ha,ma]=gmhfac(ph,sh,nc,a,b,tol)
```%GMHFAC: gmh alpha factorization of ph*sh, ph>=0, sh>=0, ph=ph', sh=sh',
%         r=rank(ph)=rank(sh) (=rank(ph*sh))
%
%         [tau,ga,ha,ma]=gmhfac(ph,sh,nc,a,b,tol)
%
%         The function computes a (g,m,h) factorization of ph*sh i.e.
%         ph*sh=g'*m*h, h*g'=eye(rank(ph*sh)), m nonsingular.
%
%         Then:
%         1) gginv(ph*sh)=g'*inv(m)*h, gginv:group generalized inverse.
%         2) tau=g'*h=ph*sh*gginv(ph*sh).
%            tau*tau=tau i.e. tau is idempotent (oblique projection).
%
%         The factorization is unique up to a change of basis.
%         This routine computes a factorization for which m is diagonal.
%
%         a   : homotopy parameter (optional)
%         b   : a switch that determines the method of computation (optional)
%           b=1 : Computation using eigenvectors
%           b=2 : Computation using svd's of ph sh
%                 faster, convergence worse compared to b=1
%           b=3 : Computation using the group inverse
%                 fastest, convergence comparable to b=1
%                 but sensitive to loss of rank
%         tol : tolerance to determine rank(ph*sh) (optional)
%
%
%         L.G. Van Willigenburg, W.L. De Koning, 05-07-96.
%
function [tau,ga,ha,ma]=gmhfac(ph,sh,nc,a,b,tol)

[nx,nx]=size(ph); zx=zeros(nx);

% Default b : method of computation
bd=1;

% Default tolerances for rank(ph*sh) determination
tola=1e-12; told=1e-9;

% Check input
if nargin==3; a=1; b=bd; tol=told;
elseif nargin==4; b=bd; tol=told;
elseif nargin==5;
if isempty(a); a=1; end; tol=told;
elseif nargin==6;
if isempty(a); a=1; end;
if isempty(b); b=bd; end;
else
error('4-7 input arguments required');
end;

if a==0; tau=eye(nx); ga=tau; ha=tau; ma=tau;
else
if a~=1; nh=nx; else; nh=nc; end;  phsh=ph*sh;
if phsh==zx;
tau=zx; ga=zeros(nh,nx); ha=ga; ma=zeros(nh,nh);

elseif b==3
%
% Computation using group inverse
% fastest but sensitive to loss of rank
%
tau=phsh*gginv(phsh,nc,tol);
if a~=1; tau=a*tau+(1-a)*eye(nx); end;

if nargout>1
[e,s,f]=svd(tau); s=diag(s); s=s(1:nc);
e=e(:,1:nc); f=f(:,1:nc);
sqs=diag(sqrt(s)); ga=sqs*e'; ha=sqs*f'; ma=[];
end;

elseif b==1
%
% Computation using eigenvectors
%
tolinv=1e-6; flag=1;
ind=0; epsind=1e-13;
while flag==1 & ind<nx
ind=ind+1;
if ind==1; phshh=phsh;
%       add small random matrix to phsh
else; phshh=phsh+epsind*psdrn(nx); end;
[e,l]=eig(phshh);
if max(max(isnan(e)))==1 | max(max(isinf(e)))==1
flag=1;
else
[ls,ils]=esort(diag(l)); es=e(:,ils);
[esi,flag]=pinvrd(es,tolinv);
end;
epsind=10*epsind;
%       if ind>1; disp(['gmhfac: recomputation eig trial ' num2str(ind)]); end
end

if flag==1;
%disp('  warning: eig version of gmhfac failed svd version used');
b=2;
end;

als=abs(ls);
if max(als)<tola
tau=zeros(nx); ga=tau; ha=tau; ma=tau;
else
ncc=min(nc,sum(als > tol*ls(1)));
ia=diag([ones(ncc,1); (1-a)*ones(nx-ncc,1)]); sia=sqrt(ia);
ga=real(sia*es'); ha=real(sia*esi); ma=diag(ls); tau=real(es*ia*esi);
end

ga=ga(1:nh,:); ha=ha(1:nh,:); ma=ma(1:nh,1:nh);

end

if b==2
%
% Computation using svd's of ph sh
% Faster but numerically less stable
%
[up,sp,vp]=svd(ph); sp=diag(sp);
[us,ss,vs]=svd(sh); ss=diag(ss);
up=up*diag(sqrt(sp)); us=us*diag(sqrt(ss));

ncc=min(nc,sum(sp > tol*sp(1)));
ncc=min(ncc,sum(ss > tol*ss(1)));

if a==1 up=up(:,1:nc); us=us(:,1:nc); end;

%     [up]=cholut(ph,tol,-1e-12); sp=diag(up);
%     [us]=cholut(sh,tol,-1e-12); ss=diag(us);

%     ncc=min(nc,sum(sp > tol*max(sp)));
%     ncc=min(ncc,sum(ss > tol*max(ss)));

%     if a==1 up=up(:,nx-nc+1:nx); us=us(:,nx-nc+1:nx); end;

[u,s,v]=svd(up'*us); s=diag(s);

if ncc==0 | max(s)<tola
tau=zeros(nx); ga=tau; ha=tau; ma=tau;
else
if a==1
s2=diag([ones(ncc,1)./sqrt(s(1:ncc)); zeros(nc-ncc,1)]);
else
s2=diag([ones(ncc,1)./sqrt(s(1:ncc)); (1-a)*ones(nx-ncc,1)]);
end
ga=(up*u*s2)'; ma=diag(s.*s); ha=s2*v'*us'; tau=ga'*ha;
end

ga=ga(1:nh,:); ha=ha(1:nh,:); ma=ma(1:nh,1:nh);

end
end
```