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,trps]=sprotv(func,opt,ptt,stt,pht,sht);
% SPFOTV: Stochastic Parameter Full 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 and dependent.
%
%         [xfkl,sigp,sigs,ptt,stt,pht,sht,trps]=spfotv(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): not used
%          opt(8): not used
%          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']
%          sigp,sigs: Minimum costs (if sigp=sigs)
%          ptt, stt, pht, sht: Solution optimal projection equations.
%
% Comments:
%          See also sproeqc sproeqe spcomco gmhfac dxfkl mintv ctrbtv obsvtv
%
%          W.L. De Koning, L.G. Van Willigenburg 28-11-95.
%
  function [xfkl,sigp,sigs,ptt,stt,pht,sht,trps]=sprotv(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;
%
% Check data
%
  [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,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; nt=(N+1)*nx;
%
% Algorithm parameter settings.
%
  opth=zeros(1,7);
  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 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; tr=1; trr=1; trt=[]; trps=[];
  ex=eye(nx); zx=zeros(nx);
  h1=0.1; exn=(1-h1)*ex+h1*ones(nx);
  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;
       if ic==1; ph=exn;
       elseif ic==2; ph=psdr(nx);
       else; ph=ex; end;
       pht(:,pn1:pn2)=ph;
    end;

    stt=zeros(nx,nt); sht=stt;
    pn2=nx; pn1=pn2-nx+1;
    for i=1:N
       if ic==1; sh=exn;
       elseif ic==2; sh=psdr(nx);
       else; sh=ex; end;
       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;

%
% St,Sh,Pt,Ph loop
%
  for a=av; endloop=0; conver=0;
  while endloop~=1

    nsweeps=nsweeps+1;

    tro=tr;
    if 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);
      [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me]=feval(func,i);
      [st,sh]=spfoeqc(pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,...
                      v,w,q,r,mc,me,st,sh,pt,ph,a);
      stt(:,pn1:pn2)=st; sht(:,pn1:pn2)=sh;
    end
    if bb~=0; stt=(1-bb)*stt+bb*stto; sht=(1-bb)*sht+bb*shto; end;

    if 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);
      [pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,v,w,q,r,mc,me]=feval(func,i);
      [pt,ph]=spfoeqe(pm,gm,cm,pms,gms,cms,pgms,pcms,pva,gva,cva,pgva,pcva,...
                      v,w,q,r,mc,me,st,sh,pt,ph,a);
      ptt(:,pn1:pn2)=pt; pht(:,pn1:pn2)=ph;
    end
    if 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)); trps=[trps tr];
%
% 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=['SPFOTV alpha=' num2str(a)];
      plconv(trt,trr,epsl,endloop,str,'Trace(P(N)+S(0))',nx,nx);
    end;
  end;
  end;

  if ~isinf(sigp)

%
% Compute initial state compensator
%
    xfkl=x0m;
%
% 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;
      fo=pm-gm*lo-ko*cm;
      xfkl=[xfkl fo ko lo'];
      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);
  end

Contact us