compute_mapping_dha.m

一个matlab的将军模型

function [destination,null_event,time_limit,out_of_bound,terminal] = ...
    compute_mapping_DHA(X0,sttype,Tstamp,srcloc,srccell)

% Compute the `mapping set` from the given polytope in the specified location.
%
% Syntax:
%   "[destination,null_event,time_limit,out_of_bound,terminal] = ...
%			compute_mapping_DHA(X0,sttype,Tstamp,srcloc,srccell,param_file)"
%
% Description:
%
%   The routine compute_mapping_DHA performs the reachability from a initial set X0, finding  the 
%   set of states that can be reached, using sampled-data analysis (i.e. discrete-time, difference
%   equations with fixed sample periods). It also finds a conservative approximation of the elapsed
%   time to reach them. 
%
%	 After this, it applies the reset functivon defined for this transition. As a last
%   operation, it wraps up the set of states with a conservative bounding box, avoiding explosion 
%   in the number of vertices and faces. 
%
%   The inputs to this function are: 
%
%   * "X0": a "linearcon" object representing the initial continuous set
%     for which the mapping is to be computed.
%  
%   * "sttype": type of state for "X0" which is either an `initial` or
%     `face` state (apply the initial displacement or not).
%
%	 * "Tstamp": vector with the time range in which the initial set occurs.
%       Note that if the sample rate is deterministic, then this is a vector
%       with two identical elements.
% 
%   * "srcloc": source location.
% 
%	 * "srccell": cell region of source
%
%
%   The outputs of this function are
%
%   % `destination is a cell array with the following fields:
%
%   * dstloc 	: location whose guard was violated during the reachability process
%	 * dstcell 	: Interior region of the dstloc
%   * region 	: linearcon object representing the set of states that violates some guard for a given
%     location.
%   * Tstamp 	: time elapsed interval to reach the region starting at the set X0.
%
%   * "null_event": a boolean flag indicating that the flow pipe computation
%     was terminated because it can be concluded that the subsequent flow
%     pipe segments will remain inside "INV" forever.
%
%   * "time_limit": a boolean flag indicating that the flow pipe computation
%     was terminated because the time limit "max_time" was exceeded.
%
% Implementation:
%   Call the mapping computation routine corresponding to the type of the
%   composite continuous dynamics of all SCSBs for the given location
%   ("fs_lin_map" for `linear` (affine) dynamics, "fs_nonlin_map" for
%   `nonlinear` dynamics, and "clk_map" for `clock` dynamics). %   
%
% See Also:
%   fs_nonlin_map,fs_lin_map,clk_map

global GLOBAL_PIHA
global GLOBAL_APPROX_PARAM
PLOT=0;
% Get the invariant polytope for the given cell location.
INV = return_invariant(srccell);

% Compute the type of the composite dynamics of all SCSBs for the FSM
% state in the source location.

overall_dynamics = check_overall_dynamics(GLOBAL_PIHA.SCSBlocks, ...
    GLOBAL_PIHA.Locations{srcloc}.q);
 
max_time = GLOBAL_APPROX_PARAM.max_time;

% Get clock structure information
clock_info = GLOBAL_PIHA.CLOCKBlocks;

null_event = 0;
time_limit = 0;
terminal = {};
out_of_bound = 0;

% inf_pointer 	             = ones(length(clock_info),1)*Tstamp(1);
% sup_pointer 	            = ones(length(clock_info),1)*Tstamp(2);
% inf_pointer_nojitter 	   = ones(length(clock_info),1)*Tstamp(1);
% sup_pointer_nojitter 	  = ones(length(clock_info),1)*Tstamp(2);

inf_pointer 	             = zeros(length(clock_info),1);
sup_pointer 	            = zeros(length(clock_info),1);
inf_pointer_nojitter 	   = zeros(length(clock_info),1);
sup_pointer_nojitter 	  = zeros(length(clock_info),1);

equilibrium=0;
check_equilibrium=0;
check_time_limit=1;

% Get information about analysis region
for k=1:GLOBAL_PIHA.NAR
   c_AR(k,:) = GLOBAL_PIHA.Hyperplanes{k}.c;
   d_AR(k,:) = GLOBAL_PIHA.Hyperplanes{k}.d;
end
AR = linearcon([],[],c_AR,d_AR);
% if PLOT
%     plot(AR,[1 0 0]);
% end

fprintf(1,'Computing flow pipe segments:')
counter = 0;

global_var;
%PLOT = 0;

switch sttype
   
	case 'init', % initial state
      initial = 1;
      
	case 'face', % face state (a patch)
      initial=0;
          
   otherwise,
      error(['Unknown state type ''' sttype '''.'])
      return
end



%equilibrium = 0; time_limit = 0;Xsim = vertices;
%first = 1; t_total = 0; stop = 0;init_T=T;


if strcmp(overall_dynamics,'linear')
    %Get info for linear case and find a lyapunov ellipsoid if one exists within the interior
    %cells of this location

        %Currently, DHA analysis only supports one clock, one SCSB block, and one Stateflow chart.
                swfunc = GLOBAL_PIHA.SCSBlocks{1}.swfunc;
                nx = GLOBAL_PIHA.SCSBlocks{1}.nx;
                nz = GLOBAL_PIHA.SCSBlocks{1}.nz;
                nup = GLOBAL_PIHA.SCSBlocks{1}.nup;
                nu = GLOBAL_PIHA.SCSBlocks{1}.nu;
                u = GLOBAL_PIHA.Locations{srcloc}.q;
                if inf_pointer(1)~=sup_pointer(1)|length(inf_pointer)>1
                    error('Currently, DHA analysis only supports one clock with deterministic behavior.')
                end
                
                %Time varying system case:
                %[sys,type] = feval(swfunc,zeros(nx+nz+nup,1),u,inf_pointer(1));
                
                [sys,type] = feval(swfunc,zeros(nx+nz+nup,1),u);
                A = sys.A;
                b = sys.b;
                r = sys.r;
                C = sys.C;
                Ap = sys.Ap;
                bp = sys.bp;
                rp = sys.rp;
                Cp = sys.Cp;
                Dp = sys.Dp;
                rpp = sys.rpp;
    
    	        % If A is invertible, precompute inverse of A
	 	        if rank(A) == size(A,1)
	  		        Ainv = inv(A);
	 	        else
  	  		        Ainv = [];
	 	        end
                
                %The value if the plant states at the next sample instant as well as the value of the 
                %controller states at the next sample instant at calculated in one step.
                %The result of this mapping is then used to test the guard conditions, and so
                %the guard conditions are testing x(k+1) and z(k+1).  If a guard is satisfied,
                %the mode is switched (and a reset is applied to the z(k+1) value of the controller state if 
                %need be), and the control law for the new mode is used to then calculate 
                %x(k+2) and z(k+2), and so on.
                %
                T=clock_info{1}.period(1);
                %Also note that, currently, only completely deterministic clocks are allowed.
                %If the transformation is given by Pk_reach=A*Pk_reach+B then the following
                %two lines compute A and B respectively (based on the linear dynamics specified
                %above).
                I=eye(size(A,1));
                
                if ~isempty(Ainv)
                    t = [ expm(A*T)+(expm(A*T)-I)*Ainv*b*Dp*C (expm(A*T)-I)*Ainv*b*Cp ;
                                   bp*C                            Ap ];
                    v = [ (expm(A*T)-I)*Ainv*(b*rpp+r) ;
                                   rp ];
                else
                    %If the inverse of A does not exist, then, in the following equation
                    %the intergral must be calculated numerically by way of the 'step_response' procedure:
                    %
                    %   x(k+1) = e^(AT)*k(k) + integral(e^(A*(T-tau))dtau * (B*u(k)+r)
                    %
                    for col_idx=1:size(A,1)
                        temp = zeros(length(b),1);
                        temp(col_idx)=1;
                        integral(:,col_idx) = step_response(A,[],temp,T);                   
                    end
                    
                    t = [ expm(A*T)+integral*b*Dp*C integral*b*Cp ;
                                   bp*C                    Ap ];
                     v = [ integral*(b*rpp+r) ;
                                   rp ];
                    
                end

               
                %Find a lyapunov ellipsoid if one exists within any of the cells that make
                %up this location's interior
                for i=GLOBAL_PIHA.Locations{srcloc}.interior_cells
                    INV_temp = return_cell_invariant(i);
                    [check_equilibrium,check_time_limit,xe,Q,gamma]= fcheck_equilibrium(t,v,INV_temp);
                    if check_equilibrium==1
                        break;
                    end
                end
                     
                

 end
            

% Compute the transformation of the initial segment

time_pointer = 0;
mapping_counter =1;
dst_struc = {};

% cycle through the clock structure items and  identifying the possible transitions
for i=1:length(clock_info)
   Pk_reach = X0;
   END_SEARCH = 0;       
	while ~END_SEARCH
      transition_found=0;
      
         
      if ~isempty(Pk_reach)
         switch overall_dynamics
         
	      	case 'linear'
                
                %If this is the first step, do not perform the transformation.
                %Just test the guard to see if it is satisfied.  This implies
                %that the system does not need to remain in the first state for one
                %sample period, it can switch modes at the first instant (i.e. t=0).
                if initial~=1                  
                    Pk_reach = transform(Pk_reach,t,v);
                    [cetest,detest,citest,ditest] = linearcon_data(Pk_reach);
                    if length(detest)>0
                        Pk_reach = grow_polytope(cetest,detest,citest,ditest,[srcloc 0 0],0);    
                    end
                    
                end
                %plot(clean_up(project(Pk_reach,[1 2])))
            
		    case 'clock'
	   	      
               v = overall_system_clock(GLOBAL_PIHA.SCSBlocks, ...
                       GLOBAL_PIHA.Locations{srcloc}.q); 
                
	      	   Pinit  	= transform(Pk_reach,eye(length(v)),v*inf_pointer(i));
	         	Pfinal 	= transform(Pk_reach,eye(length(v)),v*sup_pointer(i));
		         Xinit 	=	vertices(Pinit);
		         Xfinal 	=	vertices(Pfinal);
	   	      Pk			= polyhedron(Xinit | Xfinal);
	      	   Pk_reach	= linearcon(Pk);
         
            case 'nonlinear'
             
             if initial~=1   
                ode_param = GLOBAL_PIHA.Locations{srcloc}.q;
                 check_time_limit = 1;
          
   	             sample_points_X0 = vertices(Pk_reach);
      	         Pk = linearcon(Pk_reach);
                 %Only deterministic sample periods are currently supported.
                T=clock_info{i}.period(1);
         	    [Pk,sample_points] = seg_approx_DHA_ode('overall_system_ode_for_DHA',ode_param,...
            	         Pk,sample_points_X0,inf_pointer(i),[],T);
					    Pk_reach = Pk;
             end           
              
             otherwise,
                error(['Unknown dynamics type ''' overall_dynamics '''.'])       
                
          end 	%switch%
          if 0        
            drawnow;plot(clean_up(project(Pk_reach,[1 2])),[0 0 0]);  
          end
       
      end	%if%
      
      % ----- Advance one step of clock  ---------% 
      switch initial
    
   		case 1
   	   	 % Compute the reachable set for the first sampling
      	     inf_pointer_nojitter(i) = clock_info{i}.phase(1);
         	 sup_pointer_nojitter(i) = clock_info{i}.phase(2);
       	     inf_pointer(i) = inf_pointer_nojitter(i)  + clock_info{i}.jitter(1);
         	 sup_pointer(i) = sup_pointer_nojitter(i) +clock_info{i}.jitter(2);

          
	   	case 0
   	      % Compute the reachable set for the other sampling
           inf_pointer_nojitter(i) =inf_pointer_nojitter(i) + clock_info{i}.period(1) ;
           sup_pointer_nojitter(i) =sup_pointer_nojitter(i) + clock_info{i}.period(2) ;
      	 	inf_pointer(i) = inf_pointer_nojitter(i) +clock_info{i}.jitter(1);
       		sup_pointer(i)	= sup_pointer_nojitter(i) + clock_info{i}.jitter(2);
   	  end
      
      Tinterval          = sup_pointer(i) - inf_pointer(i);
      
      
      if (Tinterval < 0)
      	 error(['Inconsistent value of clock = [''' Tinterval '''].'])
      end
      
      
      if ~isempty(Pk_reach)
         
         % -- test if Pk_reach left cell -- %
      	if ~isempty(minus(Pk_reach,return_invariant(srccell)))
               
         	% -- Cycle through possible transitions -- %             
            for m=1:length(GLOBAL_PIHA.Locations{srcloc}.transitions)
                
          		% -- Cycle through each guard cell for each possible transition -- %             
                for k=1:length(GLOBAL_PIHA.Locations{srcloc}.transitions{m}.guard)
                   
                  % -- Test if guard cell was reached by Pk_reach -- %             
						if ~isempty(and(Pk_reach,return_invariant(GLOBAL_PIHA.Locations{srcloc}.transitions{m}.guard(k))))
                     
                     % -- compute intersection between Pk_reach and the guard cell -- %             
                     mapping  = and(Pk_reach,return_invariant(GLOBAL_PIHA.Locations...
                        {srcloc}.transitions{m}.guard(k)));
                     
                     % -- Test if transitions refers to clock used in this loop -- %             
                     if (GLOBAL_PIHA.Locations{srcloc}.transitions{m}.clock == i)
                        
                        % -- Compute location number of the destination (empty if terminal) -- % 
                            destination_sfm				= GLOBAL_PIHA.Locations{srcloc}.transitions{m}.destination;
						    [dstloc,destin_location]	= find_location(srcloc,destination_sfm,m);
                        
                        if isempty(dstloc)
                           a=1;
                        end
                      
                        % -- Test if location is terminal -- %             
                        if ~isempty(dstloc)
                           
	                     % -- Apply reset to the reachable set-- %             
                           mapping_pos_reset 	= apply_reset(mapping,GLOBAL_PIHA.Locations{srcloc}.transitions{m}.destination...
                              ,GLOBAL_PIHA.Locations{srcloc}.transitions{m});
                           if PLOT
                               plot(mapping_pos_reset,[1 0 0]);
                           end
                        
		                     % -- cycle through all the interior cells for the location of the destination -- %             
                        	for d=1:length(GLOBAL_PIHA.Locations{dstloc}.interior_cells)
                              
                              cell_number = GLOBAL_PIHA.Locations{dstloc}.interior_cells(d);
                              
				                  % -- Test if mapping intersect given cell of the destination -- %