compute_mapping_sd.m

一个matlab的将军模型

function [destination,null_event,time_limit,out_of_bound,terminal] = ...
    compute_mapping_SD(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_SD(X0,sttype,Tstamp,srcloc,srccell)"
%
% Description:
%
%   The routine SD_compute_mapping performs the reachability from a initial set X0, finding  the 
%   set of states that can be reached, for a  given clock  structure  (period, initial phase 
%   displacement and jitter). It also finds a conservative approximation of the elapsed time to 
%   reach them. 
%
%	 After this, it applies the reset function 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 uncertainty to reach the region
% 
%   * "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);
if PLOT 
    hold on, plot(INV);
end
% 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);
 
% Get approximation parameters for the given FSM state.
max_time = GLOBAL_APPROX_PARAM.max_time;
Pcon= return_parameter_cons(srcloc);

% Get clock structure information
clock_info = GLOBAL_PIHA.CLOCKBlocks;

null_event = 0;
time_limit = 0;
terminal = {};
out_of_bound = 0;
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);
inf_pointer_offset             = zeros(length(clock_info),1);
sup_pointer_offset            = zeros(length(clock_info),1);
inf_pointer_offset_nojitter  = zeros(length(clock_info),1);
sup_pointer_offset_nojitter = zeros(length(clock_info),1);
t_inf                                = zeros(length(clock_info),1);
t_sup                                = zeros(length(clock_info),1);
equilibrium=0;
check_equilibrium=0;
check_time_limit=0;

% 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

switch overall_dynamics,
   
case 'linear',
   
    	[A,b] = overall_system_matrix(GLOBAL_PIHA.SCSBlocks, ...
       	GLOBAL_PIHA.Locations{srcloc}.q);
    
    	% If A is invertible, precompute inverse of A
	 	if rank(A) == size(A,1)
	  		Ainv = inv(A);
	 	else
  	  		Ainv = [];
	 	end

		% If A is stable and the equilibrium point is enclosed in INV, then turn
		% on the equilbrium check

		[check_equilibrium,check_time_limit,xe,Q,gamma]=fcheck_equilibrium(A,b,INV);

		if check_equilibrium
		  fprintf(1,'\n - equilibrium point found')
		end
		if check_time_limit
		  fprintf(1,'\n - time limit will be enforced')
		end

   
 case 'clock',
    v = overall_system_clock(GLOBAL_PIHA.SCSBlocks, ...
        GLOBAL_PIHA.Locations{srcloc}.q);
    
    
 case 'nonlinear',
    ode_param = GLOBAL_PIHA.Locations{srcloc}.q;
    check_time_limit = 1;
    
    
 otherwise,
    error(['Unknown dynamics type ''' overall_dynamics '''.'])
    
 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;

% 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;
   transition_found=0;
   transition_found_last_time = 0;
	while ~END_SEARCH
%        if ~transition_found
%            Pk_reach = X0;   
%        end
        
      
      
      % ----- 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);
             if transition_found
                inf_pointer_offset_nojitter(i) =  inf_pointer_offset_nojitter(i) + clock_info{i}.period(1);
                sup_pointer_offset_nojitter(i) = sup_pointer_offset_nojitter(i) + clock_info{i}.period(2);
                inf_pointer_offset(i)  =  inf_pointer_offset_nojitter(i) + clock_info{i}.jitter(1);
                sup_pointer_offset(i) = sup_pointer_offset_nojitter(i) + clock_info{i}.jitter(2);
            else
                inf_pointer_offset(i)=0;
                sup_pointer_offset(i) = 0;
            end

          
	   	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);
            if transition_found
                if ~transition_found_last_time
                    inf_pointer_offset_nojitter(i) =  inf_pointer_nojitter(i) - clock_info{i}.period(1);
                    sup_pointer_offset_nojitter(i) = sup_pointer_nojitter(i) - clock_info{i}.period(2);
                    transition_found_last_time =1;
                else
                    inf_pointer_offset_nojitter(i) =  inf_pointer_offset_nojitter(i) + clock_info{i}.period(1);
                    sup_pointer_offset_nojitter(i) = sup_pointer_offset_nojitter(i) + clock_info{i}.period(2);
                end
                inf_pointer_offset(i)  =  inf_pointer_offset_nojitter(i) + clock_info{i}.jitter(1);
                sup_pointer_offset(i) = sup_pointer_offset_nojitter(i) + clock_info{i}.jitter(2);
            else
                inf_pointer_offset(i)=0;
                sup_pointer_offset(i) = 0;
                transition_found_last_time =0;
            end
               
          
      end
      
      Tinterval             = sup_pointer(i) - inf_pointer(i);
      Tinterval_offset   = sup_pointer_offset(i) - inf_pointer_offset(i);

      if (Tinterval < 0)
      	 error(['Inconsistent value of clock = [''' Tinterval '''].'])
      end
      
      if ~isempty(Pk_reach)
         switch overall_dynamics
         
	      	case 'linear'
         
		            eAT 	             = expm(A*inf_pointer(i));
					displacement		 = step_response(A,Ainv,b,inf_pointer(i));
					Xfinal             = transform(X0,eAT,displacement);
      
	   	   	        % Compute the reachable set for the given time interval 
	   			    V0 	= vertices(Xfinal);
		   		    Pk 	= seg_approx_lin(A,Ainv,b,Xfinal,V0,Tinterval);
		   	        Pk_reach_X0=Pk;

                    if transition_found
                        eAT 	               = expm(A*inf_pointer_offset(i));
					    displacement        = step_response(A,Ainv,b,inf_pointer_offset(i));
					    Xfinal                  = transform(Pk_reach,eAT,displacement);
      	   	   	        % Compute the reachable set for the given time interval 
	   			        V0 	= vertices(Xfinal);
		   		        Pk 	= seg_approx_lin(A,Ainv,b,Xfinal,V0,Tinterval_offset);
		   	            Pk_reach=Pk&Pk_reach_X0;
                    else
                        Pk_reach = Pk_reach_X0;
                    end
                    
               case 'clock'
	   	      
	      	        if transition_found
                        t_inf(i) = inf_pointer(i) - inf_pointer_offset(i);
                        t_sup(i) = sup_pointer(i) - sup_pointer_offset(i);                        
                    else
                        Pk_reach = X0;
                        t_inf(i) = inf_pointer(i);
                        t_sup(i) = sup_pointer(i);
                    end
                    Pinit  	        =    transform(Pk_reach,eye(length(v)),v*t_inf(i));
           	        Pfinal 	       =    transform(Pk_reach,eye(length(v)),v*t_sup(i));
		            Xinit 	       =	vertices(Pinit);
		            Xfinal 	      =	   vertices(Pfinal);
	                Pk			   =    polyhedron(Xinit | Xfinal);
	                Pk_reach	=    linearcon(Pk);
         
            case 'nonlinear'
               
    	                sample_points_X0 = vertices(X0);
      	            Pk = linearcon(X0);          
         	        [Pk,sample_points] = seg_approx_SD_ode('overall_system_ode',ode_param,...
            	            Pk,sample_points_X0,inf_pointer(i),sup_pointer(i),Pcon);
					Pk_reach_X0 = Pk;     
                    if transition_found
                   	    sample_points_X0 = vertices(Pk_reach);
      	                Pk = linearcon(Pk_reach);          
         	            [Pk,sample_points] = seg_approx_SD_ode('overall_system_ode',ode_param,...
            	                Pk,sample_points_X0,inf_pointer_offset(i),sup_pointer_offset(i),Pcon);
					    Pk_reach = Pk&Pk_reach_X0;                 
                    else
                        Pk_reach = Pk_reach_X0;
                    end
               
          end 	%switch%
          if PLOT        
            drawnow;plot(Pk_reach);
          end
       
      end	%if%

      transition_found=0;

      if ~isempty(Pk_reach)
         
         % -- test if Pk_reach left cell -- %
         
         if isempty(GLOBAL_PIHA.Locations{srcloc}.orig_interior_cells)
             cell_index=[];
         else
             [var01,var02,cell_index]=intersect(srccell,GLOBAL_PIHA.Locations{srcloc}.orig_interior_cells);
             if isempty(cell_index)
                interior_cells=GLOBAL_PIHA.Locations{srcloc}.orig_interior_cells;
                same_cells=0;
                for int_counter =1: length(interior_cells)
                     if isequivalent(interior_cells(int_counter),srccell)
                        same_cells=1;
                        srccell = interior_cells(int_counter);
                        break;
                    end%if
                end%for
                if ~same_cells
                    cell_index=[];
                end
            end%if
         end%if
         
         if ~isempty(minus(Pk_reach,return_invariant(srccell))) | isempty(cell_index)
               
         	% -- 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))))
                     
                         % -- record guard cell to where the Pk_reach go