File mttroot/mtt/lib/control/PPP/ppp_nlin_sim.m artifact 26d1c64307 part of check-in 5db724bef9


function [y,x,u,t,p,UU,UU_c,UU_l,t_ppp,t_est] = ppp_nlin_sim (system_name,i_ppp,i_par,A_u,w_s,N_ol,extras)

  ## usage: [y,x,u,t,p,UU,UU_c,UU_l,t_ppp,t_est] = ppp_nlin_sim (system_name,i_ppp,i_par,A_u,w_s,N_ol,extras) 
  ##
  ## 
  
  ## Simulate nonlinear PPP
  ## Copyright (C) 2000 by Peter J. Gawthrop

  ## Defaults
  if nargin<7
    extras.U_initial = "zero";
    extras.U_next = "continuation";
    extras.criterion = 1e-5;
    extras.max_iterations = 10;
    extras.v = 0.1;
    extras.verbose = 0;
    extras.estimate = 1;
  endif
  
  

  ## Names
  s_system_name = sprintf("s%s", system_name);

  ## System details -- defines simulation within ol interval
  par = eval(sprintf("%s_numpar;", system_name));
  simpar = eval(sprintf("%s_simpar;", system_name));
  x_0 = eval(sprintf("%s_state(par);", system_name));
  [n_x,n_y,n_u] = eval(sprintf("%s_def;", system_name));

  ## Sensitivity system details -- defines moving horizon simulation
  simpars = eval(sprintf("%s_simpar;", s_system_name));
  pars = eval(sprintf("%s_numpar;", s_system_name));

  ## Times
  ## -- within opt horizon
  n_Tau = round(simpars.last/simpars.dt);
  dtau = simpars.dt;
  Tau = [0:n_Tau-1]'*dtau;
  [n_tau,n_w] = size(w_s);
  tau = Tau(n_Tau-n_tau+1:n_Tau);
  w = w_s(length(w_s));		# Final value of setpoint

  ## -- within ol interval
  n_t = round(simpar.last/simpar.dt);
  dt = simpar.dt;
  t_ol = [0:n_t-1]'*dt;
  T_ol = n_t*dt;

  ## Create input basis functions
  [n_U,junk] = size(A_u);

  ## For moving horizon
  eA = expm(A_u*dtau);
  u_star_i = ones(n_U,1);
  u_star_tau = [];
  for i = 1:n_Tau
    u_star_tau = [u_star_tau; u_star_i'];
    u_star_i = eA*u_star_i;
  endfor

  ## and for actual implementation
  eA = expm(A_u*dt);
  u_star_i = ones(n_U,1);
  u_star_t = [];
  for i = 1:n_t
    u_star_t = [u_star_t; u_star_i'];
    u_star_i = eA*u_star_i;
  endfor
  
  if extras.verbose
    title("U*(tau)")
    xlabel("tau");
    plot(Tau,u_star_tau)
    title("U*(t)")
    xlabel("t_ol");
    plot(t_ol,u_star_t)
  endif
  

  ## Check number of inputs adjust if necessary
  if n_u>n_U
    disp(sprintf("Augmenting inputs with %i zeros", n_u-n_U));
    u_star_tau = [u_star_tau; zeros(n_u-n_U, n_Tau)];
    u_star_t   = [u_star_t; zeros(n_u-n_U, n_t)];
  endif
  
  if n_u<n_U
    error(sprintf("n_U (%i) must be less than or equal to n_u (%i)", \
		  n_U, n_u));
  endif
  
  ## Compute linear gains 
  [A,B,C,D] = eval(sprintf("%s_sm(par);", system_name));
  B = B(:,1); D = D(:,1);
  [k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,A_u,0,tau');

  ## Main simulation loop
  y = [];
  x = [];
  u = [];
  t = [];

  p = [];

  t_last = 0;
  UU = [];
  UU_l =[];
  UU_c =[];
  
  t_ppp = [];
  t_est = [];

  x_0s = zeros(2*n_x,1);

  if  strcmp(extras.U_initial,"linear")
    U = K_w*w - K_x*x_0;
  elseif strcmp(extras.U_initial,"zero")
    U = zeros(n_U,1);
  else
    error(sprintf("extras.U_initial = \"%s\" is invalid", extras.U_initial));
  endif

  ## Reverse time to get "previous" U
  U = expm(-A_u*T_ol)*U;

  for i = 1:N_ol		# Main loop 
    ## Compute initial U from linear case
    U_l = K_w*w - K_x*x_0;

    ## Compute initial U  for next time from continuation trajectory
    U_c = expm(A_u*T_ol)*U;

    ## Create sensitivity system state
    x_0s(1:2:2*n_x) = x_0;
    
    ## Set next U (initial value for optimisation)
    if  strcmp(extras.U_next,"linear")
      U = U_l;
    elseif strcmp(extras.U_next,"continuation")
      U = U_c;
    elseif strcmp(extras.U_next,"zero")
      U = zeros(n_U,1);
    else
      error(sprintf("extras.U_next = \"%s\" is invalid", extras.U_next));
    endif
    pars(i_ppp(:,1)) = U;	# Put initial value of U into the parameter vector

    ## Compute U by optimisation
    tick = time;
    if extras.max_iterations>0
      [U, U_all, Error, Y] = ppp_nlin(system_name,x_0s,pars,simpars,u_star_tau,w_s,i_ppp,extras);
      pars(i_ppp(:,1)) = U;	# Put final value of U into the parameter vector
    else
      Error = [];
    endif
    ppp_time = time-tick;  
    t_ppp = [t_ppp;ppp_time];
    
    ## Generate control
    u_ol = u_star_t*U;		# Not used - just for show

    ## Simulate system over one ol interval
    par(i_ppp(:,3)) = pars(i_ppp(:,1)); # Update the simulation ppp weights
    [y_ol,x_ol] = eval(sprintf("%s_sim(x_0, par, simpar, u_star_t);", system_name));


    ## Tune parameters/states
    if (extras.estimate==1)
      tick = time;
      par_est = pars(i_par(:,1));
      p = [p; par_est'];
      pars = ppp_optimise(s_system_name,x_0s,pars,simpar,u_star_t,y_ol,i_par,extras);
      est_time = time-tick;  
      t_est = [t_est;est_time];
    endif

    x_0  = x_ol(n_t+1,:)';	# Extract state for next time
    y_ol = y_ol(1:n_t,:);	# Avoid extra points due to rounding error 
    x_ol = x_ol(1:n_t,:);	# Avoid extra points due to rounding error 
    
    
    y = [y; y_ol];
    x = [x; x_ol];
    u = [u; u_ol];

    UU = [UU; U'];
    UU_l = [UU_l; U_l'];
    UU_c = [UU_c; U_c'];

    t = [t; t_ol+t_last*ones(n_t,1) ];
    t_last = t_last + T_ol; 


  endfor

endfunction


MTT: Model Transformation Tools
GitHub | SourceHut | Sourceforge | Fossil RSS ]