compute_mapping_dha.m

一个matlab的将军模型

                              if ~isempty(and(mapping_pos_reset,return_invariant(cell_number)))
                                 
                               	dst_struc{mapping_counter}.dstloc  	= dstloc;
	                           	dst_struc{mapping_counter}.mapping	= and(mapping_pos_reset,return_invariant(cell_number));
   	                        	dst_struc{mapping_counter}.dstcell 	= cell_number;
      	                  		dst_struc{mapping_counter}.Tstamp   = [inf_pointer(i) sup_pointer(i)];
                                dst_struc{mapping_counter}.transition_theta=0;
                                 if ~strcmp(overall_dynamics,'clock')
                                     %Only perform bounding box if there is more than one mapping
                                     %('if then' added by JPK 1/30/02)
                                     if length(dst_struc{mapping_counter}.mapping)>1
                                        mapping_pos_bb	= bounding_SD_box(dst_struc{mapping_counter},param_file);   
                                    else
                                        mapping_pos_bb	=   dst_struc{mapping_counter}.mapping;
                                    end    
                                else
                                    mapping_pos_bb	=   dst_struc{mapping_counter}.mapping;   
                                 end
            	               	dst_struc{mapping_counter}.mapping	= mapping_pos_bb;
               	            	mapping_counter = mapping_counter + 1;
                  	         end %if%
                           end %for%
                        else
                           %Else add to terminal list if the terminal state has not already been added.
                           found_terminal=0;
                           for q=1:length(terminal)
                               if terminal{q}==destination_sfm
                                   found_terminal=1;
                               end
                           end   
                           if ~found_terminal
                                terminal{length(terminal)+1} = destination_sfm;
                           end
                           
                        end %if%
                  	end%if%
                  	transition_found =1;

% The following lines were commented out (JPK 6/2002) because there is no apparent reason why they
% should be here.  Izaias apparently wanted to subtract the overlap of the guard cell with the reachable
% set from the entire reachable set (then intersect with the next guard and then intersect withe the other
% interior cells).  I don't think this is necessary or even correct.  For one thing, the guards are not necessarily
% disjoint.  For another thing, the subtraction operation will yield multiple linearcons as a result if the result
% is nonconvex.
%
%                   	temp = minus(Pk_reach,mapping);
%           				if ~isempty(temp)
%             				Pk_reach  = temp{1};
%          				else
%             				Pk_reach = temp;
%          				end
                       
						end	%if%
		     		end	%for%        
				end	%for%
			end	%if%
      end	%if%
      
      % If guard violated, collect information about the cells in the source location
      if transition_found |(initial & ~isempty(Pk_reach))
         
         % If reachable set is not empty
         if ~isempty(Pk_reach)
            
            % If reachable set went out original cell
            if ~isempty(minus(Pk_reach,return_invariant(srccell)))
               
               %--- test all cells ---% 
               for m=1:length(GLOBAL_PIHA.Locations{srcloc}.interior_cells) 
                  
                  % If reachable set intersects some cell
                  if (srccell~=GLOBAL_PIHA.Locations{srcloc}.interior_cells(m))&...
                          ~isempty(and(Pk_reach,return_invariant(GLOBAL_PIHA.Locations{srcloc}.interior_cells(m))))
                     
                     % create an element of destination to be returned
                     dst_struc{mapping_counter}.mapping   = ...
                    		and(Pk_reach,return_invariant(GLOBAL_PIHA.Locations{srcloc}.interior_cells(m)));
		            	dst_struc{mapping_counter}.dstloc   = srcloc;
      	         	dst_struc{mapping_counter}.dstcell  = GLOBAL_PIHA.Locations{srcloc}.interior_cells(m);
           		   	dst_struc{mapping_counter}.Tstamp   = [inf_pointer(i) sup_pointer(i)];
                    dst_struc{mapping_counter}.transition_theta=0;

% Again, I don't think it's correct to perform the set subtraction here.  We want to intersect the reachable set 
% with all of the interior cells, but we can do that with the entire reachable set (JPK 6/2002)
%                                          
%                      temp = minus(Pk_reach,dst_struc{mapping_counter}.mapping);
%          				if ~isempty(temp)
% 				            Pk_reach  = temp{1};
% 				         else
% 				            Pk_reach = temp;
% 				         end

							mapping_counter = mapping_counter + 1;
                       
                 	end %If%
                    
                end%for%
                
             end%if%
             
          end%if%
          
       end%if%
       
      % Transition for the initial step
      if initial & ~isempty(Pk_reach)
         dst_struc{mapping_counter}.mapping   = and(Pk_reach,return_invariant(srccell));
         dst_struc{mapping_counter}.dstloc   = srcloc;
         dst_struc{mapping_counter}.dstcell  = srccell;
         dst_struc{mapping_counter}.Tstamp   = [inf_pointer(i) sup_pointer(i)];
         dst_struc{mapping_counter}.transition_theta=1;

% Once again, I don't think that this set subtraction is appropriate (JPK 6/2002)
%          
%          temp = minus(Pk_reach,dst_struc{mapping_counter}.mapping);
%          if ~isempty(temp)
%             Pk_reach  = temp{1};
%          else
%             Pk_reach = temp;
%          end
         dst_struc{mapping_counter}.transition_index   = [];
     		mapping_counter = mapping_counter + 1;
   	  end
      
      
      
      % check if equilibrium point is inside mapping and if the equilibrium ellipsoid 
      % will prevent the system to make further transitions
   	if check_equilibrium
      	Vk = vertices(Pk_reach);
			% Translate the vertices of the reachable set Xk at time tk from xe
			% to the origin.
			Vk_hat = transform(Vk,eye(size(A)),-xe);
			% Then check if Vk_hat is in the stability ellipsoid centered at the
			% origin.
			equilibrium = is_in_stability_ellipsoid(Vk_hat,Q,gamma);
		end
         
      % check time limit  
   	if check_time_limit
         time_limit = (inf_pointer(i) > max_time);
		end
      
      if time_limit | equilibrium
         END_SEARCH = 1;
      end
      
      % Test if reachable set is out of bound
      if ~isempty(Pk_reach)
         if ~isempty(minus(Pk_reach,AR))
         	if isempty(and(Pk_reach,AR))
            	END_SEARCH = 1;
         	end
         	out_of_bound=1;
      	end
      end
      
      %After intersecting Pk_reach with all possible guards, all possible other interior regions,
      %and the complement of the analysis region, all that remains is the part ofthe reachable set 
      %that is still in the original source cell.  So the following line was added (JPK 6/2002).
      %This should be done only if a transition was found
      %because, otherwise we don't require that the reachable set remain within one interior cell.
      if transition_found
           Pk_reach = and(Pk_reach,return_invariant(srccell));
      end
      
      if isempty(Pk_reach)|initial
         END_SEARCH = 1;
      end
      
      counter = counter + 1;
		if (rem(counter-1,60) == 0)
   		fprintf(1,'\n')
		end
   	fprintf(1,'.')
		fprintf(1,'\n')
     	null_event = equilibrium; 
              
  end
    
  destination ={};
  for j=1:length(dst_struc)
     if ~isempty(dst_struc{j}.mapping)
        if ~isempty(minus(dst_struc{j}.mapping,AR))
            out_of_bound=1;
        else
            destination{length(destination)+1}=dst_struc{j};
        end
    end
  end
     
end

%Put a bounding box around destination regions that land in the same destination location
%and cell.

%The 'bound_and_clean' call has been commented out because we want the mappings to remain separated
%so that each one will have its respective time tag.  This is necessary now because the system
%dynamics are time-varying.

%destination = bound_and_clean(destination);


if PLOT
  %close(fig)
end

return

% -----------------------------------------------------------------------------

function destination_new = bound_and_clean(destination_old)

%Put a bounding box around all destination regions that land in the same location and state.
if ~isempty(destination_old)
    unique_dest_list = [destination_old{1}.dstloc destination_old{1}.dstcell];
    %Make a list of all unique destination locations/states.
    for i = 1:length(destination_old)
        if ~ismember([destination_old{i}.dstloc destination_old{i}.dstcell],unique_dest_list,'rows')
            unique_dest_list = [ unique_dest_list ; destination_old{i}.dstloc destination_old{i}.dstcell];
        end
    end
    
    %Use list of unique destinations to create new destination list.
    for i = 1:size(unique_dest_list,1)
        destination_temp={};
        Tstamp=[Inf 0];
        verts=vertices;
        for j = 1:length(destination_old)
            if (destination_old{j}.dstloc==unique_dest_list(i,1))&(destination_old{j}.dstcell==unique_dest_list(i,2))
                    destination_temp{length(destination_temp)+1} = destination_old{j};
                    if destination_old{j}.Tstamp(1)<Tstamp(1)
                        Tstamp(1) = destination_old{j}.Tstamp(1);
                    end 
                    if destination_old{j}.Tstamp(2)>Tstamp(2)
                        Tstamp(2) = destination_old{j}.Tstamp(2);
                    end
                    verts = verts|vertices(destination_old{j}.mapping);
            end
        end
        if length(destination_temp)>1 
        	destination_new{length(destination_new)+1} = destination_temp{1}
            destination_new{length(destination_new)}.Tstamp = Tstamp;  
            mapping_temp =  linearcon(polyhedron(verts));
            destination_new{length(destination_new)}.mapping = return_cell_invariant(unique_dest_list(i,2))&mapping_temp;
        else
            destination_new{length(destination_new)+1} = destination_temp{1};
        end        
    end
end

return

% -----------------------------------------------------------------------------

function result = is_in_stability_ellipsoid(V,Q,gamma)

% Check if the given polytope P defined by a set of vertices V is contained
% in the ellipsoid x'*Q*x <= gamma. This done by solving the quadratic
% program
%
%    max    x'*Q*x 
%  x in P 
%
% and check if the maximum is <= gamma. Solve the maximization problem by
% searching over the vertices V of P, since the global maximum of a convex
% function over a polytope occurs at some vertex of the polytope.

result = 1;
for k = 1:length(V)
  if (V(k)'*Q*V(k) > gamma)
    result = 0;
    break;
  end
end

% -----------------------------------------------------------------------------

function [P,gamma] = lyapell(A,C,d)

% Given a STABLE matrix A and an invariant polytope Cx <= d which strictly
% encloses the origin, find the largest Lyapunov ellipsoid x'*P*x = gamma
% contained in Cx <= d.

% If A is STABLE then, we must be able to solve the Lyapunov equation
P = dlyap(A',eye(size(A)));

% To fit the largest ellipsoid x'*P*x, inside the polytope CI*x <= dI which
% stictly encloses the origin, we do the following.
% The largest ellipsoid that could fit inside the kth face of the polytope
% can be found by solving the optimization problem
% 
% min        fk(x) = x'*P*x 
% subject to ck'*x = dk
%
% By writing the Lagrangian L(x,lambda) = x'*P*x + lambda(ck'*x-dk), and
% differentiate with respect to x and lambda, we have that the optimal
% solution occurs at
%
% x = -(lambda/2)*P^{-1}*ck        .............. (1)
%
% From the constraint ck'*x = dk, we have that
%
% dk = -(lambda/2)*ck'*P^{-1}*ck   .............. (2)
%
% Solving (2) for lambda and substituting (1) into the objective
% function, we have that the optimal value for the objective function is
% 
% fk(x) = (dk^2)/(ck'*P^{-1}*ck)
%
% Thus, the largest ellipsoid that contained in the polytope is given by
% x'*P*x = gamma where
%
% gamma = min { (dk^2)/(ck'*P^{-1}*ck) }
%          k

P_inv = inv(P);
gamma = Inf;
for k = 1:length(d)
  gamma_k = (d(k)*d(k))/(C(k,:)*P_inv*C(k,:)');
  if gamma_k < gamma
    gamma = gamma_k;
  end
end

%-----------------------------------------------------------------------------

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

Q=eye(size(A));
gamma = zeros(size(A,1),1);
xe=zeros(size(A,1),1);
% If A is invertible, precompute inverse of A
if rank(eye(size(A))-A) == size(A,1)
  i_minus_A_inv = inv(eye(size(A))-A);
else
  i_minus_A_inv = [];
end

check_equilibrium =0;
check_time_limit = 0;

if all(abs(eig(A))<1)
  % A is stable (in discrete time).
  [CE,dE,CI,dI] = linearcon_data(INV);
  if ~(isempty(CE) & isempty(dE))
    INV
    error('Invariant polytope must be full dimensional.')
  end
  % The equilibrium point (for discrete time) is xe = inv(I-A)*b.
  xe = i_minus_A_inv*b;
  % Translate the invariant from the equilibrium point to the origin
  dIhat = dI-CI*xe;
  % check equilibrium point enclosure
  if all(dIhat > 0)
    % Find the largest stability ellipsoid x'*Q*x = gamma
    % contained in the translated INV.
    [Q,gamma] = lyapell(A,CI,dIhat);
    check_equilibrium = 1;
  end
else 
  if any(real(eig(A)) == 0) & all(b == 0)
    % A is marginally stable and no input
    check_time_limit = 1;
  end
end