www.gusucode.com > 预览控件工具箱 > 预览控件工具箱/预览控件工具箱/PCT/@PrevDistRejSys/MkHinfPrevK.m

    function [K,E]=MkHinfPrevK(P,N,gam,bFI)
% [K,E]=MkHinfPrevK(P,N,gam,bFI)
%
% Generate Hinf-suboptimal, discrete-time, output feedback controller
% Ref: Green and Limebeer, Linear Robust Control and A Hazell, Discrete-Time Optimal
% Preview control, PhD Thesis, 2008 
%
% N:    Preview length, overrides the preview length associated with P
% gam:  Maximum allowable hinf norm
% bFI:  boolean, true if only the Full Information (FI) controller is required
% E:    Entropy associated with FI controller
% K:    Optimal FI or OF controller

%reps
%====
%0   :  Success!
%-1  :  Could not compute stabilising X
%-2  :  X was not feasible
%-3  :  X was no positive

rep=0;
tol=1e-9;

if N<1
	error('PrevTools:ntoosmall','Need N>0')
end

if isempty(bFI)
	bFI=false;
end

%%%%%%%%%% Extract partitions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[A,B1,B2,C1,C2,D11,D12,D21,D22]=GetSS(P);
[Ag,B1gw,B2g,C1g,C2g,D11g,D12g,D21g,D22g]=GetSS(P.GW); % ss matrices for G with Wz and Ww absorbed
[B1gr,B1gw,D11gr,D11gw,D21gr,D21gw]=GetSSrw(P.GW);
[Ar,Br,Cr,Dr]=ssdata(P.Wr);
[n,p,q,l,m]=Getsz(P);
[ng,p,qg,l,m]=Getsz(P.GW);
lr=P.lr;
lw=P.lw;
nwr=P.nwr;
Lh=[D11';D12']*[C1g D11gr];
Lg=Lh(:,1:ng);


%%%%%%%%%%%%%%%%%%%%Compute Y%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~bFI
	try
		GWg=P.GW(:,lr+1:end);
		GWg=GenSys(ss(GWg.A,GWg.B,GWg.C,GWg.D,GWg.Ts),qg,m);
		Yg=Xinfd(adj(GWg),gam);
		%Yg=0; disp('Warnining: Forcing Yg=0')
		
	catch
		error('PrevTools:Ygcompfailed','Failed to compute Y')
	end

	if max(eig(GetNabla(adj(P.GW(:,lr+1:end)),gam,Yg)))>tol
		error('PrevTools:Ynotfeasible','Y is not feasible')
	end

	if min(eig(Yg))<-tol
		error('PrevTools:Ynotnonnegative','Y is not nonnegative')
	end

end

%%%%%%%%%%%%%%%%%Compute Xgg Rb, and check KFI exists%%%%%%%%%%%%%%%
try
	%[Xg,Xr,Rb]=ComputeXgXr(P,N,gam);
    [Xgg,Rb]=ComputeXggRb(P,N,gam);
catch
	rethrow(lasterror)
end

%Xgg=Xg(:,1:ng);

Rb1=Rb(1:l,1:l);
Rb2=Rb(1:l,l+1:l+m)';
Rb3=Rb(l+1:l+m,l+1:l+m);
Rb3inv=inv(Rb3);

nab=Rb1-Rb2'*Rb3inv*Rb2;
V21=chol(-gam^(-2)*nab);
if norm(V21'*V21+gam^(-2)*nab)>tol
	error('PrevTools:nablanotnegative','Nabla was not negative')
end

Ac_cl_gg=Ag-B2g*Rb3inv*(B2g'*Xgg*Ag+Lg(end-m+1:end,:));
if max(abs(eig(Ac_cl_gg)))>=1
    error('PrevTools:Xnotpositive','X was not non-negative')
end

%%%%%%%%%%%%%%%%%%%%%%%Compute Z %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~bFI
    
    if max(eig(Yg*Xgg))>tol+gam^2
		error('PrevTools:XYconditionnotmet','Spectral radius of XY is greater than gam^2')
	end
    
    Zg=Yg*inv(eye(ng)-gam^-2 * Xgg*Yg);
end

%%%%%%%%%%%%%%%%%%%% Compute B'X %%%%%%%%%%%%%%%%%%%%%%%%%%%%

%If we've got this far then all conditions for existence of K have been
%passed, therefore it is worth computing B'X.

try
	BX=ComputeBX(P,N,gam);
catch
	error('PrevTools:Unknown Error','Unknown error - this really should not have happend')
end


%%%%%%%%%%%%%%%%%%%%Compute Controller%%%%%%%%%%%%%%%%%%%%%%%%%%


Bg=[zeros(ng,lr) B1gw B2g];
Br=[[eye(lr)*Dr;Br] zeros(lr+nwr,m+lw) ];

Lb=[Lh zeros(l+m,lr*(N-1)+nwr)]+[BX(:,1:ng)*Ag BX(:,1:ng)*B1gr BX(:,ng+1:ng+(N-1)*lr) BX(:,ng+(N-1)*lr+1:ng+N*lr)*Cr+BX(:,end-nwr+1:end)*Ar ];
Lb2=Lb(l+1:l+m,:);
Lb1=Lb(1:l,:);


%[A,B,R,L,Q]=GetSSb(SetN(P,N),gam); % for debugging only
%Xcheck=Xinfd(SetN(P,N),gam); % for debugging only
%Lbcheck=L'+B'*Xcheck*A; % for debugging only
%Lbcheck-Lb % for debugging only

Jh=[eye(l) zeros(l,m); zeros(m,l) -gam^2*eye(m)];
[vr3,dr3]=svd(Rb3);
V12=sqrt(dr3)*vr3;
if norm(V12'*V12-Rb3)>1e-6
	error('V12 Decomposition has failed')
end

'Lnab';
Lnab=Lb1-Rb2'*Rb3inv*Lb2;

'nabinv';
nabinv=inv(nab);

if ~bFI
    if P.N~=N 
        error('OF controller computation currently fails if P.N is incorrectly set')
    end
    'A';
    A;
    'B1';
    B1;
    'At';
    At=A-B1*nabinv*Lnab;
    Ct1=V12*Rb3inv*(Lb2-Rb2*nabinv*Lnab);
    Ct2=C2-D21*nabinv*Lnab;
    'Ct';
    Ct=[Ct1;Ct2];

    Bttil=[B1*inv(V21) zeros(n,m)];
    V21inv=inv(V21);
    V12inv=inv(V12);
    Dttil=[V12*Rb3inv*Rb2*V21inv  eye(m);D21*V21inv zeros(q,m)];

	St=Dttil*Jh*Dttil'+Ct(:,1:ng)*Zg*Ct(:,1:ng)';
	Mt=Bttil*Jh*Dttil'+At(:,1:ng)*Zg*Ct(:,1:ng)';

	St1=St(1:m,1:m);
	St2=St(1:m,1+m:end);
	St3=St(1+m:m+q,1+m:m+q);
	Mt2=Mt(:,m+1:m+q);
	St3inv=inv(St3);

	AK=At-Mt2*St3inv*Ct2-B2*V12inv*(Ct1-St2*St3inv*Ct2);
	BK=Mt2*St3inv-B2*V12inv*St2*St3inv;
	CK=-V12inv*Ct1+V12inv*St2*St3inv*Ct2;
	DK=-V12inv*St2*St3inv;

	CKt=inv(eye(m)+DK*D22)*CK;
	DKt=inv(eye(m)+DK*D22)*DK;
	AKt=AK-BK*D22*CKt;
	BKt=BK-BK*D22*DKt;
	K=ss(AKt,BKt,CKt,DKt,P.DistRejGSys.Ts);
else
	K=-inv(Rb3)*[Lb2 Rb2];
end

E=-gam^2*log(abs(det(-gam^-2*nab)));


%*************************************************************