File mttroot/mtt/lib/control/PPP/ppp_lin.m artifact a68c3b73aa part of check-in 543de8091c


function [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] = ppp_lin(A,B,C,D,A_u,A_w,tau,Q,R,P,max_cond);
  ## usage:
  ## [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,cond_uu] =
  ##  ppp_lin(A,B,C,D,A_u,A_w,tau,Q,R,P,max_cond)

  ##
  ## Linear PPP (Predictive pole-placement) computation 
  ## INPUTS:
  ## A,B,C,D: system matrices
  ## A_u: composite system matrix for U* generation 
  ##      one square matrix (A_ui) row for each system input
  ##      each A_ui generates U*' for ith system input.
  ## OR
  ## A_u: square system matrix for U* generation 
  ##      same square matrix for each system input.
  ## A_w: composite system matrix for W* generation 
  ##      one square matrix (A_wi) row for each system output
  ##      each A_wi generates W*' for ith system output.
  ## tau: row vector of times for optimisation (equispaced in time)
  ## Q column vector of output weights (defaults to unity)
  ## OR
  ## Q matrix,  each row corresponds to time-varying weight compatible with t
  ## max_cond: Maximum condition number of J_uu (def 1/eps)
  ## OUTPUTS:
  ## k_x: State feedback gain
  ## k_w: setpoint gain
  ## ie u(t) = k_w w(t) - k_x x(t)
  ## K_x, K_w: open loop gains
  ## Us0: Value of U* at tau=0
  ## J_uu, J_ux, J_uw, J_xx,J_xw,J_ww: cost derivatives
  ## cond_uu : Condition number of J_uu
  ## Copyright (C) 1999, 2000 by Peter J. Gawthrop
  ## 	$Id$	

  ## Check some dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);
  if (n_x==-1)
    return
  endif
  
  ## Default Q (output weight)
  if nargin<8
    Q = ones(n_y,1);
  endif

  ## Default R (input weight)
  if nargin<9
    R = zeros(n_u,1);
  endif

  ## Default P (terminal weight)
  if nargin<10
    P = zeros(n_x,n_x);
    P_weighting = 0;
  else
    P_weighting = 1;
  endif

  ## Default condittion number
  if nargin<11
    max_cond = 1e20;
  endif
  
  [n_U,m_U] = size(A_u);
  if (n_U != n_u*m_U)&&(n_U != m_U)
    error("A_u must be square or have N_u rows and N_u/n_u columns");
  endif

  if (n_U == m_U)		# U matrix square
    n_U = n_U*n_u;		# So same U* on each input
  endif

  
  [n_W,m_W] = size(A_w);
  if n_W>0
    if (n_W != n_y*m_W)&&(n_W != m_W)
      error("A_w must either be square or have N_w rows and N_w/n_y columns");
    endif
    square_W = (n_W== m_W);	# Flag indicates W is square
    if (n_W == m_W)		# W matrix square
      n_W = n_W*n_y;		# So same W* on each output
    endif
  endif
  

  [n_t,m_t] = size(tau);
  if n_t != 1
    error("tau must be a row vector");
  endif
  dt = tau(2) - tau(1);		# Sample interval

  [n_Q,m_Q] = size(Q);
  if ((m_Q != 1)&&(m_Q != m_t)) || (n_Q != n_y)
    error("Q must be a column vector with one row per system output");
  endif

  if (m_Q == 1)			# Convert to vector Q(i)
    Q = Q*ones(1,m_t);		# Extend to cover full range of tau
  endif
  
  ##Set up initial states
  u_0 = ones(m_U,1);
  w_0 = ones(m_W,1);

  ## Find y_U - derivative of y* wrt U.
  i_U = 0;
  x_0 = zeros(n_x,1);		# This is for x=0
  y_u = [];			# Initialise
  x_u_t = [];			# Initialise
  Us = [];			# Initialise
  for i=1:n_U			# Do for each input function U*_i
    dU = zeros(n_U,1);
    dU(++i_U) = 1;		# Create dU/dU_i 
    [ys,us,xs] = ppp_ystar (A,B,C,D,x_0,A_u,dU,tau); # Find ystar and ustar
    y_u = [y_u ys'];		# Save y_u (y for input u)  with one row for each t.
    Us = [Us us'];		# Save u (input)  with one row for each
				# t.
    x_u_t = [x_u_t xs(:,m_t)];	# x_u at terminal time
  endfor

  Ws = [];			# Initialise
  if n_W>0
    ## Find w*
    i_W = 0;
    x_0 = zeros(n_x,1);		# This is for x=0
    for i=1:n_W			# Do for each setpoint function W*i
      dW = zeros(n_W,1);
      dW(++i_W) = 1;		# Create dW/dW_i 
      [ys,ws] = ppp_ystar ([],[],[],[],[],A_w,dW,tau); # Find ystar and ustar
      Ws = [Ws ws'];		# Save u (input)  with one row for each t.
    endfor
  endif
  


  ## Find y_x - derivative of y* wrt x.
  y_x=[];
  for t_i=tau
    y = C*expm(A*t_i);
    yy = reshape(y,1,n_y*n_x);	# Reshape to a row vector
    y_x = [y_x; yy];
  endfor

  ## Compute the integrals to give cost function derivatives
  [n_yu m] = size(y_u');
  [n_yx m] = size(y_x');
  [n_yw m] = size(Ws');

  J_uu = zeros(n_U,n_U);
  J_ux = zeros(n_U,n_x);
  J_uw = zeros(n_U,n_W);
  J_xx = zeros(n_x,n_x);
  J_xw = zeros(n_x,n_W);
  J_ww = zeros(n_W,n_W);

  ## Add up cost derivatives for each output but weighted by Q.
  ## Scaled by time interval
  ## y_u,y_x and Ws should really be 3D matrices, but instead are stored
  ## with one row for each time and one vector (size n_y) column for
  ## each element of U

  ## Scale Q
  Q = Q*dt;			# Scale to give correct units
  ## Non-w bits
  for i = 1:n_y			# For each output
    QQ = ones(n_U,1)*Q(i,:);	# Resize Q
    J_uu = J_uu + (QQ .* y_u(:,i:n_y:n_yu)') * y_u(:,i:n_y:n_yu);
    J_ux = J_ux + (QQ .* y_u(:,i:n_y:n_yu)') * y_x(:,i:n_y:n_yx);
    QQ = ones(n_x,1)*Q(i,:);	# Resize Q
    J_xx = J_xx + (QQ .* y_x(:,i:n_y:n_yx)') * y_x(:,i:n_y:n_yx);
  endfor

  ## Input weighting (scalar for the moment)
  if (n_u>1)
    warning("Sorry, cant do n_u>1 just now: exiting");
    return
  endif

  ## Scale R
  R = R*dt;			# Scale to give correct units
  for i = 1:m_t
    Ust = Us(i,:);
    J_uu = J_uu + Ust'*R*Ust;
  endfor
  
  ## w bits
  if n_W>0
    for i = 1:n_y			# For each output
      QQ = ones(n_U,1)*Q(i,:);	# Resize Q
      J_uw = J_uw + (QQ .* y_u(:,i:n_y:n_yu)') * Ws (:,i:n_y:n_yw);
      QQ = ones(n_x,1)*Q(i,:);	# Resize Q
      J_xw = J_xw + (QQ .* y_x(:,i:n_y:n_yx)') * Ws (:,i:n_y:n_yw);
      QQ = ones(n_W,1)*Q(i,:);	# Resize Q
      J_ww = J_ww + (QQ .* Ws (:,i:n_y:n_yw)') * Ws (:,i:n_y:n_yw);
    endfor
  endif

  if P_weighting
    ## Terminal constraint 
    tau_last = tau(m_t);
    x_x_t = expm(A*tau_last);	# deriv. of x*(tau_2) wrt x(0)
    J_ux = J_ux + x_u_t'*P*x_x_t;
    J_uu = J_uu + x_u_t'*P*x_u_t;
    
    ## Terminal constraint - w bits
    ## NB SISO ONLY at the moment
    C = C(1,:);			# REMOVE ME
    
    ## deriv of x* wrt W
    if (cond(A)<1e10)		# Finite ss gain
      x_W_t =  (A\B)*inv(C*(A\B));
    else				# Infinite ss gain
      Y_0 = zeros(n_x,n_y); Y_0(1,:) = ones(1,n_y);
      x_W_t =  obsv(A,C)\Y_0;
    endif
    
    J_uw = J_uw + x_u_t'*P*x_W_t;
  endif
  
  ## Exit if badly conditioned
  cond_uu = cond(J_uu);
  if cond_uu>max_cond
    error(sprintf("J_uu is badly conditioned. Condition number = \
	10^%i",log10(cond_uu)));
  endif

  ## Compute the open-loop gains
  K_w = J_uu\J_uw;
  K_x = J_uu\J_ux;

  ## U*(tau) at tau=0 
  Us0 = ppp_ustar(A_u,n_u,0);		
  
  ## Compute the closed-loop gains
  k_x = Us0*K_x;
  k_w = Us0*K_w;

endfunction


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