seg_approx_ode.m

CheckMate is a MATLAB-based tool for modeling, simulating and investigating properties of hybrid dyn

function [SEG,SPf,intersect_flag] = seg_approx_ode(sys_eq,ode_param,X0,INV,SP0,t0,tf,Pcon)

% Approximate a single segment of a flow pipe for a `nonlinear` dynamics.
%
% Syntax:
%   "[SEG,SPf,intersect_flag] = seg_approx_ode(sys_eq,ode_param,X0,INV,SP0,t0,tf,Pcon)"
%
% Description:
%   Compute a conservative approximation of a flow pipe segment in the time
%   interval "[t0,tf]" for the `nonlinear` dynamics "dx/dt = f(x)". The
%   inputs to this function are
%
%   * "sys_eq": string containing file name of the derivative function
%     for the ODE (i.e., overall_system_ode in the directory \util)
%
%   * "ode_param": optional parameters for the ODE file
%
%   * "X0": a "linearcon" object representing initial set
% 
%   * "INV": a "linearcon" object representing invariant set
%
%   * "SP0": sample points that are simulated to construct the convex hull,
%     given at time t0
%
%   * "t0"/"tf" : start/final time with respect to "X0"
%
%   * Pcon : linearcon object representing the constraint on the parameters.
%
%   The outputs of this function are
%
%   * "SEG": a "linearcon" object (a polytope) representing the flow pipe
%     segment approximation.
%   
%   * "SFf":  sample points the are simulated from "SP0" to the time "tf".
% 
%   * "intersect_flag": flag to indicate intersection with boundary
% 
% Implementation:
%   To approximate the flow pipe segment, we first simulate the sample
%   points from times "t = t0" to "t = tf" and compute the convex hull of
%   the sample points at time "t = t0" and "t = tf". After the convex hull
%   is obtained, we extract the matrix-vector pair "[CI,dI]" that represents
%   the linear inequality "CI*x <= dI" for the convex hull. The normal
%   vectors for the faces of the conex hull, which are the rows of the
%   matrix "CI", are used to solve the optimization problem.
%
%
%   
%   "max (x0 in X0, t in [0,T]) ci'*x(t,x0)"
%
%
%
%   where "ci" is the "i"-th normal vector and "x(t,x0)" denotes the
%   solution to the nonlinear differential equation. The above optimization
%   problem is solved by calling the MATLAB routine for solving constrained
%   nonlinear optimization problem "constr". The computation of the
%   objective function, which includes numerical intregration of the ODE to
%   find the solution "x(t,x0)" from "x0" at time "t" is done in the
%   function "stretch_func_ode()".
%
%
%
%   After solving the optimization problem, we adjust the inequality
%   constraints for the convex hull to be "CI*x <= dInew" where "dInew(i)"
%   is the solution obtained for the "i"-th optimization problem. This, in
%   effect, expands the convex hull to cover the flow pipe segment. Put the
%   new linear inequality constraints into a "linearcon" object, call the
%   function "clean_up()" to remove redundant constraints, and return the
%   result.
%   
%
% See Also:
%   stretch_func_lin,step_response,psim_lin,fs_lin_map,linearcon,transform,
%   clean_up

% [SEG,SPf] = seg_approx_ode(sys_eq,ode_param,X0,SP0,t0,tf)
% Compute the outer approximation of the flow pipe segment from t0 to tf

global GLOBAL_APPROX_PARAM

% Simulate sample points from t0 to tf
[SPf, Siminf] = simulate_points(X0,sys_eq,ode_param,tf,Pcon);
SPf=vertices(polyhedron(SPf)); % Removes unnecessary vertices

% Compute convex hull from these extreme points
CH = polyhedron(SP0 | SPf);
[CE,dE,CI,dI] = linearcon_data(linearcon(CH));



SEG = hyperrectangle(sys_eq,ode_param,X0,t0,tf,Pcon,Siminf,SP0,SPf);

% >>>>>>>>>>>> Changing Algorithms -- DJ -- 06/30/03 <<<<<<<<<<<<
% Currently, we have a different algorithm to computation the approximation
% of the segment. Dong Jia comments the following codes
% if intersect_flag>0
% 	for k = 1:length(dI)
%       if (dInew(k) > dI(k))
%         dI(k) = dInew(k);
%       else
% 	%    if (dInew(k) < dI(k) - GLOBAL_APPROX_PARAM.poly_epsilon)
% 	%         fprintf(1,['\nWarning: Optimization did not improve the results of the convex hull approximation\nduring reachability analysis.']);
% 	%    end
%       end
% 	end
% else
%     CI=CInew;
%     dI=dI(length(dI)-length(CI1index)+1:length(dI));
% 	for k = 1:length(dI)
%       if (dInew(k) > dI(k))
%         dI(k) = dInew(k);
%       else
% 	%    if (dInew(k) < dI(k) - GLOBAL_APPROX_PARAM.poly_epsilon)
% 	%         fprintf(1,['\nWarning: Optimization did not improve the results of the convex hull approximation\nduring reachability analysis.']);
% 	%    end
%       end
% 	end
% end
% 
% SEG = clean_up(linearcon([],[],CInew,dInew));
% >>>>>>>>>>>> -------------- end (Changing Algorithms) --------------- <<<<<<<<<<<<

% >>>>>>>>>>>> Bounding Approximation -- DJ -- 06/30/03 <<<<<<<<<<<<
% After computing the hyperrectangle hull approximation, we used the faces
% of the invariant set to bound the approximation if the hyperrectangle
% intersect with the boundary of the invariant.
% Added by Dong Jia to handle the case if the hyper-rectangle hull
% approximation intersect with borders of the INV set.
intersect_flag=~issubset(SEG,INV)&&isfeasible(SEG,INV);

if intersect_flag
    [CE,dE,C,d] = linearcon_data(SEG);
    [CE,dE,CI,dI] = linearcon_data(X0);
	[CPE,dPE,CPI,dPI] = linearcon_data(Pcon);
	
	%This section has been altered (3/2002) to accomodate lower dimensional linearcon
	%objects.  This was introduced specifically for parametric analysis.
	% (1) inequality constraints
	
        [CIn,CIm]=size(CI);
        [CPIn,CPIm]=size(CPI);
        xdim=dim(X0);
        pdim=dim(Pcon);
	
        % Remember the dimensions of the state space.
        dimension=xdim;
        fminCI=[zeros(1,xdim) 1 zeros(1,pdim); zeros(1,xdim) -1 zeros(1,pdim);];%CIm+1
        fmindI=[tf;-t0];
        
        if ~isempty(CI)
            fminCI=[fminCI;CI,zeros(CIn,pdim+1)];%CPIm+1
            fmindI=[fmindI;dI];
        end;
        
        if ~isempty(CPI)
            fminCI=[fminCI;zeros(CPIn,xdim+1),CPI];%CIm+1
            fmindI=[fmindI;dPI];
        end;
	
        % (2) equality constraints
        [CEn,CEm]=size(CE);
        [CPEn,CPEm]=size(CPE);
	
        fminCE=[];
        fmindE=[];
        if ~isempty(CE)
            fminCE=[CE,zeros(CEn,pdim+1)];%CPIm+1
            fmindE=dE;
        end;
        if ~isempty(CPE) 
            if ~isempty(fminCE)
                fminCE=[fminCE;zeros(CPEn,xdim+1) CPE];%CIm+1
                fmindE=[fmindE;dPE];
            else
                fminCE=[zeros(CPEn,xdim+1) CPE];%CIm+1
                fmindE=[dPE];
            end;
        end;
	
	% (3) Set the optimization parameters
	
	tolerance = GLOBAL_APPROX_PARAM.func_tol;
	maxiter = GLOBAL_APPROX_PARAM.max_iter;
	if (tf-t0)>0
        actual_tolerance = (tf-t0)*tolerance;
	else
        actual_tolerance = tolerance;
	end
	

	options = optimset;
	options.TolFun =actual_tolerance;%f(3) of foptions
	options.MaxIter = maxiter*size(C,2);%f(14) of foptions
	options.Display=['off'];
	options.Diagnostics=['off'];
	options.LargeScale=['off'];
	
	
% >>>>>>>>>>>> Finding Intersection -- DJ -- 06/30/03 <<<<<<<<<<<<
	[CE_INV,dE_INV,CI_INV,dI_INV]=linearcon_data(INV);
    [CE_INTER,dE_INTER,CI_INTER,dI_INTER]=linearcon_data(SEG&INV);
    [CI_t,CI_idx]=intersect(CI_INV,CI_INTER,'rows');
% >>>>>>>>>>>> -------------- end (Finding Intersection) --------------- <<<<<<<<<<<<
	
	if GLOBAL_APPROX_PARAM.verbosity>=1
       fprintf('\n\nCompute flowpipe segment with additional %i faces:\n',size(CI_t,1)*2)
	end;
    
% >>>>>>>>>>>> Bounding Approximation by Faces -- DJ -- 06/30/03 <<<<<<<<<<<<
% Bound the approximation be each face of the invariant that intersects
% with the hyperrectangle hull. The compuation is performed in both
% position and negative direction of the norm of each face. In the
% following codes, the variables end with 1 are for the positive direction
% computation and those end with with 2 are for the negative direction
% computaiton.
	for l = 1:length(CI_idx)
      n_vector = CI_INV(CI_idx(l),:)';
      dlmax1 = Inf;
      dlmax2 = Inf;
      
        % compute the best initial state 
        % Store in " tindex"  the index of the time elements of  the vector Siminf(1).T that are greater than t0
        %
        [ts,tindex]=setdiff(Siminf(1).T.*(Siminf(1).T>=t0),0);
        % Computer the max arg i C'x(i)
        [minnx1,nx_index1]=min(-n_vector'*Siminf(1).X(tindex,:)');
        [minnx2,nx_index2]=min(n_vector'*Siminf(1).X(tindex,:)');
        Pinit1=Siminf(1).P0;
        Xinit1=Siminf(1).X0;
        Tinit1=Siminf(1).T(tindex(nx_index1));
        Pinit2=Siminf(1).P0;
        Xinit2=Siminf(1).X0;
        Tinit2=Siminf(1).T(tindex(nx_index1));
        for k0=2:length(Siminf)
            [ts,tindex]=setdiff(Siminf(k0).T.*(Siminf(k0).T>=t0),0);
            [nx1,nx_index1]=min(-n_vector'*Siminf(k0).X(tindex,:)');
            [nx2,nx_index2]=min(n_vector'*Siminf(k0).X(tindex,:)');
            if nx1<minnx1
                minnx1=nx1;
                Pinit1=Siminf(k0).P0;
                Xinit1=Siminf(k0).X0;
                Tinit1=Siminf(k0).T(tindex(nx_index1));
            end;
            if nx2<minnx2
                minnx2=nx2;
                Pinit2=Siminf(k0).P0;
                Xinit2=Siminf(k0).X0;
                Tinit2=Siminf(k0).T(tindex(nx_index2));
            end;
        end;

        if GLOBAL_APPROX_PARAM.verbosity==1
            fprintf('%3i ',2*l-1)
%             if mod(2*l-1,20)==0
%                 fprintf('\n')
%             end
        end;
        if GLOBAL_APPROX_PARAM.verbosity>=2
            fprintf('\n%4i:',2*l-1);
            fprintf('x0=[');
            fprintf(' %4.2f',Xinit1);
            if isempty(Pinit1)
                fprintf(']')
            else
                fprintf(']\tp=[');
                fprintf(' %4.2f',Pinit1);
                fprintf(']');
            end;
        end;
     
        dl1=-minnx1;
        if dl1<=dI_INV(CI_idx(l))
            [Xopt,dl1] = fmincon('stretch_func_ode',[Xinit1;Tinit1;Pinit1], fminCI,fmindI, fminCE,fmindE,[],[],[],options,...
                     sys_eq,ode_param,n_vector,t0,tf,dimension);
             dl1=-dl1;
         end
        
            
        if GLOBAL_APPROX_PARAM.verbosity==1
            fprintf('%3i ',2*l)
            if mod(l,10)==0
                fprintf('\n')
            end
        end;
        if GLOBAL_APPROX_PARAM.verbosity>=2
            fprintf('\n%4i:',2*l);
            fprintf('x0=[');
            fprintf(' %4.2f',Xinit2);
            if isempty(Pinit2)
                fprintf(']')
            else
                fprintf(']\tp=[');
                fprintf(' %4.2f',Pinit2);
                fprintf(']');
            end;
        end;
     
        
        dl2=-minnx2;
        if dl2<=-dI_INV(CI_idx(l))
            [Xopt,dl2] = fmincon('stretch_func_ode',[Xinit2;Tinit2;Pinit2], fminCI,fmindI, fminCE,fmindE,[],[],[],options,...
                     sys_eq,ode_param,-n_vector,t0,tf,dimension);
             dl2=-dl2;
         end
        
        if dl1<=dI_INV(CI_idx(l))
            C=[C;n_vector'];
            d=[d;dl1];
        end
        if dl2<=-dI_INV(CI_idx(l))
            C=[C;-n_vector'];
            d=[d;dl2];
        end
	end
% >>>>>>>>>>>> -------------- end (Bounding Approximation by Faces) --------------- <<<<<<<<<<<<
    
    SEG = linearcon([],[],C,d);
    intersect_flag=~issubset(SEG,INV)&&isfeasible(SEG,INV);
end
% >>>>>>>>>>>> -------------- end (Bounding Approximation) --------------- <<<<<<<<<<<<


return

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

function [Pf, Siminf] = simulate_points(X0,sys_eq,ode_param,tf,Pcon);
global GLOBAL_APPROX_PARAM
X0=vertices(X0);
Siminf=[];
 
Pf = vertices;
if isempty(Pcon) 
    if GLOBAL_APPROX_PARAM.verbosity>=1
        fprintf('\n\nSimulate %i vertices:',length(X0))
    end;
    for k = 1:length(X0)
        if GLOBAL_APPROX_PARAM.verbosity>=1
            fprintf('%3i ',k)
            if mod(k,18)==0
                fprintf('\n')
            end;
        end;
        % syntax: ode15s('F',TSPAN,Y0,OPTIONS,P1,P2,...)
        [T,X] = ode45(sys_eq,[0 tf],X0(k),[],ode_param,[]);
        Pf = Pf | X(end,:)';
        Siminf=[Siminf; struct('X0',X0(k),'P0',[],'T',T,'X',X)];
    end;
else
    
    %The following 3 lines and the above 'if' statement was changed to accomodate
    %the case when the constraint set is empty.  Ansgar Fehnker 3/2002.
    
    [CPE,dPE,CPI,dPI] = linearcon_data(Pcon);
    Pcon=linearcon(CPE,dPE,[CPE;-CPE;CPI],[dPE;-dPE;dPI]);
    Param=vertices(Pcon);
    if GLOBAL_APPROX_PARAM.verbosity>=1
        fprintf('\nSimulate %i vertices for %i parameter values:',[length(X0),length(Param)])
    end;
    for i=1:length(Param)
        if GLOBAL_APPROX_PARAM.verbosity>=1
            fprintf('\nparameter:\t %i\nvertex:',i)
        end
        for k = 1:length(X0)
            % syntax: ode15s('F',TSPAN,Y0,OPTIONS,P1,P2,...)
            if GLOBAL_APPROX_PARAM.verbosity>=1
                fprintf('%3i ',k)
                if mod(k,18)==0
                    fprintf('\n')
                end;
            end;
            [T,X] = ode45(sys_eq,[0 tf],X0(k),[],ode_param,Param(i));
            Pf = Pf | X(end,:)';
            Siminf=[Siminf; struct('X0',X0(k),'P0',Param(i),'T',T,'X',X)];
        end;
    end;
end;
return

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