Code covered by the BSD License

# Optimal reduced-order discrete-time LQG design

### Gerard Van Willigenburg (view profile)

16 May 2008 (Updated )

Solution of the SDOPE by repeated forward and backward iteration

[pt,ph,st,sh,psi1,psi2]=dproeq(pt,ph,st,sh,p,g,c,v,w,q,r,mc,me,nc,a,b,m,rtol,it);
```% DPROEQO: one iteration of the CDOPE (Conventional Discrete-time Optimal
%          Projection Equations) for Deterministic Parameter Reduced-Order LQG Design.
%          The CDOPE provide erroneous answers!
%
%          Control and estimation equations of compensator
%          for (P,G,C,V,W) based on (Q,R) and nc=<nx.
%          (P,G,C) deterministic.
%
%          [pt,ph,st,sh,psi1,psi2]=dproeqo(pt,ph,st,sh,p,g,c,v,w,q,r,nc,a,b,m,rtol);
%
%
%          W.L. De Koning, L.G. Van Willigenburg, 28-11-95.
%

function [pt,ph,st,sh,psi1,psi2]=dproeq(pt,ph,st,sh,p,g,c,v,w,q,r,mc,me,nc,a,b,m,rtol,it);
if nargin~=18; rtol=1e-9; end;
%
% full order equations
%
wp=w+c*pt*c'         ; rs=r+g'*st*g;
iwp=pinv(wp)         ; irs=pinv(rs);
kh=me+p*pt*c'        ; lh=g'*st*p+mc';
s1p=kh*iwp*kh'       ; s2s=lh'*irs*lh;
p1p=p-kh*iwp*c       ; p2s=p-g*irs*lh;
psi1=p2s *ph*p2s'+s1p; psi2=p1p'*sh*p1p +s2s;

%
%
[nx,nx]=size(p);
if it==1;
ta=eye(nx);
else;
[ta]=gmhfac(ph,sh,nc,a,m,rtol);
end
tao=eye(nx)-ta;

phn=ta*psi1*ta'; phn=0.5*(phn+phn');
shn=ta'*psi2*ta ; shn=0.5*(shn+shn');

ptn=p *pt*p'-s1p+v+tao *psi1*tao';
stn=p'*st*p -s2s+q+tao'*psi2*tao ;

if a==0; b=0; end;
pt=(1-b)*ptn+b*pt; pt=0.5*(pt+pt');
st=(1-b)*stn+b*st; st=0.5*(st+st');

ph=(1-b)*phn+b*ph;
sh=(1-b)*shn+b*sh;
% keyboard
```