hinfini.m

通过得到一个悬浮系统的状态空间表达式

%  [a,b,c,d,e]=ltiss(sys)
%
%  Extracts the state-space matrices A,B,C,D,E from the SYSTEM
%  matrix representation SYS of the LTI system
%                                        -1
%                G(s)  =  D + C (s E - A)  B
%
%  WARNING:  if the output argument E is omitted, LTISS returns
%            the realization (E\A, E\B, C, D)  of  G(s)
%
%  Input
%    SYS          SYSTEM matrix created with LTISYS
%
%  Output
%    A,B,C,D,E    state-space matrices of SYS
%
%
%  See also  LTISYS, LTITF, SINFO.

%  Author: P. Gahinet  6/94
%  Copyright 1995-2004 The MathWorks, Inc.
%       $Revision: 1.1.8.1 $

function [a,b,c,d,e]=ltiss(P)

[rp,cp]=size(P);
if ~islsys(P),
  error('SYS is not a SYSTEM matrix');
end

na=P(1,cp);

a=P(1:na,1:na); b=real(P(1:na,na+1:cp-1));
c=real(P(na+1:rp-1,1:na)); d=real(P(na+1:rp-1,na+1:cp-1));
e=imag(a);  a=real(a);

desc=max(max(abs(e))) > 0;
e=e+eye(na);

if nargout < 5 & desc, a=e\a; b=e\b; end

% [gopt,K] = hinflmi(P,r)
% [gopt,K,X1,X2,Y1,Y2] = hinflmi(P,r,g,tol,options)
%
% Given the continuous-time plant P(s), computes the best
% H-infinity performance GOPT as well as an H-infinity
% controller  K(s)  that
%    * internally stabilizes the plant, and
%    * yields a closed-loop gain no larger than GOPT.
% HINFLMI implements the LMI-based approach.
%
% To compute only GOPT, call HINFLMI with only one output argument.
% The input arguments  G, TOL, and OPTIONS  are optional.
%
% Input:
%  P          plant SYSTEM matrix  (see LTISYS)
%  R          1x2 vector specifying the dimensions of D22. That is,
%                      R(1) = nbr of measurements
%                      R(2) = nbr of controls
%  G          user-specified target for the closed-loop performance.
%             Set G=0 to compute GOPT, and set  G=GAMMA  to test
%             whether the performance GAMMA is achievable.
%  TOL        relative accuracy  required on GOPT   (default=1e-2)
%  OPTIONS    optional 3-entry vector of control parameters for
%             the numerical computations.
%             OPTIONS(1): valued in [0,1] (default=0). Reduces the
%                         norm of R when increased  -> slower
%                         controller dynamics
%             OPTIONS(2): same for S
%             OPTIONS(3): default = 1e-3. Reduced-order synthesis is
%                         performed whenever
%                          rho(X*Y) >=  ( 1 - OPTIONS(3) ) * GOPT^2
%             OPTIONS(4): displays progress of the LMI solution to the
%                         screen if OPTIONS(3)=0. Default is 'On'.
%
% Output:
%  GOPT       best H-infinity performance
%  K          central H-infinity controller for gamma = GOPT
%  X1,X2,..   X = X2/X1  and  Y = Y2/Y1  are solutions of the
%             two H-infinity Riccati inequalities for gamma = GOPT.
%             Equivalently,  R = X1  and  S = Y1  are solutions
%             of the characteristic LMIs since X2=Y2=GOPT*eye .
%
%
% See also  HINFRIC, HINFMIX, HINFGS.

% Author: P. Gahinet  10/93
% Copyright 1995-2004 The MathWorks, Inc.
%       $Revision: 1.1.8.1 $

% Reference:
%  Gahinet and Apkarian , "A Linear Matrix Inequality Approach
%  to H-infinity Control," Int. J. Robust and Nonlinear Contr.,
%  4 (1994), pp. 421-448.


function [gopt,Kcen,x1,x2,y1,y2] = hinflmi(P,r,gmin,tol,options)

if nargin <2,
  error('usage:  [gopt,K] = hinflmi(P,r,g,tol,options)');
elseif length(r)~=2,
  error('R must be a two-entry vector');
elseif min(r)<=0,
  error('The entries of R must be positive integers');
else
  if nargin < 5, options=[0 0 0 0]; end
  if nargin < 4, tol=1e-2;  end
  if nargin < 3, gmin=0;  end
end
if length(options)==3
   options(4) = 0;
end
tolred=options(3);
if tolred==0, tolred=1e-3; end

macheps=mach_eps;
gopt=[]; Kcen=[]; x1=[]; x2=[]; y1=[]; y2=[];



% compute the optimal performance in the interval [gmin,gmax]
%------------------------------------------------------------

if options(4)==0
   disp(sprintf('\n Minimization of gamma:'));
end


[gopt,x1,x2,y1,y2]=goptlmi(P,r,gmin,tol,options);

if isempty(gopt),
  disp('HINFLMI:  The LMI optimization failed!');
  return
else
  if options(4)==0
     disp(sprintf(' Optimal Hinf performance:  %6.3e \n',gopt));
  end
end
if nargout <=1, return, end



% compute the central controller
%-------------------------------


[Kcen,gopt,flag]=klmi(P,r,gopt,x1,x2,y1,y2,tolred);



% post-analysis
%--------------

[ak,bk,ck,dk]=ltiss(Kcen);
[a,b1,b2,c1,c2]=hinfpar(P,r);

if flag(1) & flag(2),
   str='OPTIONS(1:2) or ';
elseif flag(1)
   str='OPTIONS(1) or ';
elseif flag(2)
   str='OPTIONS(2) or ';
else str='';
end

if max(real(eig([a+b2*dk*c2,b2*ck;bk*c2,ak]))) >= 0,
   disp('Failure: closed-loop unstability due to numerical difficulties!')
   disp(['   Increase ' str 'GAMMA to improve reliability']);
   disp('  ');
   return
elseif abs(sum(diag(ak))) > 1e6,
   disp('Warning:  the controller has fast modes (modulus > 1e6)')
   disp(['   Increase ' str 'GAMMA to eliminate fast dynamics']);
   disp('  ');
end



% update K(s) if D22 is nonzero
%------------------------------
[rp,cp]=size(P); p2=r(1); m2=r(2);
d22=P(rp-p2:rp-1,cp-m2:cp-1);

if norm(d22,1) > 0,
  if norm(dk,1) > 0,
     M2k=eye(p2)+d22*dk; Mk2=eye(m2)+dk*d22;
     s=svd(M2k);
     if min(s) < sqrt(macheps),
       error('Algebraic loop due to nonzero D22!  Perturb D22 and recompute K(s)');
       Kcen=[];
     else
       tmp=Mk2\ck;
       Kcen=ltisys(ak-bk*d22*tmp,bk/M2k,tmp,Mk2\dk);
     end
  else
     Kcen=ltisys(ak-bk*d22*ck,bk,ck,dk);
  end
end

%  get the equation

m1=1.5*10^3;
m2=1.0*10^4;
k1=5.0*10^6;
k2=5.0*10^5;
b1=1.7*10^3;
b2=50*10^3;
A=[0 0 1 0 
    0 0 0 1
    -(k1+k2)/m1 k2/m1 -(b1+b2)/m1 b2/m1
    k2/m2 -k2/m2 b2/m2 -b2/m2];
B=[b1/m1 0
    0 0
    k1/m1-(b1*(b1+b2))/(m1*m1)  -1/m1
    (b1*b2) / (m1*m2)  1/m2];
C1=[1 0 0 0 
    0 0 0 0
    k2/m2  -k2/m2  b2/m2  -b2/m2
    -1 1 0 0];
D1=[-1 0;0 1;(b1*b2)/(m1*m2) 1/m2;0 0];
C2=[k2/m2 -k2/m2 b2/m2 -b2/m2
    -1 1 0 0];
D2=[(b1*b2)/(m1*m2) 1/m2; 0  0];
sysG=ltisys(A,B,[C1;C2],[D1;D2]);
%define
syswq0=ltisys('tf' , [0.01],[0.4 1]);
syswz1=ltisys('tf',200,1);
syswz2=ltisys('tf',0.1,1);
syswz3=ltisys('tf',[0.0318 0.4], [0.000316 0.0314 1]);
syswz4=ltisys('tf',100,1);
syswz5=ltisys('tf',1,1);
syswz=sdiag(syswz1,syswz2,syswz3,syswz4,syswz5,syswz5);
syswq=sdiag(syswq0,syswz5);
sys=smult(syswq,sysG,syswz);
[gopt,K]=hinflmi(sys,[2 1]);
%get K