Artifact 33262152e0b5a3d4c0ace8138670738014fa9e2cfaa6feed94cccba262f7a0cb:


function [y,u,t,y_e,t_e] = ppp_lin_run (Name,Simulate,ControlType,w,p_c,p_o)

  ## usage:  [y,u,t,y_e,t_e] = ppp_lin_run (Name,Simulate,ControlType,w,p_c,p_o);
  ##
  ## 
  ## Linear closed-loop PPP of lego system (and simulation)
  ##
  ## Name: Name of system (in mtt terms)
  ## Simulate = 0: real thing
  ## Simulate = 1: simulate
  ## Control = 0:  step test
  ## Control = 1:  PPP open-loop
  ## Control = 2:  PPP closed-loop
  ## w is the (constant) setpoint
  ## par_control and par_observer are structures containing parameters
  ## for the observer and controller

  ##Defaults
  if nargin<1			# Default name to dir name
    names = split(pwd,"/");
    [n_name,m_name] = size(names);
    Name = deblank(names(n_name,:));
  endif

  ## System
  sys = mtt2sys(Name);		# Create system
  [A,B,C,D] = sys2ss(sys);	# SS form
  [n_x, n_u, n_y] = abcddim(A,B,C,D); # Dimensions
  
  if nargin<2
    Simulate = 1;
  endif
  
  if nargin<3
    ControlType = 2;
  endif
  
  if nargin<4
    w = 1;
  endif
  
  if nargin<5
    p_c.N = 10;
  endif
  
  if nargin<6
    p_o.sigma = 0.001;
  endif


#   if !struct_contains(p_c,"N")
#     p_c.N = 10;			# Number of small samples per large sample
#   endif
  
  if !struct_contains(p_c,"delta_ol")
    p_c.delta_ol = 1.0;	# OL sample interval
  endif
  
  if !struct_contains(p_c,"T")
    p_c.T = 5.0;			# Last time point.
  endif
  
  if !struct_contains(p_c,"A_w")
    p_c.A_w = 0;
  endif
  
  if !struct_contains(p_c,"A_u")
    p_c.N_u = 3;
    a_u = 2.0;
    p_c.A_u = ppp_aug(p_c.A_w,laguerre_matrix(p_c.N_u-1,a_u));
  endif
  
  if !struct_contains(p_c,"Method")
    p_c.Method = "lq"; 
    p_c.Q = eye(n_y);
    p_c.R = (0.1^2)*eye(n_u);
    p_c.N_u = n_x;
  endif
  
  [p_c.N_u,M_u] = size(p_c.A_u);
  if (p_c.N_u<>M_u)
    error("A_u must be square");
  endif
  
  ## Check w.
  [n_w,m_w] = size(w);
  if ( (n_w<>n_y) || (m_w<>1) )
    error(sprintf("ppp_lin_run: w must a column vector with %i elements",n_y));
  endif
  
  ## Initialise
  x_0 = zeros(n_x,1);
  x_est = x_0;

  ## Initilise simulation state
  x = x_0;
##x(2) = 0.2;		
		#   x(2) = y_0(1);
				#   x(4) = y_0(2);

  if ControlType==0		# Step input
    I = 1;			# 1 large sample
    p_c.delta_ol = p_c.T	# I
    K_w = zeros(p_c.N_u,n_y);
    K_w(1,1) = 1;
    K_w(2,1) = -1;
    K_x = zeros(p_c.N_u,n_x);
    U = K_w*w;			# Initial control U
  else				# PPP control
    I = ceil(p_c.T/p_c.delta_ol); # Number of large samples
    if strcmp(p_c.Method, "original")
      tau = [10:0.1:11]*(2/a_u);	# Time horizons
      [k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,tau); # Design
    elseif strcmp(p_c.Method, "lq")
      tau = [0:0.001:1.0]*5; # Time horizons
      [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,p_c.A_u] \
	  = ppp_lin_quad (A,B,C,D,tau,p_c.Q,p_c.R);
    else
      error(sprintf("Method %s not recognised", p_c.Method));
    endif
    
    U = K_w*w;			# Initial control U

    ## Checks
    [ol_zeros, ol_poles] = sys2zp(sys)
    cl_poles = eig(A - B*k_x)
  endif

  ## Observer design
  Ad = expm(A*p_c.delta_ol);		# Discrete-time transition matrix
  if (ControlType==2)
    G = eye(n_x);			# State noise gain 
    sigma_x = eye(n_x);		# State noise variance
    Sigma = p_o.sigma*eye(n_y);	# Measurement noise variance
    
    L = dlqe(Ad,G,C,sigma_x,Sigma)
  else
    L = zeros(n_x,n_y);
  endif
  
  obs_poles = eig(Ad-L*C)

  ## Short sample interval
  dt = p_c.delta_ol/p_c.N;

  ## Control loop
  y = [];
  u = [];
  t = [];
  y_e = [];
  t_e = [];
  for i=1:I
    i
    if Simulate
      t_sim = [0:p_c.N]*dt;
      [yi,ui,xsi] = ppp_ystar (A,B,C,D,x,p_c.A_u,U,t_sim);
      x = xsi(:,p_c.N+1);
      y_now = yi(:,p_c.N+1);
    else			# The real thing
      to_rt(U');		# Send U
      data = from_rt(p_c.N);	# Receive data
      [yi,ui] = convert_data(data);
      y_now = yi(:,p_c.N);	# Current output
    endif
    

    ## Zero-gain (OL) observer with state resetting
    [x_est y_est] = ppp_int_obs (x_est,y_now,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
    
				#       ## Reset states
				#       x_est(2) = y_now(1);	# Position
				#       x_est(4) = y_now(2)/g_s;	# Angle 
    
    ##Control
    U = K_w*w- K_x*x_est;

    ## Save
    ti  = [(i-1)*p_c.N:i*p_c.N-1]*dt; 
    t = [t;ti'];
    y = [y;yi(:,1:p_c.N)'];
    u = [u;ui(:,1:p_c.N)'];
    y_e = [y_e; y_est'];
    t_e = [t_e; (i*p_c.N)*dt];
  endfor

  ## Put data on file (so can use for identification)
  filename = sprintf("%s_ident_data.dat",Name);
  eval(sprintf("save -ascii %s t y u",filename));


  ## Plot
  gset nokey
  title("");
  boxed=0;
  monochrome=1;
  grid;
  xlabel("t");

  ylabel("y");
  figure(1);plot(t,y, t_e,y_e,"+");
 				#  figfig("OL_y","eps",boxed,monochrome);
  ylabel("u");
  figure(2);plot(t,u);
 				#  figfig("OL_u","eps",boxed,monochrome);

endfunction

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