Code covered by the BSD License  

Highlights from
Optimal reduced-order discrete-time LQG design

Optimal reduced-order discrete-time LQG design

by

 

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);
% DPROTINO: Erroneous 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. Based on the erroneous CDOPE!
%
%          [f,k,l,sigp,sigs,spr,pt,st,ph,sh,sigc,trps]=dprotino(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]=dprotino(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)
%
% Comments:
%          See also dproeqo, gmhfac
%
%          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]=dproeqo(ptc,phc,stc,shc,p,g,c,v,w,q,r,mc,me,nc,a,bb,m,rtol,it);
      [osc,endc,conver,bb,nocon,tps,tpst,b,iti]=rocochk(osc,conver,bb,it,itm,itc,ptc,stc,...
       tpso,tpst,epsl,maxtps,'DPROTINO','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
    sigc=Inf; sigs=Inf; sigp=Inf; spr=sperad(p); f=[]; k=[]; l=[];
  end;

Contact us