seg_approx_dha_ode.m

一个matlab的将军模型

function [SEG,SPf] = seg_approx_DHA_ode(sys_eq,ode_param,X0,SP0,t0,Pcon,period)

% Approximate a single segment of a flow pipe for a `nonlinear` dynamics.
%
% Syntax:
%   "[SEG,SPf] = seg_approx_DHA_ode(sys_eq,ode_param,X0,SP0,t0,tf)"
%
% 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
%
%   * "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.
%
%   * period: period of the current sample.  The linearcon returned by the function
%             will be the reachable set after 'period' time has passed.
%
%   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".
%
% 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


    
% Simulate sample points from t0 to tf
   [SPf,Siminff] = simulate_points(SP0,sys_eq,ode_param,t0,t0+period,Pcon);
   SPf=vertices(polyhedron(SPf)); % Removes unnecessary vertices
   CH = polyhedron(SPf);

if isempty(Pcon)
    Pcon = linearcon([],[],[],[]);
end   
   
% Compute convex hull from these extreme points
[CE,dE,CI,dI] = linearcon_data(linearcon(CH));
%If there are equality constraints, add them to the list of faces.
if ~isempty(CE)
    for i=1:length(dE)
        CI = [CI ; CE(i,:) ];
        CI = [CI ; -CE(i,:) ];
        dI = [dI ; dE(i,1) ];
        dI = [dI ; -dE(i,1) ];
    end
end
[CInew,dInew] = shwrap_ode(CI,sys_eq,ode_param,X0,t0,t0+period,Pcon,Siminff);
for k = 1:length(dI)
  if (dInew(k) > dI(k))
    dI(k) = dInew(k);
  else
    if (dInew(k) < dI(k) - poly_param('epsilon'))
      fprintf(1,['Warning: Optimization result is not reliable, falling' ...
	    ' back to the convex hull approximation\n']);
    end
  end
end
SEG = clean_up(linearcon([],[],CI,dI));
return

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

function [Pf, Siminf] = simulate_points(X0,sys_eq,ode_param,t0,tf,Pcon);

X0=vertices(X0);
Siminf=[];
Param=vertices(Pcon);
Pf = vertices;
if isempty(Param) 
    for k = 1:length(X0)
         %Now propagate the controller states forward one sample step,
         %being sure to use the old controller states to propagate the 
         %continuous part of the system (the plant).  This must be done
         %because the k+1'th controller state is calculated, but the
         %k'th controller state must be used to determine the controller
         %output for this sample period.
         [x_contr,nx,nz] = one_step_of_controller(X0(k),t0,ode_param);
        
        % syntax: ODE45('F',TSPAN,Y0,OPTIONS,P1,P2,...
        [T,X] = ode45(sys_eq,[t0 tf],X0(k),[],ode_param,[],X0(k));
        
        %Now combine the discrete time controller update from the two 
        %update operations.
        X_update(1,1:nx) = X(end,1:nx);
        X_update(1,nx+1:nx+nz) = x_contr(nx+1:nx+nz);
        
        temp_length = length(Pf);
        Pf = Pf | X_update';
        if length(Pf)>temp_length
            Siminf=[Siminf; struct('X0',X0(k),'P0',[],'T',T,'X',X_update)];
        end
    end;
else
    for i=1:length(Param)
        for k = 1:length(X0)
            
            %This parameter section is, as of yet, untested.
            
            [x_contr,nx,nz] = one_step_of_controller(X0(k),t0,ode_param);
            
            % syntax: ODE45('F',TSPAN,Y0,OPTIONS,P1,P2,...)
            [T,X] = ode45(sys_eq,[t0 tf],X0(k),[],ode_param,Param(i));
            
            %Now combine the discrete time controller update from the two 
            %update operations.
            X_update(1,1:nx) = X(end,1:nx);
            X_update(1,nx+1:nz) = x_contr(nx+1:nz);
        
            Pf = Pf | X_update';
            Siminf=[Siminf; struct('X0',X0(k),'P0',Param(i),'T',T,'X',X_update)];
           
        end;
    end;
end;
return

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

function [C,d] = shwrap_ode(C,sys_eq,ode_param,X0,t0,tf,Pcon,Siminf)

% Shrink wrap the flow pipe segment in a polytope using the normal vectors
% stored in the rows of the given matrix C, i.e. compute the smallest linear
% constraint set given the set of directions C that contains the flow pipe
% segment at tf.
% C             : a set of directions stacked together in a matrix
% sys_eq        : a string containing file name of the derivative
%                 function for the ODE
% ode_param     : optional parameters for the ODE file
% X0            : a constraint representing initial set X0
% t0/tf         : start/final time with respect to X0
% Pcon : A linearcon object representing the constraint on the parameters.
%   Siminf : A structure that records information about the trajectory
%       Siminf.X0 = initial point for the trajectory
%       Siminf. P0 = parameter used for computing the trajectory
%       Siminf.T = vector of simulation times for the trajectory
%       Siminf.X = vector of simulated points. 

% Siminfo contains information from the simulation in order to make a better 
% initial guess

% extract linear inequality constraints defining X0

[CE,dE,CI,dI] = linearcon_data(X0);
[CPE,dPE,CPI,dPI] = linearcon_data(Pcon);
    
% (1) inequality constraints

    [CIn,CIm]=size(CI);
        [CPIn,CPIm]=size(CPI);
        % Remember the dimensions of the state space.
        dimension=CIm;
  
    
    fminCI=[CI,zeros(CIn,CPIm);zeros(CPIn,CIm), CPI];
    fmindI=[dI;dPI];
       

    % (2) equality constraints (not dependent on t)
    [CEn,CEm]=size(CE);
    [CPEn,CPEm]=size(CPE);

    fminCE=[];
    fmindE=[];
    if ~isempty(CE)
        fminCE=[CE,zeros(CEn,CPIm)];%CPIm+1
        fmindE=dE;
    end;
    if ~isempty(CPE)
        fminCE=[fminCE;zeros(CPEn,CIm),CPE];%CIm+1
        fmindE=[fmindE;dPE];
    end;

% (3) Set the optimization parameters

options = optimset;
options.TolFun =0.2*1e-9;%f(3) of foptions
options.iter = 4e8*size(C,2);%f(14) of foptions
options.Display=['off'];
options.Diagnostics=['off'];
options.LargeScale=['off'];

% Stretch along each given direction to cover the flow pipe segment
d = zeros(size(C,1),1);        


% (4) Optimize for each direction

for l = 1:size(C,1)
  n_vector = C(l,:)';
  dlmax = Inf;
  
  
      Pinit=Siminf(1).P0;
      Xinit=Siminf(1).X0;

        
    [Xopt,dl] = fmincon('stretch_func_ode_for_DHA',[Xinit;Pinit], fminCI,fmindI, fminCE,fmindE,[],[],[],options,...
             sys_eq,ode_param,n_vector,t0,tf,dimension);
    
    % The end of my hack  <- Ansgar
    if (dl < dlmax)
        dlmax = dl;
    end
    d(l) = -dlmax;
end
return