Code covered by the BSD License

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

### Gerard Van Willigenburg (view profile)

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.
%
%
%          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
```