stretch_func_ode_for_dha.m

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

function f = stretch_func_ode_for_DHA(X,sys_eq,ode_param,n_vector,t0,tf,dimension)

% Compute objective function for the optimization problem in the flow pipe
% segment approximation procedure for `nonlinear` dynamics.
% 
% Syntax:
%   "[f,g] = stretch_func_ode(X,sys_eq,ode_param,n_vector,C_matrix,d_vector,t0,tf)"
%  
% Description:
%   Compute the following objective function value for the given "x0" and "t"
% 
%
%
%   "max (x0 in X0) n_vector'*x(tf,x0)"
%
%
%
%   where "n_vector" is the normal vector and "x(tf,x0)" denotes the solution
%   to the nonlinear differential equation.
%
% Implementation:
%   The ODE solution "x(tf,x0)" is computed for the given "tf" and "x0" using
%   the ODE solver "ode45" in MATLAB.  The optimization variables for the
%   objective function are passed in as a single variable "X" whose first
%   "n" element is the vector "x0".  The
%   objective function for the optimization is a function of "X"
%   only. "sys_eq", "ode_param", "n_vector","C_matrix", "d_vector", "t0",
%   and "tf" are optional parameters (See MATLAB help on "constr.m" for more
%   detail). "g" represents the constraints for the optimization problem,
%   which are that "x0" must be in the set "X0" (specified by the
%   "C_matrix"-"d_vector" pair) and that "t" must lie in
%   "[t0,tf]". "ode_param" contains optional parameters for the ODE file
%   (see MATLAB help on "odefile" for more detail).
%
% See Also:
%   seg_approx_ode

%fprintf('\nstrech_func_ode> X: %3.0f %3.0f',size(X));   


x0 = X(1:dimension,1);

  t=tf;
  p= X(dimension+1:end,1);

if (t ~= 0)
    %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,t0,ode_param);
        
        
    % syntax: ODE45('F',TSPAN,Y0,OPTIONS,P1,P2,...)
    [T,X] = ode45(sys_eq,[t0 tf],x0,[],ode_param,[],x0);
        
    %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);
        
    xt =  X_update';
    
else
  xt = x0;
end

% objective function value
f = -n_vector'*xt;