📄 compute_mapping_sd.m
字号:
dest_cell = 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 -- %
clock_number = GLOBAL_PIHA.Locations{srcloc}.transitions{m}.clock;
flag_process_transition =0;
if ~isempty(clock_number)
if clock_number==i
flag_process_transition =1;
end
end
if ~flag_process_transition
%pause
end
if 1%flag_process_transition
% -- 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
% -- Test whether the cell is included in the list of interior cells for the location -- %
if isempty(GLOBAL_PIHA.Locations{dstloc}.interior_cells)
cell_index=[];
else
[var01,var02,cell_index]=intersect(dest_cell,GLOBAL_PIHA.Locations{dstloc}.interior_cells);
if isempty(cell_index)
interior_cells=GLOBAL_PIHA.Locations{dstloc}.interior_cells;
same_cells=0;
for int_counter =1: length(interior_cells)
if isequivalent(interior_cells(int_counter),dest_cell)
same_cells=1;
dest_cell = interior_cells(int_counter);
break;
end%if
end%for
if ~same_cells
cell_index=[];
end%if
end%if
end%if
% -- cycle through all the interior cells for the location of the destination -- %
found_intersection_region = 0;
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 -- %
if ~isempty(and(mapping_pos_reset,return_invariant(cell_number)))
found_intersection_region=1;
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')
mapping_pos_bb = bounding_SD_box(dst_struc{mapping_counter});
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%
if ~found_intersection_region & isempty(cell_index)
add_region(dstloc,dest_cell);
cell_number = dest_cell;
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')
mapping_pos_bb = bounding_SD_box(dst_struc{mapping_counter});
else
mapping_pos_bb = dst_struc{mapping_counter}.mapping;
end
dst_struc{mapping_counter}.mapping = mapping_pos_bb;
mapping_counter = mapping_counter + 1;
end
else
terminal{length(terminal)+1} = destination_sfm;
end %if%
end%if%
transition_found =1;
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 ~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
%destination = dst_struc;
if PLOT
%close(fig)
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
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -