plotpipe.m

一个matlab的将军模型

function plotpipe(nextloc,type)

global_var;


A_plus =[ -3.3216            -25.736            0.0;...
          25.736              -3.3216           0.0;...
           3.14631            -5.10887    0.0];

A_minus =[-3.3216           -25.736            0.0;...
          25.736             -3.3216           0.0;...
           3.14631            -5.10887    0.0];

B = [263.824;-349.729;29378];
T = 0.002;
N = 4;
Ninterval = 10;
hard_calc= 1;
hold on;

if type==1
   actloc = nextloc;
   fprintf('Drawing  polytopes, please wait... ');%
   for i=1:length(actloc)
      var_location 	= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(1);
  	   if ~(strcmp(var_location,'terminal'))& ~(strcmp(var_location,'out_of_bound'));
         var_face			= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(2);
         var_state		= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(3);
         hold on;
         plot(GLOBAL_AUTOMATON{var_location}.face{var_face}.state{var_state}.polytope,[.8 .8 .9]);%[sqrt(i)/i (length(actloc) - i)/length(actloc) log(i+1)/(i+1)]);
         i;pause;
  	   end
   end
	printf('done! \n');
 
else
      for i=1:length(nextloc)
   MAP={};
  var_location 	= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(1);
  if ~(strcmp(var_location,'terminal'))
      if ismember(var_location,GLOBAL_XSYS2AUTO_MAP{GLOBAL_PIHA.InitialLocations}(1))
         X0  = GLOBAL_AUTOMATON{i}.initstate{length(GLOBAL_AUTOMATON{i}.initstate)}.polytope;
         MAP = GLOBAL_AUTOMATON{i}.initstate{length(GLOBAL_AUTOMATON{i}.initstate)}.mapping;
      else   
       	var_face			= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(2);
      	var_state		= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(3);
         X0   = GLOBAL_AUTOMATON{var_location}.face{var_face}.state{var_state}.polytope;
         MAP  = GLOBAL_AUTOMATON{var_location}.face{var_face}.state{var_state}.mapping;
      end
      if (GLOBAL_PIHA.Locations{var_location}.q(1) == 1)
         b  = B;
         AM = A_plus;
      elseif (GLOBAL_PIHA.Locations{var_location}.q(1) == 2)
         b  = [B(1);B(2);-B(3)];
         AM = A_minus;
      elseif true
         AM = zeros(3,3);
         b  = zeros(3);
      end
  		if rank(AM) == size(AM,1)
		  Ainv = inv(AM);
		else
		  Ainv = [];
		end
      inv = location_invariant(var_location);
      [CE,dE,CI,dI] = linearcon_data(inv);
      [Xsim,Tstamp,Telapsed]= compute_flow_sim2(AM,Ainv,b,X0,CI,dI,T);
      Xini = X0;
      Vini = vertices(Xini);
    	eAT = expm(AM*Telapsed/Ninterval);
 		displacement = step_response(AM,Ainv,b,Telapsed/Ninterval);
      for k = 1:Ninterval
    		Vk = transform(Vini,eAT,displacement);
    		Xk = transform(Xini,eAT,displacement);
         CH = polyhedron(Vini|Vk);
         plot(CH,[0.5 0.7 0.6]);
         Vini = Vk;
         Xini = Xk;
      end
      if ~hard_calc
      	eAT = expm(AM*Tstamp);
 			displacement = step_response(AM,Ainv,b,Tstamp);
      	Xpoly  = transform(Xsim,eAT,displacement);
         REG2 	 = polyhedron(vertices(Xsim)|vertices(Xpoly));
         CI 	 = get_param(REG2,'CI'); 
         dI 	 = get_param(REG2,'dI'); 
         REG	 = linearcon([],[],CI,dI);
      else
         REG = psim_lin(AM,b,Xsim,Tstamp,1);
         fprintf(1,'REG length %4.0f \n',length(REG));
      end
      for j=1:length(REG)
         save tete inv REG
        	plot(inv&REG(j),[.7 .7 .6]);%[i*0.0005 j*0.2 0.8])
      end
	end
   fprintf(1,'Iteration # %4.0f \n',i)
   pause;
end
fprintf('done! \n');
end
return
% -----------------------------------------------------------------------------

function [Xsim,Tstamp,Telapsed] = compute_flow_sim2(A,Ainv,b,X0,CI,dI,T)

% simulate the dynamics  for each vertice, finding the new set of points
% where vector field leaves the invariant. Only for a `linear` dynamics.
%
% Syntax:
%   "[Xsim,Tstamp] = compute_flow_sim(A,Ainv,b,X0,CI,dI,T)"
%
% Description:
%   Simulate each vertice, approximating the time when trajectory crosses
%   any hyperplane. Returns:
%
%   - The new vertices (For the time when first trajectory hits a hyperplane)
%   
%   - Time stamp between the first and the last crossing of any hyperplane
%
%    The inputs to this function are
%
%   * "A": the system matrix
%
%   * "Ainv": the inverse of "A" if it exists, otherwsie it should be
%     "[]"
%
%   * "b": constant input vector for the affine dynamics
%
%   * "X0": a "linearcon" object represeting the initial set
%
%   * "CI","dI": sample points on "X0" that are simulated to construct the
%     convex hull.
%
%   * "T": the time step for the trajectory
%
%   The output "Xsim" is a "linearcon" object,(a polytope) representing the
%   transformed segment approximation. "Tstamp" is a real number recording the
%   time stamp for the flowpipe which englobes the intersection region with 
%   the invariant.
%
% Implementation:
%   To approximate the segment, we do the following:
%   
%   1) simulate the points until the first trajectory hits a hyperplane. 
%      Record the value of the vertices at that time and also the time value.
%
%   2) continue simulation until the last trajectory  hits a hyperplane.
%      Record the time value.
%
%    The simulation of each point uses the following expression:
%
%   "x(T) = e^{A*T}*x(0) + inv(A)*(e^{A*T} - I)*b"
%
% See Also:
%   stretch_func_lin,step_response,psim_lin,fs_lin_map,linearcon,transform,
%   clean_up

timing = 0;
init_T = T;
max_counter = 4;%approx_param.max_bissection;
%eAT 	 = expm(A*T);
%displacement = step_response(A,Ainv,b,T);

% Transfrom sample points by eAT
crossing = 0;
bissection_counter = 0;

[point_index,hyperplane_index] = check_crossing(vertices(X0),CI,dI);
if length(point_index)
   crossing = 1;
   bissection_counter = max_counter;
   fprintf(1,'initial set has 1 or more vertices outside invariant\n')
end
while (~crossing) | (bissection_counter <= max_counter)
	eAT 	 = expm(A*T);
	displacement = step_response(A,Ainv,b,T);
   Xt = transform(X0,eAT,displacement);
   % Check if any point cross a hyperplane
   [point_index,hyperplane_index] = check_crossing(vertices(Xt),CI,dI);
   if length(point_index)
      crossing = 1;
      T = T/2;
		bissection_counter = bissection_counter + 1;
   else
   	X0 = Xt;   
     	crossing = 0;
      timing = timing + T;
   end
end
Telapsed = timing;
Xsim = X0;
T = init_T;
all_crossing = 0;
bissection_counter = 0;
time_stamp = 0;
while (~all_crossing) | (bissection_counter <= max_counter)
	eAT 	 = expm(A*T);
	displacement = step_response(A,Ainv,b,T);
   Xt = transform(X0,eAT,displacement);
   % Check if all point cross a hyperplane
   [point_index,hyperplane_index] = check_crossing(vertices(Xt),CI,dI);
   if length(point_index)==length(vertices(Xt))
      all_crossing = 1;
      bissection_counter = bissection_counter + 1;
      if (bissection_counter > max_counter)
         time_stamp = time_stamp + T;
      end
      T = T/2;
   else
      X0 = Xt;
      time_stamp = time_stamp + T;
      all_crossing = 0;
   end
end
Tstamp = time_stamp;

return

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

function [point_index,hyperplane_index] = check_crossing(Vt,CI,dI)

% Check whether any point of Vt cross the boundary of the invariant INV.
%
% Vt               : Set of vertices (vertices object)
% CI,dI            : Matrix and vector with information about the invariant.
% 
% Returns:
%
% point_index      : An vector with the vertices outside the boundary. 
%                    Vertices in the boundary are considered IN. If no vertice 
%                    is found, point_index returns empty.
% hyperplane_index : A matrix where j-th row informs in which side of the hyperplane
%                    the j-th point in point_index is located. If no point_index is
%                    empty, hyperplane_index is also empty.

global GLOBAL_APPROX_PARAM

point_index = vertices;
hyperplane_index = [];
for i = 1: length(Vt)
	var_comp = CI*Vt(i) - dI < 0 + GLOBAL_APPROX_PARAM.poly_point_tol;
	if var_comp
   else
      point_index = point_index | Vt(i);
      hyperplane_index((size(hyperplane_index,1) + 1),:) = var_comp';
	end
end
return