Code covered by the BSD License  

Highlights from
Reduced-order discrete-time LQG design for systems with white parameters

Reduced-order discrete-time LQG design for systems with white parameters

by

 

22 May 2008 (Updated )

Optimal compensation of time-varying discrete-time linear systems with white stochastic parameters

[xfkl,sigp,sigs,ptt,stt,pht,sht]=sprotvv(func,opt,ptt,stt,pht,sht);
% SPROTVV:Stochastic Parameter Reduced Order Time Varying LQG compensation.
%         Optimal compensation of (P(i),G(i),C(i),V(i),W(i),x0m,x0c) based on
%         (Q(i),R(i),H) and nc(i)<=nx.
%         (P(i),G(i),C(i)) stochastic.
%          Time shift i=0,1,..,N -> i=1,2,..,N+1
%
%         [xfkl,sigp,sigs,ptt,stt,pht,sht]=sprotvv(func,opt,ptt,stt,pht,sht);
% Input:
%          func  : string containing the function name of the function that
%                  specifies all problem parameters (see e.g. cospfutv).
%          opt   : optional array containing algorithm parameter settings.
%          opt(1): convergence tolerance, epsl, default 1e-6. 
%          opt(2): iteration limit, maxtps, default 1e20.
%          opt(3): maximum number of iterations (sweeps), itm, default 5000. 
%          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): not used
%         opt(10): stepsize homotopy parameter, default 1
%      opt(10:..): individual values homotopy parameter, default none
% ptt,stt,pht,sht: initial values ptt,stt,pht,sht (optional)
%
% Output:
%          Time shift i=0,1,..,N -> i=1,2,..,N+1
%          xfkl : Optimal compensator
%                 [x1 f1 k1 l1' f2 k2 l2' ... fN kN lN']
%                 The reduced-order compensator matrices are then:
%                 x1(1:nc(1); fi(1:nc(i+1),1:nc(i))
%                 ki(1:nc(i+1),:); li(:,1:nc(i))
%                 (the other elements of xfkl are zero)
%          sigp,sigs: Minimum costs (if sigp=sigs)
%          ptt, stt, pht, sht: Solution optimal projection equations.
%
% Comments:
%          See also sproeqc sproeqe spcomco gmhfac
%
%          W.L. De Koning, L.G. Van Willigenburg 28-11-95.
%
  function [xfkl,sigp,sigs,ptt,stt,pht,sht]=sprotvv(func,opt,ptt,stt,pht,sht);

%
% Check number of input arguments and get
% problem data from func
%
  if nargin==1; no=0;
  elseif nargin==2; no=max(size(opt));
  elseif nargin==6; no=max(size(opt));
  else; error('one,two or six input arguments required'); end;
  [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,pgva,pcva,gva,cva,v,w,q,r,mc,me,nc,H,x0c,N,x0m]=feval(func,1);
  for i=1:N
    [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me,nc,H,x0c,N,x0m]=feval(func,i);
    v=[v me]; q=[q mc]; [nx,ny,nu,q,mc,v,me]=pgcchk(pm,gm,cm,v,w,q,r);
    [nxs,nus,nys]=varchk(nx,nu,ny,pms,gms,cms,pva,gva,cva,pgms,pcms,pgva,pcva);
  end;
  [nx,nu]=size(gm);[ny,nx]=size(cm); nt=(N+1)*nx;
%
% Algorithm parameter settings.
%
  opth=zeros(1,8);
  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=1e20; end
  if opt(3)>0; itm=opt(3); else; itm=5000; 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 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;
%
% Boundary conditions recursions
%
  ptb=x0c; phb=x0m*x0m' ;stf=H ; shf=zeros(nx,nx);
%
% Initializations
%
  nsweeps=0; endloop=0; tr=1; trr=1; trt=[];
  if nargin~=6
%
% Set initial pt, st, ph ,sh
%
    ptt=zeros(nx,nt); pht=ptt;
    pn2=nx; pn1=pn2-nx+1;
    ptt(:,pn1:pn2)=ptb; pht(:,pn1:pn2)=phb;
    for i=1:N
       pn2=pn2+nx; pn1=pn1+nx;
       [ph]=iniphsh(ic,nx,nc(i));
       pht(:,pn1:pn2)=ph;
    end;

    stt=zeros(nx,nt); sht=stt;
    pn2=nx; pn1=pn2-nx+1;
    for i=1:N
       [sh]=iniphsh(ic,nx,nc(i));
       sht(:,pn1:pn2)=sh;
       pn2=pn2+nx; pn1=pn1+nx;
    end;
    stt(:,pn1:pn2)=stf; sht(:,pn1:pn2)=shf;
  else
    pn2=nt; pn1=pn2-nx+1;
    stt(:,pn1:pn2)=stf; sht(:,pn1:pn2)=shf;
    pn2=nx; pn1=pn2-nx+1;
    ptt(:,pn1:pn2)=ptb; pht(:,pn1:pn2)=phb;
  end;

  tat=zeros(nx,nt);
%
% St,Sh,Pt,Ph loop
%
  for a=av; endloop=0; conver=0; trps=[];
  while endloop~=1

    nsweeps=nsweeps+1;

    tro=tr; pn2=nx; pn1=pn2-nx+1;
    for i=1:N+1
      ph=pht(:,pn1:pn2); sh=sht(:,pn1:pn2);
      [ta]=gmhfac(ph,sh,nc(i),a,m,rtol);
      tat(:,pn1:pn2)=ta;
      pn2=pn2+nx; pn1=pn1+nx;
    end;

    if a~=0 & bb~=0; stto=stt; shto=sht; end;
    pn2=nt; pn1=pn2-nx+1;
    st=stt(:,pn1:pn2); sh=sht(:,pn1:pn2);
    for i=N:-1:1
      pn2=pn2-nx; pn1=pn1-nx;
      pt=ptt(:,pn1:pn2); ph=pht(:,pn1:pn2); ta=tat(:,pn1:pn2);
      [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me]=feval(func,i);
      [st,sh]=sproeqc(pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,...
                      v,w,q,r,mc,me,st,sh,pt,ph,ta,1);
      stt(:,pn1:pn2)=st; sht(:,pn1:pn2)=sh;
    end
    if a~=0 & bb~=0; stt=(1-bb)*stt+bb*stto; sht=(1-bb)*sht+bb*shto; end;

    if a~=0 & bb~=0; ptto=ptt; phto=pht; end;
    pn2=nx; pn1=pn2-nx+1;
    pt=ptt(:,pn1:pn2); ph=pht(:,pn1:pn2);
    for i=1:N
      pn2=pn2+nx; pn1=pn1+nx;
      st=stt(:,pn1:pn2); sh=sht(:,pn1:pn2); ta=tat(:,pn1:pn2);
      [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me]=feval(func,i);
      [pt,ph]=sproeqe(pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,...
                      v,w,q,r,mc,me,st,sh,pt,ph,ta,1);
      ptt(:,pn1:pn2)=pt; pht(:,pn1:pn2)=ph;
    end
    if a~=0 & bb~=0; ptt=(1-bb)*ptt+bb*ptto; pht=(1-bb)*pht+bb*phto; end;

    tr=trace(pt+stt(:,1:nx)); trt=[trt tr];
    trr=abs((tr-tro)/(tr+tro));
%
% Test convergence.
%
    if nsweeps>itm
      disp('Maximum number of iterations (sweeps) exceeded');
      endloop=1; sigp=Inf; sigs=sigp; sigc=sigp;
    elseif trr>maxtps
      disp('Algorithm failed, divergence');
      endloop=1; sigp=Inf; sigs=sigp; sigc=sigp;
    elseif trr<epsl;
      conver=conver+1;
    end

    if conver==2; bb=0.9*b;
    elseif conver==3; endloop=1; sigp=0;
    else; bb=b; end;
%
% Display progress computation.
%
    if (itc>0 & rem(nsweeps,itc)==0) | (itc~=0 & endloop==1)
      str=['SPROTVV alpha=' num2str(a)];
      plconv(trt,trr,epsl,endloop,str,'Trace(P(N)+S(0))',nx,nc);
    end;
  end;
  trps=[trps tr];
  end;

  if ~isinf(sigp)
%
% Final update tat, tgt, tht
%
    tgt=zeros(nx,nt); tht=tgt;
    pn2=nx; pn1=pn2-nx+1;
    for i=1:N+1
      ph=pht(:,pn1:pn2); sh=sht(:,pn1:pn2);
      [ta,tg,th]=gmhfac(ph,sh,nc(i),a,m,rtol);
      zxc=zeros(nx-nc(i),nx);
      tat(:,pn1:pn2)=ta;
      tgt(:,pn1:pn2)=[tg; zxc];
      tht(:,pn1:pn2)=[th; zxc];
      pn2=pn2+nx; pn1=pn1+nx;
    end;
%
% Compute initial state compensator
%
    xc=tht(:,1:nx)*x0m; xfkl=xc;
%
% Compute cost and compensator matrices
%
    sigp=0; sigs=0; pn2=nx; pn1=pn2-nx+1;
    for i=1:N
      st=stt(:,pn1+nx:pn2+nx); sh=sht(:,pn1+nx:pn2+nx);
      pt=ptt(:,pn1:pn2); ph=pht(:,pn1:pn2);
      [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me]=feval(func,i);
      [sigpi,sigsi,ko,lo]=spcomco(pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,q,r,v,w,mc,me,st,sh,pt,ph);
      sigp=sigp+sigpi; sigs=sigs+sigsi;
      th=tht(:,pn1+nx:pn2+nx); tg=tgt(:,pn1:pn2);
      tg=tg(1:nc(i),:); th=th(1:nc(i+1),:);
      f=th*(pm-gm*lo-ko*cm)*tg';k=th*ko;l=lo*tg';
      z1=zeros(nc(i+1),nx-nc(i)); z2=zeros(nx-nc(i+1),nx);
      zk=zeros(nx-nc(i+1),ny); zl=zeros(nx-nc(i),nu);
      f=[f z1;z2]; k=[k;zk]; l=[l zl'];
      xfkl=[xfkl f k l'];
      pn2=pn2+nx; pn1=pn1+nx;
    end
%
% Add costs associated to boundary conditions
%
    pn2=nx;pn1=pn2-nx+1;
    st=stt(:,pn1:pn2);sh=sht(:,pn1:pn2);
    pn2=(N+1)*nx;pn1=pn2-nx+1;
    pt=ptt(:,pn1:pn2);ph=pht(:,pn1:pn2);
    sigp=sigp+trace(stf*(pt+ph)+shf*pt);
    sigs=sigs+trace(ptb*(st+sh)+phb*st);
  else;
    sigc=Inf; sigs=sigp; xfkl=[];
  end

Contact us