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