mldsim.m

由matlab开发的hybrid系统的描述语言

% function [xt1, dt, zt, yt, fl, eps] = mldsim( m, xt, ut, Options )
% Simulates an MLD System. Given the MLD system and the current state and 
% input, computes the next state and the current output.
%
% INPUT:        
% m      : MLD system
% xt     : current state
% ut     : current input
% Options: 1. Specify with Options.solver the miqp solver desired, e.g. like:
%             Options.solver      = 'miqp';
%             SEE PANMIP
%          2. Specify options for the choosen solver e.g. like:
%             Options.miqp.solver = 'linprog';
%             SEE PANMIP
%          3. Specify options for the simulation
%             Options.large        : specifies the magnitude of the box bounds 
%                                    for the solver 
%          4. Specify options to debug models (see implementation for details)
%             Options.relaxIneq = 1: relax inequalities to maintain feasibility
%             Options.relaxU = 1   : relax inputs
%
% OUTPUT:
% xt1    : next time step 
% dt     : delta vector
% zt     : zeta vector
% yt     : output vector
% fl     : flags from the MIP solver
% eps    : epsilon needed in relaxation (see implementation for details)
%
% REMARK:
% Requires panmip as solver interface and some solver supported by panmip
%
% (C) 1998--2002 D. Mignone
% Automatic Control Laboratory, ETH Zentrum, CH-8092 Zurich, Switzerland
% [e-mail]
%
% see license.txt for the terms and conditions.

% History:      date       ver. subject                                       
%               1998.08.27 1.0	Initial Version   
%               1999.07.14   	Switched to panmiqp.m
%               2000.10.18   	Interface and solvers updated
%               2000.10.30   	Check for empty matrices in the MLD model included
%               2000.10.31   	Empty contraint matrices are allowed
%               2000.11.01   	Options includes large bounds for unbounded va-
%                            	riables as a field Options.large (=positive scalar)
%               2001.02.09   	Makes columns out of the input arguments
%               2002.02.21   	Added epsilon to relax inequalities and maintain 
%                            	feasibility: Options.relaxIneq = 1
%               2002.03.04   	Handles initial solution as part of the Options 
%               2002.03.18  	Added epsilon to relax inputs: Options.relaxU = 1
%               2002.04.15 1.1	correct 'Options.iniSol'
%               2002.08.26      FT removed fault handlers
function [xt1, dt, zt, yt, fl, eps] = mldsim( m, xt, ut, Options )

% argument check
% --------------
error(nargchk(2,4,nargin));

if nargin < 4
   Options = [];
end

% extract matrices
% ----------------
[A,B1,B2,B3,C,D1,D2,D3,E1,E2,E3,E4,E5,Ext] = mld2mat(m,[]);

nx = Ext.nx;
nu = Ext.nu;
nz = Ext.nz;
nd = Ext.nd;
ny = Ext.ny;
ne = Ext.ne;

% more argument checks
% --------------------
  
if isempty(xt) & (nx ~=0)
   warning('empty state, assuming 0')
   xt = zeros(nx,1);
elseif length(xt) ~= nx
   error('wrong dimension of state xt')
end
xt = xt(:);

if isempty(ut) & (nu ~=0)
   warning('empty input, assuming 0')
   ut = zeros(nu,1);
elseif length(ut) ~= nu
   error('wrong dimension of input ut')
end
ut = ut(:);

% the following checks are required because the sum []+a where a is a scalar
% gives [] as a result, this sum might occur in the state update below


if ne >= 1
   
   % find d(t) and z(t)
   % ------------------
   ivars= 1:nd;
   Q    = zeros(nd+nz,nd+nz);
   b    = zeros(nd+nz,1);
   
   if ~isfield(Options,'large')
      vlb  = [zeros(1,nd) -inf*ones(1,nz)]';
      vub  = [ ones(1,nd)  inf*ones(1,nz)]';
   else
      vlb  = [zeros(1,nd) -Options.large*ones(1,nz)]';   
      vub  = [ ones(1,nd)  Options.large*ones(1,nz)]';  
   end   

   ctype          = char(ones(size(E1,1),1)*'L');
   vartype        = char(ones(size(Q,1),1)*'C');
   vartype(ivars) = 'B';
   
   if ~isfield(Options, 'relaxIneq')
   	  Options.relaxIneq = 0;
   end;
   if ~isfield(Options, 'relaxU')
   	  Options.relaxU = 0;
   end;

   if ~isfield(Options,'solver')
      Options.solver = 'miqp';    % default solver
   end
   
   switch Options.solver
   case 'miqp'
      if ~isfield(Options,'miqp')
         Options.miqp = [];
      end	 
      if ~isfield(Options.miqp,'solver')
         Options.miqp.solver = 'linprog';
      end
      if ~isfield(Options.miqp,'optimset')
         Options.miqp.optimset = [];
      end
      if ~isfield(Options.miqp,'bignumber')
         Options.miqp.bignumber= inf;	
      end
   end

   % build standard MLD-system
   F = [E2 E3];
   g = [E1*ut+E4*xt+E5];

   % initial solution
   if ~isfield(Options, 'iniSol')
   	   	iniSol = zeros(nd+nz, 1);
   else 
   		iniSol = Options.iniSol;
   end;
   
   if Options.relaxIneq & (0==1)
   	
		% augment MLD-system by one epsilon to relax the inequalities:
		% min  eps
		% s.t. F <= g + eps
		%      eps >= 0
		
   		Q = [Q zeros(nd+nz,1); zeros(1,nd+nz) 0];
		b = [b; 1];

		F = [F -ones(ne,1); zeros(1,nd+nz) -1];
		g = [g; 0];
		
		ctype = [ctype; 'L'];
		vartype = [vartype; 'C'];
		vlb = [vlb; 0];
		vub = [vub; 1];
		
		iniSol = [iniSol; 0];
   end;
   
   if Options.relaxIneq & (0==0)
   	
		% augment MLD-system by ne (=number of ineq.) epsilons to relax every 
		% inequality seperately:
		% min  Sum eps_i
		% s.t. F <= g + eps
		%      eps_i >= 0
		
   		Q = [Q zeros(nd+nz,ne); zeros(ne,nd+nz) zeros(ne,ne)];
		b = [b; ones(ne,1)];

		F = [F -eye(ne); zeros(ne,nd+nz) -eye(ne)];
		g = [g; zeros(ne,1)];
		
		ctype = [ctype; char(ones(ne,1)*'L')];
		vartype = [vartype; char(ones(ne,1)*'C')];
		vlb = [vlb; zeros(ne,1)];
		vub = [vub; ones(ne,1)];
		
		iniSol = [iniSol; zeros(ne,1)];
   end;
   
   if Options.relaxU
   
   		% augment MLD-system by nu (=number of inputs) epsilons to relax every
		% input (continuous and binary) seperately:
		% min  Sum epsA_i
		% s.t. E2*d +E3*z <= E1*(u-eps) +E4*x +E5
		%      -epsA <= eps <= epsA
		%      -1 <= eps <= 1
		%      epsA >= 0
		% 
		% epsA = abs(eps)
		
		Q = [Q zeros(nd+nz, 2*nu); zeros(2*nu, nd+nz+2*nu)]; 
   		b = [b; zeros(nu,1); ones(nu,1)];
		
		F = [E2 E3 E1 zeros(ne,nu); zeros(nu,nd+nz) -eye(nu) -eye(nu); zeros(nu,nd+nz) eye(nu) -eye(nu)];
		g = [g; zeros(2*nu,1)];
		
		ctype = [ctype; char(ones(2*nu,1)*'L')];
		vartype = [vartype; char(ones(2*nu,1)*'C')];
		vlb = [vlb; -ones(nu,1); zeros(nu,1)];
		vub = [vub; ones(nu,1); ones(nu,1)];
		
		iniSol = [iniSol; zeros(2*nu,1)];
   end;
        			   
   % let's solve our problem
   [dzmin, fm, fl, Eflag]= panmip(Q, b, F, g, ctype, [], vartype, vlb, vub, iniSol, Options);
      
   if fl == 1
      dt = dzmin(1:nd);
      zt = dzmin(nd+1:nd+nz);

      % compute output and next state
      xt1 = A*xt(:) + B1*ut(:) + B2*dt(:) + B3*zt(:);
      yt  = C*xt(:) + D1*ut(:) + D2*dt(:) + D3*zt(:);
   else
      warning('Constraints lead to infeasible or unbounded solutions')
      xt1 = zeros(nx,1);
      yt  = zeros(ny,1);
      dt  = dzmin(1:nd);
      zt  = dzmin(nd+1:nd+nz);
   end
   
   if Options.relaxIneq,  eps = dzmin(nd+nz+1:length(dzmin)); 
   elseif Options.relaxU, eps = dzmin(nd+nz+1:nd+nz+nu);
   else                   eps = NaN*ones(ne,1); 
   						  %eps = NaN*ones(nu,1);
   end;
			   			   
else
   
   % Extreme case of an unconstrained MLD system
   % -------------------------------------------
   % in this case no delta and z are allowed to be present and the state update
   % is done by taking the discrete time update of a linear model
   
   if (nd ~= 0)|(nz ~= 0)
      error('no inequalities are specified that define delta and z')
   end
   
   xt1 = A*xt(:)+B1*ut(:);
   yt  = C*xt(:)+D1*ut(:);  
   
   fl  = 1; 
   
   dt  = [];
   zt  = [];
    		   
end