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

[f,k,l,sigp,sigs,spr,pt,st,ph,sh,sigc,trps,it]=dprotin(p,g,c,v,w,q,r,nc,opt,pt,st,ph,sh);
```% DPROTIN: Deterministic Parameter Reduced-Order Time-Invariant iNfinite horizon LQG compensation.
%          Optimal compensation of (P,G,C,V,W) based on (Q,R) and nc<=nx.
%          (P,G,C) deterministic.
%
%          [f,k,l,sigp,sigs,spr,pt,st,ph,sh,sigc,trps]=dprotin(p,g,c,v,w,q,r,nc,opt,pt,st,ph,sh);
% or
%          [f,k,l,sigp,sigs,spr,pt,st,ph,sh,sigc,trps]=dprotin(func,opt,pt,st,ph,sh);
%
% Input:
%          func  : string containing the function name of the function that
%                  specifies all problem parameters (see e.g. cofu).
%              q : q or [q mc] mc=cross term criterion
%              v : v or [v me] me=cross covariance
%          opt   : optional array containing algorithm parameter settings.
%          opt(1): convergence tolerance, epsl, default 1e-6.
%          opt(2): iteration limit, maxtps, default 1e10.
%                  iteration stops if trace(pt+st)>maxtps.
%          opt(3): maximum number of iterations, itm, default 10000.
%          opt(4): plot parameter, itc, default 0.
%                  0    : no plot.
%                  <0   : plot at the end
%                  >0   : plot every itc iterations.
%          opt(5): numerical damping coefficient, 0<b<1, default 0.25.
%                  if opt(5)>1 | opt(5)<0 then b=0
%          opt(6): initial conditions ph, sh, ic=1,2,3, default 1
%                  ic=1: 0.9*eye(nx)+0.1*ones(nx)
%                  ic=2: random
%                  ic=3: eye(nx)
%          opt(7): method to compute projection, m=1,2,3, default 1
%                  m=1: Eigenvalues
%                  m=2: SVD's
%                  m=3: Group inverse
%          opt(8): relative tolerance rank(ph*sh), rtol default 1e-9
%          opt(9): Automatic adjustment numerical damping, cc
%                  cc=0 activated (default) otherwise deactivated
%         opt(10): stepsize homotopy parameter
%      opt(10:..): individual values homotopy parameter, default none
%     pt,st,ph,sh: initial values pt,st,ph,sh
%
% Output:
%          f, k, l     : Optimal compensator.
%          sigp, sigs  : Minimum costs (if sigp=sigs).
%          pt,st,ph,sh : Solution optimal projection equations.
%          sigc        : Costs of computed compensator.
%                        (compensator optimal: sigc=sigp=sigs).
%          trps         : Trace(pt+st) (array in case of homotopy algorithm)
%
%
%          L.G. Van Willigenburg, W.L. De Koning, 28-11-95.
%

function [f,k,l,sigp,sigs,spr,pt,st,ph,sh,sigc,trps,it]=dprotin(p,g,c,v,w,q,r,nc,opt,pt,st,ph,sh);

%
% Check number of input arguments and get
% problem data from func=p if p is a string
%
if isstr(p);
if nargin==1; no=0;
elseif nargin==2; opt=g; no=max(size(opt));
elseif nargin==6; opt=g; no=max(size(opt));
pt=c; st=v; ph=w; sh=q;
else error('wrong number of input arguments'); end;
func=p; [p,g,c,v,w,q,r,nc]=feval(func);
else
if nargin==8; no=0;
elseif nargin==9 ; no=max(size(opt));
elseif nargin==13; no=max(size(opt));
else error('wrong number of input arguments'); end;
end;
%
% Check dimensions
%
[nx,ny,nu,q,mc,v,me]=pgcchk(p,g,c,v,w,q,r);
[nx1,nx2]=size(nc);
if nx1~=1 | nx2~=1; error('nc must be scalar'); end;
if nc<1 | nc>nx; error('nc incompatible with p'); end;
%
% Algorithm parameter settings.
%
opth=zeros(1,9);
if no~=0; opth(1:no)=opt(1:no); end;
opt=opth;
if opt(1)>0; epsl=opt(1); else; epsl=1e-6; end
if opt(2)>0; maxtps=opt(2); else; maxtps=1e10; end
if opt(3)>0; itm=opt(3); else; itm=10000; end
itc=opt(4);
if opt(5)==0; b=0.25;
elseif opt(5)<0 | opt(5)>1; b=0;
else b=opt(5); end; bb=b;
if opt(6)>=1 & opt(6)<=3; ic=round(opt(6)); else; ic=1; end
if opt(7)>=1 & opt(7)<=3; m=round(opt(7)); else; m=1; end
if opt(8)>0; rtol=opt(8); else; rtol=1e-9; end;
if opt(9)~=0; cc=1; else; cc=0; end;
if no==10; if opt(10)<0 | opt(10)>1;
error('0 < opt(10) < 1 not satisfied')
else; da=1/round(1/opt(10)); av=[0:da:1];
end;
elseif no>10; av=sort(opt(10:no));
av=av-av(1); av=av/av(no-9);
else; av=1; end;
%
% Initialization recursions.
%
it=0; tps=1; tpsr=1; tpst=[]; trps=[];
if nargin~=13 & nargin~=6
pt=zeros(nx); st=pt;
[ph]=iniphsh(ic,nx,nc);
[sh]=iniphsh(ic,nx,nc);
end;
ptc=pt; phc=ph; stc=st; shc=sh;
%
% pt,ph,st,sh loop
%
for a=av;
conver=0; endc=0; osc=0; iti=0;
while endc~=1;

it=it+1; iti=iti+1; tpso=tps;

[ptc,phc,stc,shc,psi1,psi2]=dproeq(ptc,phc,stc,shc,p,g,c,v,w,q,r,mc,me,nc,a,bb,m,rtol);
[osc,endc,conver,bb,nocon,tps,tpst,b,iti]=rocochk(osc,conver,bb,it,itm,itc,ptc,stc,...
tpso,tpst,epsl,maxtps,'DPROTIN','Trace(P+S)',nx,nc,b,a,cc,iti);

if nocon==2;
ptc=pt; phc=ph; stc=st; shc=sh;
end;

end
pt=ptc; ph=phc; st=stc; sh=shc;
trps=[trps tps];
end

if ~nocon;
%
% Determine compensator and costs
%
lo=pinv(g'*st*g+r)*(g'*st*p+mc');
ko=(me+p*pt*c')*pinv(c*pt*c'+w);
fo=p-g*lo-ko*c;
sigp=trace(q*pt+(q+lo'*r*lo -2*mc*lo )*ph);
sigs=trace(v*st+(v+ko *w*ko'-2*me*ko')*sh);
[ta,tg,th]=gmhfac(ph,sh,nc,a,m);
f=th*fo*tg'; k=th*ko; l=lo*tg';
pa=[p -g*l ; k*c f]; spr=sperad(pa);

if nargout>=11
qa=[q -mc*l ; -l'*mc' l'*r*l ];
va=[v  me*k';  k *me' k *w*k'];
pp=dlyap(pa,va) ; sigc=trace(qa*pp);
%      ss=dlyap(pa',qa); sigc=trace(va*ss);
end;
else
disp(' '); disp(['  System may not be nc=' num2str(nc) ' compensatable']);
sigc=Inf; sigs=Inf; sigp=Inf; spr=sperad(p); f=[]; k=[]; l=[];
end;
```