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.
## t: 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);
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");
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
## 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;
## 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