compute_mapping_dha_vr.m

一个matlab的将军模型

         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 ~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(i)));
		            	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;
                     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;
         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
      
      if isempty(Pk_reach)|initial
         END_SEARCH = 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;
         	Pk_reach = and(Pk_reach,AR);
      	end
      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.
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 = lyap(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(A) == size(A,1)
  Ainv = inv(A);
else
  Ainv = [];
end

check_equilibrium =0;
check_time_limit = 0;

if all(real(eig(A)) < 0)
  % A is stable
  [CE,dE,CI,dI] = linearcon_data(INV);
  if ~(isempty(CE) & isempty(dE))
    INV
    error('Invariant polytope must be full dimensional.')
  end
  % The equilibrium point is xe = -Ainv*b.
  xe = -Ainv*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