% Script file  Beam_numpar.m
%% numpar file (Beam_numpar.m)
%% Generated by MTT at Thu Apr 22 07:00:08 BST 1999
% Global variable list
global ...
     area ...
     areamoment ...
     beamlength ...
     beamthickness ...
     beamwidth ...
     density ...
     ei ...
     n ...
     youngs ...
     dk ...
     dm ...
     dz ...
     rhoa ;
 %  -*-octave-*- Put Emacs into octave-mode
 %  Numerical parameter file (Beam_numpar.txt)
 %  Generated by MTT at Mon Apr 19 06:24:08 BST 1999

 %  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  %% Version control history
 %  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  %% $Id$
 %  %% $Log$
 %  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 %  Parameters
n =  7;
beamlength =  0.58;
beamwidth =  0.05;
beamthickness =  0.005;
youngs =  1e6;
density =  1e5;
area =  beamwidth*beamthickness;
areamoment =  (beamthickness*beamwidth^2)/12;

ei=  58.6957			; %  from Reza
rhoa=  0.7989			; %  from Reza
 
dz =  beamlength/n;  %  BernoulliEuler
dm =  rhoa*dz;  %  BernoulliEuler
dk =  ei/dz;  %  BernoulliEuler

% -*-octave-*- Put Emacs into octave-mode%
function [mtta,mttb,mttc,mttd] = Beam_sm();
% [mtta,mttb,mttc,mttd] = Beam_sm();
%System Beam, representation sm, language m;
%File Beam_sm.m;
%Generated by MTT on Thu Apr 22 07:02:48 BST 1999;
%
%====== Set up the global variables ======%
global ...
area ...
areamoment ...
beamlength ...
beamthickness ...
beamwidth ...
density ...
ei ...
n ...
youngs ...
dk ...
dm ...
dz ...
rhoa ;
%a matrix%
mtta = zeros(14,14);
mtta(1,2) = -dk/dz;
mtta(2,1) = 1.0/(dm*dz);
mtta(2,3) = -2.0/(dm*dz);
mtta(2,5) = 1.0/(dm*dz);
mtta(3,2) = (2.0*dk)/dz;
mtta(3,4) = -dk/dz;
mtta(4,3) = 1.0/(dm*dz);
mtta(4,5) = -2.0/(dm*dz);
mtta(4,7) = 1.0/(dm*dz);
mtta(5,2) = -dk/dz;
mtta(5,4) = (2.0*dk)/dz;
mtta(5,6) = -dk/dz;
mtta(6,5) = 1.0/(dm*dz);
mtta(6,7) = -2.0/(dm*dz);
mtta(6,9) = 1.0/(dm*dz);
mtta(7,4) = -dk/dz;
mtta(7,6) = (2.0*dk)/dz;
mtta(7,8) = -dk/dz;
mtta(8,7) = 1.0/(dm*dz);
mtta(8,9) = -2.0/(dm*dz);
mtta(8,11) = 1.0/(dm*dz);
mtta(9,6) = -dk/dz;
mtta(9,8) = (2.0*dk)/dz;
mtta(9,10) = -dk/dz;
mtta(10,9) = 1.0/(dm*dz);
mtta(10,11) = -2.0/(dm*dz);
mtta(10,13) = 1.0/(dm*dz);
mtta(11,8) = -dk/dz;
mtta(11,10) = (2.0*dk)/dz;
mtta(11,12) = -dk/dz;
mtta(12,11) = 1.0/(dm*dz);
mtta(12,13) = -2.0/(dm*dz);
mtta(13,10) = -dk/dz;
mtta(13,12) = (2.0*dk)/dz;
mtta(13,14) = -dk/dz;
mtta(14,13) = 1.0/(dm*dz);
%b matrix%
mttb = zeros(14,1);
mttb(11) = 1.0/dz;
mttb(13) = -2.0/dz;
%c matrix%
mttc = zeros(1,14);
mttc(1,1) = 1.0/dm;
%d matrix%
mttd = zeros(1,1);

function [A,B,C,D] = NMPsystem ()

  ## usage:  [A,B,C,D] = NMPsystem ()
  ##
  ## NMP system example (2-s)/(s-1)^3

  A = [3 -3 1
       1  0  0
       0  1  0];

  B = [1 
       0 
       0];

  C = [0 -0.5 1];

  D = 0;



endfunction

function [A,B,C,D] = TwoMassSpring (k,m_1,m_2)

  ## usage:  [A,B,C,D] = TwoMassSpring (k,m_1,m_2)
  ##
  ## Two mass-spring example from Middleton et al.  EE9908

  ###############################################################
  ## Version control history
  ###############################################################
  ## $Id$
  ## $Log$
  ## Revision 1.2  1999/05/18 22:31:26  peterg
  ## Fixed error in dim of D
  ##
  ## Revision 1.1  1999/05/18 22:28:56  peterg
  ## Initial revision
  ##
  ###############################################################


  A = [0    1 0 0
       -k/m_1 0 k/m_1 0
       0    0 0 1
       k/m_2 0 -k/m_2 0];
  B = [0
       1/m_1
       0 
       0];
  C = [1 0 0 0
       0 0 1 0];

  D = zeros(2,1);

endfunction

function [A,B,C,D] = airc
% System AIRC 
% This system is the aircraft example from the book:
% J.M Maciejowski: Multivariable Feedback Design  Addison-Wesley, 1989
% It has 5 states, 3 inputs and 3 outputs.

% P J Gawthrop Jan 1998

A = [    0         0    1.1320         0   -1.0000
         0   -0.0538   -0.1712         0    0.0705
         0         0         0    1.0000         0
         0    0.0485         0   -0.8556   -1.0130
         0   -0.2909         0    1.0532   -0.6859];

B = [    0         0         0
   -0.1200    1.0000         0
         0         0         0
    4.4190         0   -1.6650
    1.5750         0   -0.0732];

C = [1     0     0     0     0
     0     1     0     0     0
     0     0     1     0     0];

D = zeros(3,3);

function [A,B,C,D]=autm
% System AUTM
% This system is the automotive gas turbine example from the book:
% Y.S. Hung and A.G.J. Macfarlane: "Multivariable Feedback. A
% quasi-classical approach."  Springer 1982
% It has 12 states, 2 inputs and 2 outputs.

% P J Gawthrop Jan 1998

%A-matrix
A = zeros(12,12);
A(1,2) 	= 1;
A(2,1) 	= -0.202; A(2,2) = -1.150;
A(3,4) 	= 1;
A(4,5) 	= 1;
A(5,3) 	= -2.360; A(5,4) = -13.60; A(5,5) = -12.80;
A(6,7) 	= 1;
A(7,8) 	= 1;
A(8,6) 	= -1.620; A(8,7) = -9.400; A(8,8) = -9.150;
A(9,10) = 1;
A(10,11) = 1;
A(11,12) = 1;
A(12,9) = -188.0; A(12,10) = -111.6; A(12,11) = -116.4; A(12,12) = -20.8;

%B-matrix
B = zeros(12,2);
B(2,1)   =  1.0439; B(2,2)   = 4.1486;
B(5,1)   = -1.794;  B(5,2)   = 2.6775;
B(8,1)   =  1.0439; B(8,2)   = 4.1486;
B(12,1)  = -1.794;  B(12,2)  = 2.6775;

%C-matrix
C = zeros(2,12);
C(1,1)  = 0.2640; C(1,2)  = 0.8060; C(1,3) = -1.420; C(2,4) = -15.00; 
C(2,6)  = 4.9000; C(2,7)  = 2.1200; C(2,8) = 1.9500; C(2,9) = 9.3500;
C(2,10) = 25.800; C(2,11) = 7.1400;

%D-matrix
D = zeros(2,2);

function A = butterworth_matrix (n,p)

  ## usage:  A = butterworth (n,p)
  ##
  ## A-matrix for generating nth order Butterworth functions with parameter p

  ## Copyright (C) 2000 by Peter J. Gawthrop

  ## Butterworth poly
  pol = ppp_butter(n,p);
  
  ## Create A matrix (controller form)
  A = [-pol(2:n+1)
       eye(n-1) zeros(n-1,1)];
	
endfunction

function A = damped_matrix (frequency,damping)

  ## usage:  A = damped_matrix (frequency,damping)
  ##
  ## Gives an A matrix with eigenvalues with specified 
  ## frequencies and damping ratio

  N = length(frequency);

  if nargin<2
    damping = zeros(size(frequency));
  endif
  
  if length(damping) != N
    error("Frequency and damping vectors have different lengths");
  endif
  
  A = zeros(2*N,2*N);
  for i=1:N
    j = 2*(i-1)+1;
    A_i = [-2*damping(i)*frequency(i) -frequency(i)^2
	   1                           0];
    A(j:j+1,j:j+1) = A_i;
  endfor
  
endfunction

function A = laguerre_matrix (n,p)

  ## usage:  A = laguerre_matrix (n,p)
  ##
  ## A-matrix for generating nth order Laguerre functions with parameter p

  ## Copyright (C) 1999 by Peter J. Gawthrop

  ## Create A matrix
  A = diag(-p*ones(n,1));
  for i=1:n-1
    A = A + diag(-2*p*ones(n-i,1),-i);
  endfor

endfunction

function [A,v] = ppp_aug (A_1,A_2)

  ## usage:  [A,v] = ppp_aug (A_1,A_2)
  ##
  ## Augments square matrix A_1 with square matrix A_2 to create A=[A_1 0; A_2 0];
  ## and generates v, a compatible column vector with unit elements

  ## Copyright (C) 1999 by Peter J. Gawthrop


  [n_1,m_1] = size(A_1);
  if n_1 != m_1
    error("A_1 must be square");
  endif
  
  [n_2,m_2] = size(A_2);
  if n_2 != m_2
    error("A_2 must be square");
  endif

  A = [A_1            zeros(n_1,n_2)
       zeros(n_2,n_1) A_2];

  v = ones(n_1+n_2,1);

endfunction

function pol = ppp_butter (order,radius)

  ## usage:  pol = cgpc_butter (order,radius)
  ##
  ## Butterworth polynomial of given order and pole radius
  ## Copyright (C) 1999 by P.J. Gawthrop

  ## 	$Id$	

  theta = pi/(2*order);		# Angle with real axis

  even = (floor(order/2)==order/2);
  if even
    pol=1; N=order/2;
  else
    pol=[1 radius]; N=(order-1)/2;
  endif
  
  for i=1:N
    pol=conv(pol, [1 2*radius*cos(i*theta) radius^2]);
  endfor

endfunction

function [Ac,Bc,Cc,Dc] = ppp_closedloop (A,B,C,D,k_x,k_w,l_x,l_y)

  ## usage:  [Ac,Bc,Cc,Dc] = ppp_closedloop (A,B,C,K_x,K_w,K_y,L)
  ##
  ## 


  ## Closed loop input  is [w;v]. 
  ## Closed loop output is [y;u]. 
  ## w is reference signal
  ## v is input disturbance
  ## Inputs:
  ## 	A,B,C,D		MIMO linear system matrices
  ## 	k_x,k_w,k_y	Gain matrices: u = k_w*w - k_x*x
  ##	L		Observer gain matrix
  ## Outputs
  ## 	Ac,Bc,Cc,Dc	Closed-loop charecteristic polynomial	

  ## Copyright (C) 1999 by Peter J. Gawthrop

  ## System dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Create matrices describing closed-loop system
  Ac = [ A,  -B*k_x     
	l_x*C,  (A - l_x*C - B*k_x)];

  Bc = [B*k_w    B
	B*k_w    zeros(n_x,n_u)];

  Cc = [C               zeros(n_y,n_x)
	zeros(n_u,n_x)  -k_x          ];

  Dc = [zeros(n_y,n_y)     zeros(n_y,n_u)
	k_w              zeros(n_u,n_u)];

endfunction

function [J J_U] = ppp_cost (U,x,W,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww)

  ## usage: [J J_U] = ppp_cost (U,x,W,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww)
  ## Computes the PPP cost function given U,x and W
  ## 
  ## J_uu,J_ux,J_uw,J_xx,J_xw,J_ww cost derivatives from ppp_lin

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  J = U'*J_uu*U/2 + U'*(J_ux*x - J_uw*W) - x'*J_xw*W + x'*J_xx*x/2 + W'*J_ww*W'/2;
  J_U = J_uu*U + (J_ux*x - J_uw*W) ;

endfunction

function name = ppp_ex1 (ReturnName)

  ## usage:  ppp_ex1 ()
  ##
  ## PPP example - an unstable, nmp siso system
  ## 	$Id$	


  ## Example name
  name = "Linear unstable non-minimum phase third order system- Laguerre inputs";

  if nargin>0
    return
  endif
  

  ## System - unstable & NMP
  [A,B,C,D] = NMPsystem;
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Setpoint
  A_w = ppp_aug(0,[]);

  ## Controller

  ##Optimisation horizon
  t = [4.0:0.05:5];

  ## A_u
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w);

  ## Design and plot
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w,cond_uu] = ppp_lin_plot (A,B,C,D,A_u,A_w,t);


  ## Compute exact version
  poles = sort(eig(A_u));	# Desired poles - eigenvalues of A_u
  poles = poles(1:n_x);		# Loose the last one - due to setpoint 
  clp = poly(poles);		# Closed-loop cp
  kk = clp(2:n_x+1)+A(1,:);	# Corresponding gain
  A_c = A-B*kk;			# Closed-loop A
  K_X = ppp_open2closed (A_u,[A_c B*k_w; [0 0 0 0]],[kk -k_w]); # Exact
	
  ## Compute K_x using approx values
  A_c_a = A-B*k_x; 			
  K_X_comp = ppp_open2closed (A_u,[A_c_a B*k_w; [0 0 0 0]],[k_x -k_w]); # Computed Kx

  format bank
  log_cond_uu = log10(cond_uu)
  Exact_closed_loop_poles = poles'
  Approximate_closed_loop_poles = cl_poles
  Exact_k_x = kk
  Approximate_k_x = k_x
  Exact_K_X = K_X
  Approximate_K_X = [K_x -K_w]
  Computed_K_x = K_X_comp
  K_xw_error = Approximate_K_X-K_X
  format
endfunction

function name = ppp_ex10 (ReturnName)

  ## usage:  name = ppp_ex10 (ReturnName)
  ##
  ## PPP example - shows a standard multivariable system
  ##
 
  ## Example name
  name = "Remotely-piloted vehicle example:  system RPV from J.M Maciejowski: Multivariable Feedback Design";

  if nargin>0
    return
  endif
  
  ## System
  [A,B,C,D] = rpv;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = 1*[0.9:0.01:1];		# Time horizon
  A_w = 0;		# Setpoint
#   TC = 0.1*[1 1];		# Time constants for each input
#   A_u = [];
#   for tc=TC			# Input
#     A_u = [A_u;ppp_aug(laguerre_matrix(2,1/tc), 0)];
#   endfor
  A_u = ppp_aug(laguerre_matrix(2,5.0), A_w)
  Q = [1;1];		# Output weightings

  ## Design and plot
  W = [1;2]
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W);

endfunction

function [name,T,y,u,ys,us,J] = ppp_ex11 (ReturnName)

  ## usage:   [name,T,y,u,ys,us,T1,du,dus] = ppp_ex11 (ReturnName)
  ##
  ## PPP example

  ## 	$Id$	


  ## Example name
  name = "Input constraints +-1.5 on u* at tau=0,0.5,1,1.5,2";

  if nargin>0
    return
  endif
  
  ## System
  A = [-3 -3  -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5  1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Controller
  t = [6:0.02:7];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w); # Input functions

  Q = ones(n_y,1);;
  

  ## Constaints
  Gamma = [];
  gamma = [];

  ## Constaints - u
  Tau_u = [0:0.5:2];
  one = ones(size(Tau_u));
  limit = 1.5;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constraints - y
  Tau_y = [];			# No output constraints
  one = ones(size(Tau_y));
  limit = 1.5; 
  Min_y = -limit*one;
  Max_y =  limit*one;
  Order_y = 0*one;

  ## Simulation
  W=1;
  x_0 = zeros(3,1);

  ## Constrained - open-loop
  disp("Designing controller");
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t,Q); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);

  Gamma = Gamma_u;
  gamma = gamma_u;

  ## Constrained OL simulation
  disp("Computing constrained ol response");
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  disp("Computing unconstrained ol response");
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constained and unconstrained y*");
  xlabel("t");
  grid;
  plot(T,ys,T,ysu)

  ## Non-linear - closed-loop
    disp("Computing constrained closed-loop response");
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			  Tau_u,Min_u,Max_u,Order_u, \
			  Tau_y,Min_y,Max_y,Order_y,W,x_0);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T,y,T,u,T,ys,T,us);

  ## Compute derivatives.
  dt = t(2)-t(1);
  du = diff(u)/dt;
  dus = diff(us)/dt;
  T1 = T(1:length(T)-1);
  ##plot(T1,du,T1,dus);
endfunction

function [name,T,y,u,ys,us,J,T1,du,dus] = ppp_ex12 (ReturnName)

  ## usage:   [name,T,y,u,ys,us,T1,du,dus] = ppp_ex12 (ReturnName)
  ##
  ## PPP example - shows input derivative constraints  
  ## $Id$


  ## Example name
  name = "Input derivative constraints +-1 on u* at tau=0,0.5,1,1.5,2";

  if nargin>0
    return
  endif
  
  ## System
  A = [-3 -3  -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5  1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [4:0.02:5];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w); # Input functions
  Q = ones(n_y,1);;

  ## Constaints - du*/dtau
  Tau = [0:0.5:2];
  one = ones(size(Tau));
  limit = 1;
  Min = -limit*one;
  Max =  limit*one;
  Order = one;
  [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order);

  W=1;
  x_0 = zeros(3,1);

  ## Constrained - open-loop
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin (A,B,C,D,A_u,A_w,t,Q);
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar(A,B,C,D,x_0,A_u,U,T);

  ## Non-linear - closed-loop
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			   Tau,Min,Max,Order, \
			   [],[],[],[], W,x_0);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T,y,T,u,T,ys,T,us);

  ## Compute derivatives.
  dt = t(2)-t(1);
  du = diff(u)/dt;
  dus = diff(us)/dt;
  T1 = T(1:length(T)-1);
  ##plot(T1,du,T1,dus);
endfunction

function name = ppp_ex13 (ReturnName)

  ## usage:  ppp_ex13 ()
  ##
  ## PPP example: Sensitivity minimisation (incomplete)


  ## Example name
  name = "Sensitivity minimisation (incomplete)";

  if nargin>0
    return
  endif
  

  ## System - unstable
  A = [-3 -3 -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5 1
       0  1.0 0];
  D = [0;0];

  ## Setpoint
  A_w = [0;0]

  ## Controller
  t =[0:0.1:5];			# Optimisation horizon
  t1 =[0:0.1:1];
  t2 =[1.1:0.1:3.9];	
  t3 =[4:0.1:5];
  

  A_u = ppp_aug(laguerre_matrix(3,5.0), 0);
  q_s=1e3;
  Q = [exp(5*t)
       q_s*exp(-t)]
  size(Q)
  W = [1;0];

  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W)

endfunction

function [name,T,y,u,ys,us,ysu,usu,J] = ppp_ex14m (ReturnName)

  ## usage:   [name,T,y,u,ys,us,ysu,usu,J] = ppp_ex14 (ReturnName)
  ##
  ## PPP example - shows output constraints on nonlinear system
  ## 	$Id$	


  ## Example name
  name = "Output constraints -0.1 on y* at tau=0.1,0.5,1,2";

  if nargin>0
    if ReturnName
      return
    endif
  endif
  
  ## System
  A = [-3 -3  -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5  1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [4:0.02:5];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w); # Input functions
  Q = ones(n_y,1);;

  ## Constaints - u
  Tau_u = [];
  one = ones(size(Tau_u));
  limit = 3;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constraints - y
  Tau_y = [0.1 0.5 1 2]
  one = ones(size(Tau_y));
  Min_y =  -0.01*one; # Min_y(5) = 0.99;
  Max_y =  1e5*one;   # Max_y(5) = 1.01;
  Order_y = 0*one;

  ## Simulation
  W=1;
  x_0 = zeros(3,1);

  ## Constrained - open-loop
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t,Q); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);
  [Gamma_y,gamma_y] = ppp_output_constraint  (A,B,C,D,x_0,A_u,Tau_y,Min_y,Max_y,Order_y);

  Gamma = [Gamma_u; Gamma_y];
  gamma = [gamma_u; gamma_y];

  ## Constrained OL simulation
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constained and unconstrained y*");
  xlabel("t");
  grid;
  plot(T,ys,T,ysu)

  ## Non-linear - closed-loop
  movie = 1; 
  if movie
    hold on;
  endif
  
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			  Tau_u,Min_u,Max_u,Order_u, \
			  Tau_y,Min_y,Max_y,Order_y,W,x_0,movie);

  hold off;
#   title("y,y*,u and u*");
#   xlabel("t");
#   grid;
#   plot(T,y,T,u,T,ysu,T,usu);

  ## Compute derivatives.
  dt = t(2)-t(1);
  du = diff(u)/dt;
  dus = diff(us)/dt;
  T1 = T(1:length(T)-1);
  ##plot(T1,du,T1,dus);
endfunction

function  [name,T,y,u,ye,ue,J] = ppp_ex15 (ReturnName)

  ## usage:  ppp_ex15 ()
  ##
  ## PPP example - an unstable, nmp siso system
  ## 	$Id$	

  ## Example name
  name = "Linear unstable non-minimum phase third order system - intermittent control";

  if nargin>0
    return
  endif
  

  ## System - unstable
  A = [3 -3  1
       1  0  0
       0  1  0];
  B = [10
       0 
       0];
  C = [0 -0.5 1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Setpoint
  A_w = ppp_aug(0,[]);

  ## Controller
  t =[4.0:0.01:5.0];		# Optimisation horizon
  dt = t(2)-t(1);
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w);
  Q = 1;			# Weight

  ##Simulate
  W = 1;			# Setpoint
  x_0 = zeros(n_x,1);		# Initial state


  ## Closed-loop intermittent solution
  Delta_ol = 0.5		# Intermittent time

  disp("Intermittent control simulation");
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			  [],[],[],[], \
			  [],[],[],[],W,x_0,Delta_ol);

  ## Exact closed-loop
  disp("Exact closed-loop");
  [k_x,k_w] = ppp_lin (A,B,C,D,A_u,A_w,t,Q);
  [ye,Xe] = ppp_sm2sr(A-B*k_x, B, C, D, T, k_w*W, x_0); # Compute Closed-loop control
  ue = k_w*ones(size(T))*W - k_x*Xe';


  title("y and u, exact and intermittent");
  xlabel("t");
  grid;
  plot(T,y,T,u,T,ye,T,ue);

endfunction

function [name,T,y,u,ys,us,J] = ppp_ex16 (ReturnName)

  ## usage:   [name,T,y,u,ys,us,T1,du,dus] = ppp_ex16 (ReturnName)
  ##
  ## PPP example

  ## 	$Id$	


  ## Example name
  name = "Input constraints +-1.5 on u* at tau=0,0.1,0.2..,2.0 - intermittent control";

  if nargin>0
    return
  endif
  
  ## System
  A = [-3 -3  -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5  1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Controller
  t = [5:0.01:6];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w); # Input functions
  A_u = ppp_aug(laguerre_matrix(1,0.5), A_u); # Add some extra slow modes
  Q = ones(n_y,1);;

  ## Constaints
  Gamma = [];
  gamma = [];

  ## Constaints - u
  Tau_u = [0:0.1:2]; 
  one = ones(size(Tau_u));
  limit = 1.5;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constaints - y
  Tau_y = [];
  one = ones(size(Tau_y));
  limit = 1.5; 
  Min_y = -limit*one;
  Max_y =  limit*one;
  Order_y = 0*one;

  ## Simulation
  W=1;
  x_0 = zeros(3,1);

  ## Constrained - open-loop
  disp("Control design");
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t,Q); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);

  Gamma = Gamma_u;
  gamma = gamma_u;

  disp("Open-loop simulations");
  ## Constrained OL simulation
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constrained and unconstrained y* and u*");
  xlabel("t");
  grid;
  axis([0 6 -1 2]);
  plot(T,ys,T,ysu,T,us,T,usu)
  axis;

  ## Non-linear - closed-loop
  disp("Closed-loop simulation");
  Delta_ol = 0.1;
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			  Tau_u,Min_u,Max_u,Order_u, \
			  Tau_y,Min_y,Max_y,Order_y,W,x_0,Delta_ol);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T,y,T,u,T,ys,T,us);
endfunction

function [name,T,y,u,ys,us,ysu,usu,J] = ppp_ex17 (ReturnName)

  ## usage:   [name,T,y,u,ys,us,ysu,usu,J] = ppp_ex17 (ReturnName)
  ##
  ## PPP example - shows output constraints on nonlinear system
  ## 	$Id$	


  ## Example name
  name = "Output constraints -0.1 on y* at tau=0.1,0.5,1,2 - intermittent control";

  if nargin>0
    if ReturnName
      return
    endif
  endif
  
  ## System
  A = [-3 -3  -1
       1  0  0
       0  1  0];
  B = [1 
       0 
       0];
  C = [0 -0.5  1];
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [9:0.02:10];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(3,2.0), A_w); # Input functions
  Q = ones(n_y,1);;

  ## Constraints - u
  Tau_u = [];
  one = ones(size(Tau_u));
  limit = 3;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constraints - y
  Tau_y = [0.1 0.5 1 2]
  one = ones(size(Tau_y));
  Min_y =  -0.01*one; # Min_y(5) = 0.99;
  Max_y =  1e5*one;   # Max_y(5) = 1.01;
  Order_y = 0*one;

  ## Simulation
  W=1;
  x_0 = zeros(3,1);

  ## Constrained - open-loop
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t,Q); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);
  [Gamma_y,gamma_y] = ppp_output_constraint  (A,B,C,D,x_0,A_u,Tau_y,Min_y,Max_y,Order_y);

  Gamma = [Gamma_u; Gamma_y];
  gamma = [gamma_u; gamma_y];

  ## Constrained OL simulation
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];

  ## OL solution
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constained and unconstrained y*");
  xlabel("t");
  grid;
  plot(T,ys,T,ysu)

  ## Non-linear - closed-loop
  delta_ol = 0.1;
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			  Tau_u,Min_u,Max_u,Order_u, \
			  Tau_y,Min_y,Max_y,Order_y,W,x_0,delta_ol);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T,y,T,u,T,ysu,T,usu);

endfunction

function name = ppp_ex18 (ReturnName)

  ## usage:  ppp_ex18 ()
  ##
  ## PPP example - an unstable, nmp siso system
  ## 	$Id$	


  ## Example name
  name = "First order with redundant inputs";

  if nargin>0
    return
  endif
  
  ## System 
  A = 1
  B = 1
  C = 1
  D = 0;

  ## Setpoint
  A_w = ppp_aug(0,[]);

  ## Controller
  ##Optimisation horizon
  t =[2:0.1:3];

  ## A_u
  A_u = diag([0  -2 -4 -6])

  [ol_poles,cl_poles] = ppp_lin_plot (A,B,C,D,A_u,A_w,t)

  
endfunction

function [name,T,y,u,ys,us,J] = ppp_ex19 (ReturnName,n_extra,T_extra)

  ## usage:   [name,T,y,u,ys,us,T1,du,dus] = ppp_ex19 (ReturnName)
  ##
  ## PPP example

  ## 	$Id$	


  ## Example name
  name = "Input constraints with redundant U*";

  if (nargin>0)&&(ReturnName==1)
    return
  endif


  if nargin<2
    n_extra = 3
  endif
  
  if nargin<3
    T_extra = 2.0
  endif
  

  ## System
  A = 1
  B = 1
  C = 1
  D = 0;
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Controller
  t = [2:0.01:3];		# Time horizon
  A_w = 0;
  A_u = diag([0  -6]);
  A_u = ppp_aug(A_u,laguerre_matrix(n_extra,1/T_extra))
  Q = 1;
  ## Constraints
  Gamma = [];
  gamma = [];

  ## Constraints - u
  Tau_u = [0 0.1 0.5 1 1.5 2];
  Tau_u = 0;
  one = ones(size(Tau_u));
  limit = 1.5;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constraints - y
  Tau_y = [];
  one = ones(size(Tau_y));
  limit = 1.5; 
  Min_y = -limit*one;
  Max_y =  limit*one;
  Order_y = 0*one;

  ## Simulation
  W=1;
  x_0 = zeros(n_x,1);

  ## Constrained - open-loop
  disp("Control design");
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);

  Gamma = Gamma_u;
  gamma = gamma_u;

  disp("Open-loop simulations");
  ## Constrained OL simulation
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constrained and unconstrained y*");
  xlabel("t");
  grid;
  plot(T,ys,T,ysu)

  ## Non-linear - closed-loop
  disp("Closed-loop simulation");
  [T1,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			   Tau_u,Min_u,Max_u,Order_u, \
			   Tau_y,Min_y,Max_y,Order_y,W,x_0);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T1,y,T,ys,T1,u,T,us);
endfunction

function name = ppp_ex2 (Return_Name)

  ## usage:  Name = ppp_ex2 (Return_Name)
  ##
  ## PPP example: Effect of slow desired closed-loop
  ## 	$Id$	


  ## Example name
  name = "Effect of slow desired closed-loop: closed-loop is same as open loop";

  if nargin>0
    return
  endif
  
  ## System
  A = -1;			# Fast - time constant = 1
  B = 0.5;			# Gain is 1/2
  C = 1;
  D = 0;

  ## Controller
  t =[9:0.1:10];		# Optimisation horizon

  A_u = [-0.1 0			# Slow - time constant = 10
         1 0];

  A_w = 0;			# Constant set point

  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w] = ppp_lin_plot(A,B,C,D,A_u,A_w,t)
endfunction

function [name,T,y,u,ys,us,ysu,usu] = ppp_ex20 (ReturnName)

  ## usage:  ppp_ex20 ()
  ##
  ## PPP example - 2nd-order 2i2o system
  ## 	$Id$	


  ## Example name
  name = "2nd-order 2i2o system with 1st-order basis"

  if nargin>0
    return
  endif

  ## System  
  A = [-2 -1
       1   0];
  B = [[1;0], [1;2]];
  C = [ [0,1]; [2,1]];
  D = zeros(2,2);

#  sys = ss2sys(A,B,C,D);
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

#   ## Display it
#   for j = 1:n_u
#     for i = 1:n_y
#       sysout(sysprune(sys,i,j),"tf")
#       step(sysprune(sys,i,j),1,5);
#     endfor
#   endfor
  
  ## Setpoint
  A_w = 0;

  ## Controller

  ##Optimisation horizon
  t =[4:0.01:5];

  ## A_u
  pole = [3];
  poles = 1;
  A_u = ppp_aug(butterworth_matrix(poles,pole),0);
  Q = ones(n_y,1);;

  ## Setpoints
  W = [1:n_y]';

  ## Design and plot
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w,cond_uu] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W);

  format bank
  cl_poles

  A_c = A-B*k_x;			# Closed-loop A
  A_cw = [A_c B*k_w*W
          zeros(1,n_x+1)]

  log_cond_uu = log10(cond_uu)
				#  K_xwe = ppp_open2closed(A_u,A_cwe,[k_xe -k_we*W]); # Exact Kx
#  K_xwc = ppp_open2closed(A_u,A_cw,[k_x -k_w*W]); # Computed Kx
				#  Exact_K_xw = K_xwe
  PPP_K_xw = [K_x -K_w*W] 
#  Comp_K_xw = K_xwc

#  Error = Approx_K_xw - Comp_K_xw

endfunction

# function name = ppp_ex20 (ReturnName)

#   ## usage:  name = ppp_ex20 (ReturnName)
#   ##
#   ## PPP example -- a standard multivariable example
#   ## 	$Id$	



#   ## Example name
#   name = "Turbogenerator example:  system TGEN from J.M Maciejowski: Multivariable Feedback Design";

#   if nargin>0
#     return
#   endif
  
  ## System
  [A,B,C,D] = airc;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [9:0.1:10];		# Time horizon
  A_w = zeros(n_y,1);		# Setpoint
  TC = 2*[1 1];		# Time constants for each input
 #  A_u = [];
#   for tc=TC			# Input
#     A_u = [A_u;ppp_aug(laguerre_matrix(3,1/tc), 0)];
#   endfor
 A_u =  ppp_aug(laguerre_matrix(5,1.0), 0);
 Q = [1;1];		# Output weightings

  ## Constraints
  Gamma = [];
  gamma = [];

  ## Constraints - u
  Tau_u = [0 0.1 0.5 1 1.5 2];
  Tau_u = 0;
  one = ones(size(Tau_u));
  limit = 1.5;
  Min_u = -limit*one;
  Max_u =  limit*one;
  Order_u = 0*one;

  ## Constraints - y
  Tau_y = [];
  one = ones(size(Tau_y));
  limit = 1.5; 
  Min_y = -limit*one;
  Max_y =  limit*one;
  Order_y = 0*one;

  ## Simulation
  W=[1;2;3];
  x_0 = zeros(n_x,1);

  ## Constrained - open-loop
  disp("Control design");
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw] = ppp_lin  (A,B,C,D,A_u,A_w,t); # Unconstrained design
  [Gamma_u,gamma_u] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u);

  Gamma = Gamma_u;
  gamma = gamma_u;

  disp("Open-loop simulations");
  ## Constrained OL simulation
  [u,U] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma);
  T = [0:t(2)-t(1):t(length(t))];
  [ys,us] = ppp_ystar (A,B,C,D,x_0,A_u,U,T);

  ## Unconstrained OL simulation
  [uu,Uu] = ppp_qp (x_0,W,J_uu,J_ux,J_uw,Us0,[],[]);
  [ysu,usu] = ppp_ystar (A,B,C,D,x_0,A_u,Uu,T);

  title("Constrained and unconstrained y*");
  xlabel("t");
  grid;
  plot(T,ys,T,ysu)

  ## Non-linear - closed-loop
  disp("Closed-loop simulation");
  [T1,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, \
			   Tau_u,Min_u,Max_u,Order_u, \
			   Tau_y,Min_y,Max_y,Order_y,W,x_0);

  title("y,y*,u and u*");
  xlabel("t");
  grid;
  plot(T1,y,T,ys,T1,u,T,us);


#endfunction

function [name,T,y,u,ys,us,ysu,usu] = ppp_ex21 (ReturnName)

  ## usage:  ppp_ex21 ()
  ##
  ## PPP example - 2nd-order 2i2o system system with anomalous behaviour"
  ## 	$Id$	


  ## Example name
  name = "2nd-order 2i2o system with anomalous behaviour"

  if nargin>0
    return
  endif

  ## System  
  A = [-2 -1
       1   0];
  B = [[1;0], [1;2]];
  C = [ [0,1]; [2,1]];
  D = zeros(2,2);

  sys = ss2sys(A,B,C,D);
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  ## Display it
  for j = 1:n_u
    for i = 1:n_y
      sysout(sysprune(sys,i,j),"tf")
      step(sysprune(sys,i,j),1,5);
    endfor
  endfor
  
  ## Setpoint
  A_w = 0;

  ## Controller

  ##Optimisation horizon
  t =[4:0.1:5];

  ## A_u
  pole = [4];
				#A_u = ppp_aug(laguerre_matrix(2,pole),0);
				#A_u = ppp_aug(diag([-3,-4]),0);
  A_u = ppp_aug(butterworth_matrix(2,pole),0);
  Q = ones(n_y,1);;

  ## Setpoints
  W = [1:n_y]';

  ## Initial condition
  x_0 = [0;0.5];

  ## Design and plot
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w,cond_uu] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W,x_0);

  format short
  cl_poles

  A_c = A-B*k_x;			# Closed-loop A
  A_cw = [A_c B*k_w*W
          zeros(1,n_x+1)]

  log_cond_uu = log10(cond_uu)
				#  K_xwe = ppp_open2closed(A_u,A_cwe,[k_xe -k_we*W]); # Exact Kx
				#AA_u = ppp_inflate([A_u;A_u]);
  K_xwc = ppp_open2closed(A_u,A_cw,[k_x -k_w*W]) # Computed Kx
				#  Exact_K_xw = K_xwe
  Approx_K_xw = [K_x -K_w*W] 
  format

endfunction

function name = ppp_ex3 (Return_Name)

  ## usage:  Name = ppp_ex3 (Return_Name)
  ##
  ## PPP example: Uncoupled 5x5 system
  ##  	$Id$	




  ## Example name
  name = "Uncoupled NxN system - n first order systems";

  if nargin>0
    return
  endif
  
  ## System - N uncoupled integrators
  N = 3
  A = -0.0*eye(N);
  B = eye(N);
  C = eye(N);
  D = zeros(N,N);

  t =[4:0.1:5];			# Optimisation horizon
  ## Create composite matrices
  A_u = [];			# Initialise
  A_w = [];			# Initialise

  for i=1:N			
    ## Setpoint - constant
    a_w = ppp_aug(0,[]);
    A_w = [A_w;a_w];

    ## Controller
    a_u = ppp_aug(-i,a_w);
    A_u = [A_u; a_u];
 endfor
  
  A_u = [-diag([1:N])]

  Q = ones(N,1);		# Equal output weightings
  W = ones(N,1);		# Setpoints are all unity

  ## Design and simulate
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u] = ppp_lin(A,B,C,D,A_u,A_w,t);
  # [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = \
  #     ppp_lin_plot(A,B,C,D,A_u,A_w,t,Q,W);

  Approximate_K_x = K_x#(1:2:2*N,:)
  A_c = A-B*k_x;
  Closed_Loop_Poles = eig(A-B*k_x)
  ## Now try out the open/closed loop theory
#   A_u = -diag(1:N);		# Full A_u matrix
#   A_c = -diag(1:N);		# Ideal closed-loop
#   k_x = diag(1:N);		# Ideal feedback
  KK = ppp_open2closed (ppp_inflate(A_u),A_c,k_x);
  Exact_K_x_tilde = KK
  

endfunction

function name = ppp_ex4 (ReturnName)

  ## usage:  ppp_ex4 ()
  ##
  ## PPP example -- a 1i2o system with performance limitations
  ## 	$Id$	



  ## Example name
  name = "Resonant system (1i2o): illustrates performance limitations with 2 different time-constants";

  if nargin>0
    return
  endif
  

  ##  Mass- sping damper from Middleton et al EE9908

  ## Set parameters to unity
  m_1 = 1;		
  m_2 = 1;
  k = 1;

  ## System
  [A,B,C,D] = TwoMassSpring (k,m_1,m_2);

  for TC = [0.4 1]
    disp(sprintf("\nClosed-loop time constant = %1.1f\n",TC));
    ## Controller
    A_w = zeros(2,1);	# Setpoint: Unit W* for each output
    t =[11:0.1:12];			# Optimisation horizon
    [A_u] = ppp_aug(laguerre_matrix(4,1/TC), 0);	# U*

    Q = [1;0];

    ## Design and plot
    [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q)
    hold on;
  endfor

  hold off;
endfunction

function name = ppp_ex5 (ReturnName)

  ## usage:  ppp_ex5 (ReturnName)
  ##
  ## PPP example -- a 28-state vibrating beam system
  ## 	$Id$	


  ## Example name
  name = "Vibrating beam: 14 state regulation problem with 7 beam velocities as output";

  if nargin>0
    return
  endif
  
  
  ## System - beam
  Beam_numpar;
  [A,B,C,D]=Beam_sm;
  
  ## Redo C and D to reveal ALL velocities
  c = C(1);
  C = zeros(7,14);
  for i = 1:7
    C(i,2*i-1) = c;
  endfor
  D = zeros(7,1);

  e = eig(A);			# Eigenvalues
  N = length(e);
  frequencies = sort(imag(e));
  frequencies = frequencies(N/2+1:N); # Modal frequencies

  ## Controller
  ## Controller design parameters
  t = [0.4:0.01:0.5];		# Optimisation horizon

  Q = ones(7,1); 

  ## Specify input basis functions 
  ##  - damped sinusoids with same frequencies as beam
  damping_ratio = 0.2;		# Damping ratio of inputs
  A_u = damped_matrix(frequencies,0.2*ones(size(frequencies)));
  u_0 = ones(14,1);		# Initial conditions

  A_w = zeros(7,1);		# Setpoint
  W =  zeros(7,1);		# Zero setpoint

  ## Set up an "typical" initial condition
  x_0 = zeros(14,1);
  x_0(2:2:14) = ones(1,7);	# Set initial twist to 1.

  ## Simulation
  [ol_poles,cl_poles] =  ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W,x_0);

  

endfunction

function [name,T,y,u,J] = ppp_ex6 (ReturnName)

  ## usage:  [name,T,y,u,J] = ppp_ex6 (ReturnName)
  ##
  ## PPP example -- PPP for redundant actuation
  ## 	$Id$	


  ## Example name
  name = "Two input-one output system with input constraints";

  if nargin>0
    return
  endif
  
  ## System
  A = 0;
  B = [0.5 1];
  C = 1;
  D = [0 0];
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [4:0.1:5];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = [-2;-0.5];		# Input
  Q = 1;			# Output weight

  ## Constrain  input 1 at time tau=0
  Tau = 0;
  Max = 1;
  Min = -Max;
  Order = 0;
  i_u = 1;
  
  ## Simulation
  W=1;
  x_0 = 0;

  ## Linear
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t);
  
  ## Non-linear
  movie = 0;
  [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, Tau,Min,Max,Order, \
	      [],[],[],[], W,x_0);
  title("y,u_1,u_2");
  xlabel("t");
  grid;
  plot(T,y,T,u);

endfunction

function name = ppp_ex7 (ReturnName)

  ## usage:  name = ppp_ex7 (ReturnName)
  ##
  ## PPP example -- standard multivariable system
  ## 	$Id$	


  ## Example name
  name = "Aircraft example:  system AIRC from J.M Maciejowski: Multivariable Feedback Design";

  if nargin>0
    return
  endif
  
  ## System
  [A,B,C,D] = airc;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [4:0.01:5];		# Time horizon
  A_w = 0;		# Setpoint (same for each input)
  #A_u = ppp_aug(laguerre_matrix(5,1), 0) # Same for each input 
  A_u = laguerre_matrix(5,1); # Same for each input 
  Q = ones(3,1);		# Output weightings
  ## Design and plot
  W = [1;2;3]
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W);
   cl_poles

  ## Try open-closed theory but using computed values:
  A_c = A - B*k_x; eig(A_c)
  K_x
  KK = ppp_open2closed (A_u,A_c,k_x)

  100*((KK-K_x)./KK)
endfunction

function name = ppp_ex8 (ReturnName)

  ## usage:  name = ppp_ex8 (ReturnName)
  ##
  ## PPP example - standard multivariable example
  ## 	$Id$	

  ## Example name
  name = "Automotive gas turbine example:  system AUTM from J.M Maciejowski: Multivariable Feedback Design";

  if nargin>0
    return
  endif
  
  ## System
  [A,B,C,D] = autm;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [4:0.1:5];		# Time horizon
  A_w = 0;			# Setpoint
  A_u = ppp_aug(laguerre_matrix(4,2.0), 0) # Input
  Q = [1;1e3];		# Output weightings

  ## Design and plot
  W = [1;2]
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W);

endfunction

function name = ppp_ex9 (ReturnName)

  ## usage:  name = ppp_ex9 (ReturnName)
  ##
  ## PPP example -- a standard multivariable example
  ## 	$Id$	



  ## Example name
  name = "Turbogenerator example:  system TGEN from J.M Maciejowski: Multivariable Feedback Design";

  if nargin>0
    return
  endif
  
  ## System
  [A,B,C,D] = tgen;
  [n_x,n_u,n_y] = abcddim(A,B,C,D)

  ## Controller
  t = [1.0:0.01:2.0];		# Time horizon
#   A_w = zeros(n_y,1);		# Setpoint
#   TC = 2*[1 1];		# Time constants for each input
#   A_u = [];
#   for tc=TC			# Input
#     A_u = [A_u;ppp_aug(laguerre_matrix(3,1/tc), 0)];
#   endfor
  A_w = 0;
  A_u = ppp_aug(ppp_aug(laguerre_matrix(2,1.0),laguerre_matrix(2,2.0)), A_w)
  Q = [1;1];		# Output weightings

  ## Design and plot
  W = [1;2]
  [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W);

#   ol_poles, cl_poles
#   k_x,k_w
#   K_X = [K_x -K_w]
#   A_c = A - B*k_x;
#   K_X_comp = ppp_open2closed (A_u,[A_c B*k_w*W; zeros(1,n_x+1)],[k_x -k_w*W])
endfunction

function ppp_examples ()

  ## usage:  ppp_examples ()
  ##
  ## Various menu-driven PPP examples


  str="menu(""Predictive Pole-Placement (PPP) examples"",""Exit"",""All examples"; # Menu string

  used = 2;
  option=used;  

  while option>1

    exists=1; 
    i_example=1;		# Example counter
    while exists
      name=sprintf("ppp_ex%i",i_example);
      exists=(exist(name)==2);
      if exists
	title = eval(sprintf("%s(1);", name)); 
	str = sprintf("%s"",""%s",str,title);
	i_example++;
      endif
    endwhile
    n_examples = i_example-1;

    str = sprintf("%s"" );\n",str);

    option=eval(str);		# Menu - ask user

    if option>1			# Do something - else return
      if option==2		# All examples
	Examples=1:n_examples;
      else			# Just the chosen examples
	Examples = option-used;
      endif
      for example=Examples	# Do the chosen examples
	eval(sprintf("Title = ppp_ex%i(1);",example));
	disp(sprintf("Evaluating example ppp_ex%i:\n\t %s\n", example, Title));
	eval(sprintf("ppp_ex%i;",example));
      endfor
    endif
    
    
  endwhile

endfunction

function A_i = ppp_extract (A_u,input)

  ## usage:  A_i = ppp_extract (A_u)
  ##
  ## Extracts the ith A_u matrix.

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  [n,m] = size(A_u);		# Size of composite A_u matrix
  square = (n==m);		# Its a square matrix so same U* on each input
  if square
    A_i = A_u;			# All a_u the same
  else
    N = m;			# Number of U* functions per input  
    n_u = n/m;
    if floor(n_u) != n_u	# Check that n_u is an integer
      error("A_u must be square or be a column of square matrices");
    endif
    
    if input>n_u
      error("Input index too large");
    endif
    
    ## Extract the ith matrix
    start = (input-1)*N;
    range=(start+1:start+N);
    A_i = A_u(range,:);
  endif

endfunction

function A_m = ppp_inflate (A_v)

  ## usage:  A_m = ppp_inflate (A_v)
  ##
  ## Creates the square matrix A_m with the matrix elements of the column
  ## vector of square matrices A_v.

  ## Copyright (C) 2001 by Peter J. Gawthrop

  [N,M] = size(A_v);

  if N<M
    error("A_v must have at least as many rows as columns");
  endif
  
  n = N/M;			# Number of matrix elements in A_v

  if round(n)<>n
    error("A_v must be a column vector of square matrices");
  endif
  
  A_m = [];
  for i = 1:n
    A_m = ppp_aug(A_m,ppp_extract(A_v,i));
  endfor

endfunction

function [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order,i_u,n_u)

  ## usage:  [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order)
  ##
  ## Derives the input constraint matrices Gamma and gamma
  ## For Constraints Min and max at times Tau 
  ## Order=0 - input constraints
  ## Order=1 - input derivative constraints
  ## etc
  ## i_u: Integer index of the input to be constrained
  ## n_u: Number of inputs
  ## NOTE You can stack up Gamma and gamma matrices for create multi-input constraints.

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	


  ## Sizes
  [n_U,m_U] = size(A_u);	# Number of basis functions
  [n,N_t] = size(Tau);		# Number of constraint times
  
  ## Defaults
  if nargin<5
    Order = zeros(1,N_t);
  endif

  if nargin<6
    i_u = 1;
    n_u = 1;
  endif
  
  if N_t==0			# Nothing to be done
    Gamma = [];
    gamma = [];
    return
  endif
  
  if n != 1
    error("Tau must be a row vector");
  endif
  
  n = length(Min);
  m = length(Max);
  o = length(Order);

  if (n != N_t)||(m != N_t)||(o != N_t)
    error("Tau, Min and max must be the same length");
  endif
  
  ## Extract the A_i matrix for this input
  A_i = ppp_extract(A_u,i_u);

  ## Create the constraints in the form: Gamma*U < gamma
  Gamma = [];
  gamma = [];
  one = ones(m_U,1);
  i=0;

  zero_l = zeros(1,(i_u-1)*m_U); # Pad left-hand
  zero_r = zeros(1,(n_u-i_u)*m_U); # Pad right-hand
  for tau = Tau			# Stack constraints for each tau
    i++;
    Gamma_tau = ( A_i^Order(i) * expm(A_i*tau) * one )';
    Gamma_tau = [ zero_l Gamma_tau zero_r ]; # Only for i_uth input
    Gamma = [Gamma;[-1;1]*Gamma_tau]; # One row for each of min and max

    gamma_tau = [-Min(i);Max(i)];
    gamma = [gamma;gamma_tau];
  endfor

endfunction

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,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,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
  if nargin<8
    Q = ones(n_y,1);
  endif

  ## Default order
  if nargin<9
    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

  [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
  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] = 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.
  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/m_t;			# Scale by T/dt = number of points
  ## 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

  ## 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

  ## 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

  ## 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

function U = ppp_lin_con_sol (x,w,J_uu,J_ux,J_uw)

  ## usage:  U = ppp_lin_con_sol (x,w,J_uu,J_ux,J_uw)
  ##
  ## 

  ## Pass info to the solnp algorithm
  global ppp_J_uu, ppp_J_ux, ppp_J_uw

endfunction

function [l_x,l_y,L_x,L_y,J_uu,J_ux,J_uw,Us0] = ppp_lin_obs (A,B,C,D,A_u,A_y,t,Q)

  ## usage:  [l_x,l_y,L_x,L_y,J_uu,J_ux,J_uw,Us0] = ppp_lin_obs  (A,B,C,D,A_u,A_y,t,Q)
  ##
  ## 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.
  ## A_y: composite system matrix for W* generation 
  ##      one square matrix (A_yi) row for each system output
  ##      each A_yi 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)

  ## OUTPUTS:
  ## l_x: State feedback gain
  ## l_y: setpoint gain
  ## ie u(t) = l_y w(t) - l_x x(t)
  ## L_x, L_y: open loop gains
  ## J_uu, J_ux, J_uw: cost derivatives
  ## Us0: Value of U* at tau=0

  ## Check some dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);
  if (n_x==-1)
    return
  endif

  ## Default Q
  if nargin<8
    Q = ones(n_y,1);
  endif
  

#   B_x = eye(n_x);		# Pseudo B
#   D_x = zeros(n_y,n_x);		# Pseudo D
  [l_x,l_y,L_x,L_y,J_uu,J_ux,J_uw,Us0] = ppp_lin(A',C',B',D',A_u',A_y',t,Q);
  
  l_x = l_x';
  l_y = l_y';
  L_x = L_x';
  L_y = L_y';
endfunction

function [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w,cond_uu] =  ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W,x_0)

  ## usage:   [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,t,Q,W,x_0)
  ##
  ## Linear PPP (Predictive pole-placement) computation with plotting
  ## 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.
  ## 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)
  ## W: Constant setpoint vector (one element per output)
  ## x_0: Initial state
  ## OUTPUTS:
  ## Various poles 'n zeros
  ## 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
  ## J_uu, J_ux, J_uw: cost derivatives

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  ## Some dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);
  [n_U,m_U]=size(A_u);
  square = (n_U==m_U);		# Its a square matrix so same U* on each input
  [n_W,m_W]=size(A_w);
  if n_W==m_W			# A_w square
    n_W = n_W*n_y;		# Total W functions
  endif
  

  [n_t,m_t] = size(t);

  ## Default Q
  if nargin<8
    Q = ones(n_y,1);
  endif

  ## Default W
  if nargin<9
    W = ones(n_W,1)
  endif

  ## Default x_0
  if nargin<10
    x_0 = zeros(n_x,1);
  endif

  ## Check some dimensions
  [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

  [n,m] = size(W);
  if ((m != 1) || (n != n_W))
    error("W must be a column vector with one element per system output");
  endif

  [n,m] = size(x_0);
  if ((m != 1) || (n != n_x))
    error("x_0 must be a column vector with one element per system state");
  endif

  ## Simulate
  [y,ystar,t,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_sim(A,B,C,D,A_u,A_w,t,Q,W,x_0);

  ## Plot
  xlabel("Time"); title("y* and y"); grid;
  plot(t,ystar,t,y);

  ## Compute some pole/zero info
  cl_poles = eig(A-B*k_x)';
  ol_poles = eig(A)';

  if nargout>3
    ol_zeros = tzero(A,B,C,D)';
    cl_zeros = tzero(A-B*k_x,B,C,D)';
  endif

endfunction

function [y,ystar,t,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_sim(A,B,C,D,A_u,A_w,tau,Q,W,x_0)

  ## usage:   [ol_poles,cl_poles,ol_zeros,cl_zeros,k_x,k_w,K_x,K_w] = ppp_lin_plot (A,B,C,D,A_u,A_w,tau,Q,W,x_0)
  ##
  ## Linear PPP (Predictive pole-placement) computation with plotting
  ## 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.
  ## 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)
  ## W: Constant setpoint vector (one element per output)
  ## x_0: Initial state
  ## OUTPUTS:
  ## y : closed-loop output
  ## ystar : open-loop moving-horizon output
  ## t : time axis

  ## Copyright (C) 2001 by Peter J. Gawthrop
  ## 	$id: ppp_lin_plot.m,v 1.13 2001/01/26 16:03:13 peterg Exp $	

  ## Some dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);
  [n_U,m_U]=size(A_u);
  square = (n_U==m_U);		# Its a square matrix so same U* on each input
  [n_W,m_W]=size(A_w);
  if n_W==m_W			# A_w square
    n_W = n_W*n_y;		# Total W functions
  endif
  

  [n_tau,m_tau] = size(tau);

  ## Default Q
  if nargin<8
    Q = ones(n_y,1);
  endif

  ## Default W
  if nargin<9
    W = ones(n_W,1)
  endif

  ## Default x_0
  if nargin<10
    x_0 = zeros(n_x,1);
  endif

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

  [n,m] = size(W);
  if ((m != 1) || (n != n_W))
    error("W must be a column vector with one element per system output");
  endif

  [n,m] = size(x_0);
  if ((m != 1) || (n != n_x))
    error("x_0 must be a column vector with one element per system state");
  endif

  ## Control design
  disp("Designing controller");
  [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);

  ## Set up simulation times
  dtau = tau(2) -tau(1);		# Time increment
  t =  0:dtau:tau(length(tau));	# Time starting at zero

  ## Compute the OL step response
  disp("Computing OL response");
  U = K_w*W - K_x*x_0;
  ystar = ppp_ystar (A,B,C,D,x_0,A_u,U,t);

  ## Compute the CL step response
  disp("Computing CL response");
  y = ppp_sm2sr(A-B*k_x, B, C, D, t, k_w*W, x_0); # Compute Closed-loop control


endfunction

function [U, U_all, Error, Y] = ppp_nlin (system_name,x_0,us,t,par_0,free,w,extras)

  ## usage:  U = ppp_nlin (system_name,x_0,u,t,par_0,free,w,weight)
  ##
  ## 

   if nargin<8
     extras.criterion = 1e-8;
     extras.max_iterations = 10;
     extras.v = 0.1;
     extras.verbose = 1;
   endif

   ## Details
   [n_x,n_y,n_u] = eval(sprintf("%s_def;", system_name));
   [n_us,n_tau] = size(us);

   ## Checks
   if (n_us<>n_u)
     error(sprintf("Inputs (%i) not equal to system inputs (%i)", n_us, n_u));
   endif
   
  [par,Par,Error,Y] = ppp_optimise(system_name,x_0,us,t,par_0,free,w,extras);

  U = par(free(:,1));
  U_all = Par(free(:,1),:);
endfunction

function [y,x,u,t,U,U_c,U_l] = ppp_nlin_sim (system_name,A_u,tau,t_ol,N_ol,w,extras)

  ## usage:  [y,x,u,t,U,U_c,U_l] = ppp_nlin_sim (system_name,A_u,tau,t_ol,N,w)
  ##
  ## 
  
  ## 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;
  endif
  
  

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

  ## System details 
  par = eval(sprintf("%s_numpar;", system_name));
  x_0 = eval(sprintf("%s_state;", system_name))
  [n_x,n_y,n_u] = eval(sprintf("%s_def;", system_name));

  ## Sensitivity system details
  sympars = eval(sprintf("%s_sympar;", s_system_name));
  pars = eval(sprintf("%s_numpar;", s_system_name));

  ## Times
  n_tau = length(tau);
  dtau = tau(2)-tau(1);		# Optimisation sample time
  Tau = [0:dtau:tau(n_tau)+eps]; # Time  in the moving axes
  n_Tau = length(Tau);
  dt = t_ol(2)-t_ol(1);
  n_t = length(t_ol);
  T_ol = t_ol(n_t)+dt
  

  ## Weight and moving-horizon setpoint
  weight = [zeros(n_y,n_Tau-n_tau), ones(n_y,n_tau)]; 
  ws = w*ones(1,n_tau);

  ## 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)
  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
		  
  ## Indices of U and sensitivities
  FREE = "[";
  free_name = "ppp";
  for i=1:n_U
    FREE = sprintf("%ssympars.%s_%i sympars.%s_%is", FREE, free_name, \
		   i, free_name, i);
    if i<n_U
      symbol = "; ";
    else
      symbol = "];";
    endif
    FREE = sprintf("%s%s", FREE, symbol);
  endfor
FREE
  free = eval(FREE);

  ## 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 = [];
  t_last = 0;
  UU = [];
  UU_l =[];
  UU_c =[];
  
  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
    ## 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
    ## Put initial value of U into the parameter vector
    pars(free(:,1)) = U;

    ## Compute U
    tick = time;
    if extras.max_iterations>0
      [U, U_all, Error, Y] = ppp_nlin(s_system_name,x_0s,u_star_tau,tau,pars,free,ws,extras);
    else
      Error = [];
    endif
    opt_time = time-tick;  
    printf("Optimisation %i took %i iterations and %2.2f sec\n", i, \
	   length(Error), opt_time);
#     title(sprintf("Moving horizon trajectories: Interval %i",i));
#     grid;
#     plot(tau,Y)
  
    ## Generate control

    u_ol = U'*u_star_t(1:n_U,:);

    ## Simulate system 
    ## (Assumes that first gain parameter is one)
    U_ol = [u_ol; zeros(n_u-1,n_t)]; # Generate the vector input
    [y_ol,x_ol] = eval(sprintf("%s_sim(x_0, U_ol, t_ol, par);", system_name));

    ## Extract state for next time
    x_0  = x_ol(:,n_t);

    y = [y y_ol(:,1:n_t)];
    x = [x x_ol(:,1:n_t)];
    u = [u u_ol(:,1:n_t)];

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

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

  ## Rename returned matrices
  U = UU;
  U_l = UU_l;
  U_c = UU_c;
endfunction

function [K_x T] = ppp_open2closed (A_u,A_c,k_x,x_0);

  ## usage:  [K_x T] = ppp_open2closed (A_u,A_c,k_x,x_0);
  ## K_x is the open-loop matrix as in U = K_w W - K_x x
  ## Note that K_x is a column vector of matrices - one matrix per input.
  ## T is the transformation matrix: x = T*Ustar; A_c = T*A_u*T^-1; U = (k_x*T)'
  ## A_u: The control basis-function matrix
  ## Us0: The initial value of Ustar
  ## A_c: closed-loop system matrix
  ## k_x: closed-loop feedback gain
  ## x_0: initial state

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	


  ## Check sizes
  n_o = is_square(A_u);
  n_c = is_square(A_c);

  if (n_o==0)||(n_c==0)||(n_o<>n_c)
    error("A_u and A_c must be square and of the same dimension");
  endif

  [n_u,n_x] = size(k_x);

  ## Defaults
  if nargin<4
    x_0 = zeros(n_c,1);
  endif

  ## Create U*(0)
  ##Us0  = ppp_ustar(A_u,n_u);
  Us0 = ones(1,n_o);

  ## Decompose A_u and Us0 into two bits:
  if n_o==n_c
    A_w = [];
    u_0 = Us0(1:n_c)';		# Assume same Us0 on each input
  else
    A_w = A_u(n_c+1:n_o, n_c+1:n_o)
    A_u = A_u(1:n_c, 1:n_c)
    U_w = Us0(1,n_c+1:n_o)'
    u_0 = Us0(1:n_c)'
  endif
  
  if !is_controllable(A_u,u_0)
    error("The pair [A_u, u_0] must be controllable");
  endif

  ## Controllability matrices
  C_o = u_0;
  C_c = x_0;
  for i=1:n_c-1
    C_o = [C_o A_u^i*u_0]; 
    C_c = [C_c A_c^i*x_0];
  endfor

  ## Transformation matrix: x = T*Ustar; A_c = T*A_u*T^-1; U = (k_x*T)'
  iC_o = C_o^-1;
  T = C_c*iC_o;

  K_x = [];
  for j = 1:n_u
    ## K_j matrix
    K_j = [];
    for i=1:n_c;
      ## Create T_i = dT/dx_i
      T_i = zeros(n_c,1);
      T_i(i) = 1;
      for k=1:n_c-1;
	A_k = A_c^k;
	T_i = [T_i A_k(:,i)];
      endfor
      T_i = T_i*iC_o;
      kj = k_x(j,:);		# jth row of k_x
      K_ji = kj*T_i;		# ith row of K_j
      K_j = [K_j; K_ji];
    endfor
    K_x = [K_x; K_j'];
  endfor

endfunction

function [par,Par,Error,Y,iterations] = ppp_optimise(system_name,x_0,u,t,par_0,free,y_0,extras);
  ## Usage: [par,Par,Error,Y,iterations] = ppp_optimise(system_name,x_0,u,t,par_0,free,y_0,weight,extras);
  ##  system_name     String containg system name
  ##  y_s   actual system output
  ##  theta_0   initial parameter estimate
  ##  free  Indices of the free parameters within theta_0
  ##  weight Weighting function - same dimension as y_s
  ##  extras.criterion convergence criterion
  ##  extras.max_iterations limit to number of iterations
  ##  extras.v  Initial Levenberg-Marquardt parameter

  ## Copyright (C) 1999,2000 by Peter J. Gawthrop

  ## Extract indices
  i_t = free(:,1); # Parameters
  i_s = free(:,2)'; # Sensitivities

  if nargin<8
    extras.criterion = 1e-5;
    extras.max_iterations = 10;
    extras.v = 1e-5;
    extras.verbose = 0;
  endif
  

  [n_y,n_data] = size(y_0);

  n_th = length(i_s);
  error_old = inf;
  error_old_old = inf;
  error = 1e50;
  reduction = 1e50;
  par = par_0;
  Par = par_0;
  step = ones(n_th,1);
  Error = [];
  Y = [];
  iterations = 0;
  v = extras.v;			# Levenverg-Marquardt parameter.
  r = 1;			# Step ratio

  while (abs(reduction)>extras.criterion)&&\
	(abs(error)>extras.criterion)&&\
	(iterations<extras.max_iterations)

    iterations = iterations + 1; # Increment iteration counter

    [y,y_par] = eval(sprintf("%s_sim(x_0,u,t,par,i_s)", system_name)); # Simulate

    ##Evaluate error, cost derivative J and cost second derivative JJ
    error = 0; 
    J = zeros(n_th,1);
    JJ = zeros(n_th,n_th);
    
    for i = 1:n_y
      E = y(i,:) - y_0(i,:);	#  Error
      error = error + (E*E');	# Sum the error over outputs
      y_par_i = y_par(i:n_y:n_y*n_th,:);
      J  = J + y_par_i*E';	# Jacobian
      JJ = JJ + y_par_i*y_par_i'; # Newton Euler approx Hessian (with LM
				# term
    endfor

    if iterations>1 # Adjust the Levenberg-Marquardt parameter
      reduction = error_old-error;
      predicted_reduction =  2*J'*step + step'*JJ*step;
      r = predicted_reduction/reduction;
      if (r<0.25)||(reduction<0)
	v = 4*v;
      elseif r>0.75
	v = v/2;
      endif

      if extras.verbose
	printf("Iteration: %i\n", iterations);
	printf("  error:  %g\n", error);
	printf("  reduction:  %g\n", reduction);
	printf("  prediction: %g\n", predicted_reduction);
	printf("  ratio:      %g\n", r);
	printf("  L-M param:  %g\n", v);
	printf("  parameters: ");
	for i_th=1:n_th
	  printf("%g ", par(i_t(i_th)));
	endfor
	printf("\n");
	  
      endif
    
      if reduction<0		# Its getting worse
	par(i_t) = par(i_t) + step; # rewind parameter
	error = error_old;	# rewind error
	error_old = error_old_old; # rewind old error
	if extras.verbose
	  printf(" Rewinding ....\n");
	endif
	
      endif

    endif

    ## Compute step using pseudo inverse
    JJL = JJ + v*eye(n_th);	# Levenberg-Marquardt term
    step =  pinv(JJL)*J;	# Step size

    par(i_t) = par(i_t) - step; # Increment parameters
    error_old_old = error_old;	# Save old error
    error_old = error;		# Save error

    ##Some diagnostics
    Error = [Error error];	# Save error
    Par = [Par par];		# Save parameters
    Y = [Y; y];			# Save output

  endwhile

endfunction

function [Gamma,gamma] = ppp_output_constraint (A,B,C,D,x_0,A_u,Tau,Min,Max,Order,i_y)

  ## usage:  [Gamma,gamma] = ppp_output_constraint (A,B,C,D,A_u,Tau,Min,Max,Order)
  ##
  ## Derives the output constraint matrices Gamma and gamma
  ## For Constraints Min and max at times Tau 
  ## Order=0 - output constraints
  ## Order=1 - output derivative constraints
  ## etc
  ## NOTE You can stack up Gamma and gamma matrices for create multi-output constraints.

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  ## Sizes
  [n_x,n_u,n_y] = abcddim(A,B,C,D); # System dimensions
  [n_U,m_U] = size(A_u);	# Number of basis functions
  [n,n_tau] = size(Tau);		# Number of constraint times
  
  if n_tau==0			# Nothing to be done
    Gamma = [];
    gamma = [];
    return
  endif

  ## Defaults
  if nargin<10
    Order = zeros(1,n_tau);
  endif

  if nargin<11
    i_y = 1;			# First output
  endif

  if n != 1
    error("Tau must be a row vector");
  endif
  
  n = length(Min);
  m = length(Max);
  o = length(Order);

  if (n != n_tau)||(m != n_tau)||(o != n_tau)
    error("Tau, Min, Max and Order must be the same length");
  endif
  

  ## Compute Gamma 
  Gamma = [];
  for i=1:n_U
    U = zeros(n_U,1); U(i,1) = 1; # Set up U_i
    y_i = ppp_ystar (A,B,C,D,x_0,A_u,U,Tau);# Compute y* for ith input for each tau
    y_i = y_i(:,i_y:n_y:n_y*n_tau); # Pluck out output i_y
    Gamma = [Gamma [-y_i';y_i']]; # Put in parts for Min and max
  endfor

  ## Compute gamma
  gamma = [-Min';Max'];

endfunction

function [u,U,J] = ppp_qp (x,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma)

  ## usage:  [u,U] = ppp_qp (x,W,J_uu,J_ux,J_uw,Gamma,gamma)
  ## INPUTS:
  ##      x: system state    
  ##      W: Setpoint vector
  ##      J_uu,J_ux,J_uw: Cost derivatives (see ppp_lin)
  ##      Us0: value of U* at tau=0 (see ppp_lin)
  ##      Gamma, gamma: U constrained by Gamma*U <= gamma 
  ## Outputs:
  ##      u: control signal
  ##      U: control weight vector
  ##
  ## Predictive pole-placement of linear systems using quadratic programming
  ## Use ppp_input_constraint and ppp_output_constraint to generate Gamma and gamma
  ## Use ppp_lin to generate J_uu,J_ux,J_uw
  ## Use ppp_cost to evaluate resultant cost function

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  ## Check the sizes
  n_x = length(x);

  [n_U,m_U] = size(J_uu);
  if n_U != m_U
    error("J_uu must be square");
  endif

  [n,m] = size(J_ux);
  if (n != n_U)||(m != n_x)
    error("J_ux should be %ix%i not %ix%i",n_U,n_x,n,m);
  endif


  if length(gamma)>0		# Constraints exist: do the QP algorithm 
    U = qp(J_uu,(J_ux*x - J_uw*W),Gamma,gamma); # QP solution for weights U
    #U = pd_lcp04(J_uu,(J_ux*x - J_uw*W),Gamma,gamma); # QP solution for weights U
    u = Us0*U;			# Control signal
  else			# Do the unconstrained solution
    ## Compute the open-loop gains
    K_w = J_uu\J_uw;
    K_x = J_uu\J_ux;

    ## Closed-loop control
    U = K_w*W - K_x*x;		# Basis functions weights - U(t)
    u = Us0*U;			# Control u(t)
  endif

endfunction

function [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, Tau_u,Min_u,Max_u,Order_u, Tau_y,Min_y,Max_y,Order_y, W,x_0,Delta_ol,movie)

  ## usage: [T,y,u,J] = ppp_qp_sim (A,B,C,D,A_u,A_w,t,Q, Tau_u,Min_u,Max_u,Order_u, Tau_y,Min_y,Max_y,Order_y, W,x_0,movie)
  ## Needs documentation - see ppp_ex11 for example of use.
  ## OUTPUTS
  ## T: Time vector
  ## y,u,J output, input and cost

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	
  
  if nargin<19			# No intermittent control
    Delta_ol = 0;
  endif

  if nargin<20			# No movie
    movie = 0;
  endif

  ## Check some sizes
  [n_x,n_u,n_y] = abcddim(A,B,C,D);

  [n_x0,m_x0] = size(x_0);
  if (n_x0 != n_x)||(m_x0 != 1)
    error(sprintf("Initial state x_0 must be %ix1 not %ix%i",n_x,n_x0,m_x0));
  endif
  
  ## Input constraints (assume same on all inputs)
  Gamma_u=[];
  gamma_u=[];
  for i=1:n_u
    [Gamma_i,gamma_i] = ppp_input_constraint (A_u,Tau_u,Min_u,Max_u,Order_u,i,n_u);
    Gamma_u = [Gamma_u; Gamma_i];
    gamma_u = [gamma_u; gamma_i];
  endfor

  ## Output constraints
  [Gamma_y,gamma_y] = ppp_output_constraint  (A,B,C,D,x_0,A_u,Tau_y,Min_y,Max_y,Order_y);

  ## Composite constraints - t=0
  Gamma = [Gamma_u; Gamma_y];
  gamma = [gamma_u; gamma_y];

  ## Design the controller
  disp("Designing controller");
  [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww] = ppp_lin (A,B,C,D,A_u,A_w,t,Q);

  ## Set up various time vectors
  dt = t(2)-t(1);		# Time increment

  ## Make sure Delta_ol is multiple of dt
  Delta_ol = floor(Delta_ol/dt)*dt

  if Delta_ol>0			# Intermittent control
    T_ol = 0:dt:Delta_ol-dt;	# Create the open-loop time vector
  else
    T_ol = 0;
    Delta_ol = dt;
  endif
  
  T_cl = 0:Delta_ol:t(length(t))-Delta_ol; # Closed-loop time vector
  n_Tcl = length(T_cl);
  
  Ustar_ol = ppp_ustar(A_u,n_u,T_ol); # U* in the open-loop interval
  [n,m] = size(Ustar_ol);
  n_U = m/length(T_ol);		# Determine size of each Ustar

  ## Discrete-time system
  csys = ss2sys(A,B,C,D);
  dsys = c2d(csys,dt);
  [Ad, Bd] = sys2ss(dsys);

  x = x_0;			# Initialise state

  ## Initialise the saved variable arrays
  X = [];
  u = [];
  du = [];
  J = [];
  tick= time;
  i = 0;
  disp("Simulating ...");
  for t=T_cl			# Outer loop at Delta_ol
    ##disp(sprintf("Time %g", t));
    ## Output constraints
    [Gamma_y,gamma_y] = ppp_output_constraint  (A,B,C,D,x,A_u,Tau_y,Min_y,Max_y,Order_y);
    
    ## Composite constraints 
    Gamma = [Gamma_u; Gamma_y];
    gamma = [gamma_u; gamma_y];
    
    ## Compute U(t)
    [uu U] = ppp_qp (x,W,J_uu,J_ux,J_uw,Us0,Gamma,gamma); # Compute U
 
    ## Compute the cost (not necessary but maybe interesting)
#    [J_t] = ppp_cost (U,x,W,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww); # cost
#    J = [J J_t];

    ## Simulation loop
    i_ol = 0;
    for t_ol=T_ol		# Inner loop at dt

      ## Compute ol control
      i_ol = i_ol+1;
      range = (i_ol-1)*n_U + 1:i_ol*n_U; # Extract current U*
      ut = Ustar_ol(:,range)*U;	# Compute OL control (U* U)

      ## Simulate the system
      i = i+1;
      X = [X x];		# Save state
      u = [u ut];		# Save input
      x = Ad*x + Bd*ut;	# System

#       if movie			# Plot the moving horizon
# 	tau = T(1:n_T-i);	# Tau with moving horizon
# 	tauT = T(i+1:n_T);	# Tau with moving horizon + real time
# 	[ys,us,xs,xu,AA] = ppp_ystar (A,B,C,D,x,A_u,U,tau); # OL response
# 	plot(tauT,ys, tauT(1), ys(1), "*")
#       endif
    endfor
  endfor
  
  ## Save the last values
  X = [X x];		# Save state
  u = [u ut];		# Save input

  tock = time;
  Iterations = length(T_cl)
  Elapsed_Time = tock-tick
  y = C*X + D*u;		# System output

  T = 0:dt:t+Delta_ol;		# Overall time vector

endfunction

function [y,y_s] = ppp_sim (system_name,x_0,u,t,par,i_s,external)

  ## mtt_sim: Simulates system  sensitivity functions. 
  ## usage:  [y,y_s] = ppp_sim (system_name,x_0,u,t,par,i_s)
  ##   system_name string containing name of the sensitivity system
  ##   x_0         initial state 
  ##   u           system input (one input per row)
  ##   t           row vector of time
  ##   par         system parameter vector
  ##   i_s         indices of sensitivity parameters 


  if nargin<7
    external = 0;
  endif
  
  ## Some sizes
  n_s = length(i_s);
  n_t = length(t);


  for i = 1:n_s

    ## Set sensitivity parameters
    par(i_s(i)) = 1.0;
    par(complement(i_s(i),i_s)) = 0;

    if external
      par_string = "";
      for i_string=1:length(par)
	par_string = sprintf("%s %s", par_string, num2str(par(i_string)));
      endfor
      data_string = system(sprintf("./%s_ode2odes.out %s | cut -f 2-%i", \
				   system_name, par_string, 1+n_s));
      Y = str2num(data_string)';
    else
      Y = eval(sprintf("%s_sim(x_0,u,t,par);", system_name));
    endif

    [n_Y,m_Y] = size(Y);
     n_y = n_Y/2;
    if i==1	
      y = Y(1:2:n_Y,:);		# Save up the output
      y_s = zeros(n_s*n_y, n_t); # Initialise for speed
    endif
 
    y_s((i-1)*n_y+1:i*n_y,:)  = Y(2:2:n_Y,:);	# Other sensitivities
    
  endfor

title("");
plot(t,y);

endfunction

function [Y,X] = ppp_sm2sr(A,B,C,D,T,u0,x0);
  ## Usage [Y,X] = ppp_sm2sr(A,B,C,D,T,u0,x0);
  ## Computes a step response
  ## A,B,C,D- state matrices
  ## T vector of time points
  ## u0 input gain vector: u = u0*unit step.

  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  [Ny,Nu] = size(D);
  [Ny,Nx] = size(C);

  if nargin<6
    u0 = zeros(Nu,1);
    u0(1) = 1;
  end;

  if nargin<7
    x0 = zeros(Nx,1);
  end;

  [N,M] = size(T);
  if M>N
    T = T';
    N = M;
  end;



  one = eye(Nx);

  Y = zeros(Ny,N);
  X = zeros(Nx,N);

  dt = T(2)-T(1);		# Assumes fixed interval
  expAdt = expm(A*dt);		# Compute matrix exponential
  i = 0;
  expAt = one;

  DoingStep = max(abs(u0))>0;	# Is there a step component?
  DoingInitial = max(abs(x0))>0; # Is there an initial component?
  for t = T'
    i=i+1;
    if Nx>0
      x = zeros(Nx,1);
      if DoingStep
	x = x + ( A\(expAt-one) )*B*u0;
      endif
      if DoingInitial
	x = x + expAt*x0;
      endif
      
      expAt = expAt*expAdt;

      X(:,i) = x;
      if Ny>0
	y = C*x + D*u0;
	Y(:,i) = y;
      endif
    elseif Ny>0
      y = D*u0;
      Y(:,i) = y;
    endif
  endfor


endfunction

function Ustar = ppp_ustar (A_u,n_u,tau,order)

  ## usage:  Us = ppp_ustar(A_u,n_u,tau)
  ##
  ## Computes the U* matrix at time tau in terms of A_u
  ## n_u : Number of system inputs
  ## If tau is a vector, computes U* at each tau and puts into a row vector:
  ##     Ustar = [Ustar(tau_1) Ustar(tau_2) ...]
  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	

  if nargin<2
    n_u = 1;
  endif
  
  if nargin<3
    tau = 0;
  endif
  
  if nargin<4
    order = 0;
  endif
  

  [n,m] = size(A_u);		# Size of composite A_u matrix
  N = m;			# Number of U* functions per input  
  nm = n/m;

  if (nm != n_u)&&(n!=m)	# Check consistency
    error("A_u must be square or be a column of square matrices");
  endif

  u_0 = ones(N,1);

  Ustar = [];
  for t = tau;
    ustar = [];
    for i = 1:n_u
      A_i = ppp_extract(A_u,i);
      Ak = A_i^order;
      eA = expm(A_i*t);
      ustar = [ustar; zeros(1,(i-1)*N), (Ak*eA*u_0)', zeros(1,(n_u-i)*N)];
    endfor
    Ustar = [Ustar ustar];
  endfor


endfunction

function [y_u, Us] = ppp_y_u (A,B,C,D,A_u,u_0,tau)

  ## usage:  y_u = ppp_y_u (A,B,C,D,A_u,u_0,t)
  ##
  ## Computes y_u derivative of y* wrt U
  ## Called by ppp_lin
  ## OBSOLETE

  ###############################################################
  ## Version control history
  ###############################################################
  ## $Id$
  ## $Log$
  ## Revision 1.6  2000/12/27 16:41:05  peterg
  ## *** empty log message ***
  ##
  ## Revision 1.5  1999/05/31 01:58:01  peterg
  ## Now uses ppp_extract
  ##
  ## Revision 1.4  1999/05/12 00:10:35  peterg
  ## Modified for alternative (square) A_u
  ##
  ## Revision 1.3  1999/05/03 23:56:32  peterg
  ## Multivariable version - tested for N uncoupled systems
  ##
  ## Revision 1.2  1999/05/03 00:38:32  peterg
  ## Changed data storage:
  ## y_u saved as row vector, one row for each time, one column for
  ## each U
  ## y_x saved as row vector, one row for each time, one column for
  ## each x
  ## W* saved as row vector, one row for each time, one column for
  ## each element of W*
  ## This is consistent with paper.
  ##
  ## Revision 1.1  1999/04/29 06:02:43  peterg
  ## Initial revision
  ##
  ###############################################################



  ## Check argument dimensions
  [n_x,n_u,n_y] = abcddim(A,B,C,D);
  if (n_x==-1)
    return
  endif

  [n,m] = size(A_u);		# Size of composite A_u matrix
  N = m;			# Number of U* functions per input
  
  y_u = [];			# Initialise
  Us = [];
  
#   for input=1:n_u		# Do for each system input
#     a_u = ppp_extract(A_u,input); # Extract the relecant A_u matrix
#     for i=1:N			# Do for each input function U*_i
#       C_u = zeros(1,N); C_u(i) = 1; # Create C_u for this U*_i
#       b = B(:,input);		# B vector for this input
#       d = D(:,input);		# D vector for this input
#       [y,u] = ppp_transient (t,a_u,C_u,u_0,A,b,C,d); # Compute response for this input
#       y_u = [y_u y'];		# Save y_u (y for input u)  with one row for each t.
#       Us = [Us u'];		# Save u (input)  with one row for each t.
#     endfor
#   endfor
  i_U = 0;
  x_0 = zeros(n_x,1);		# This is for x=0
  for input=1:n_u		# Do for each system input
    a_u = ppp_extract(A_u,input); # Extract the relevant A_u matrix
    for i=1:N			# Do for each input function U*_i
      dU = zeros(N*n_u,1);
      dU(++i_U) = 1;		# Create dU/dU_i 
      [ys,us] = 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.
    endfor
  endfor

endfunction

function [ys,us,xs,xu,AA] = ppp_ystar (A,B,C,D,x_0,A_u,U,tau)

  ## usage:  [ys,us,xs,xu,AA] = ppp_ystar (A,B,C,D,x_0,A_u,U,tau)
  ##
  ## Computes open-loop moving horizon variables at time tau
  ## Inputs:
  ## A,B,C,D     System matrices
  ## x_0         Initial state
  ## 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
  ## U           Column vector of optimisation coefficients  
  ## tau         Row vector of times at which outputs are computed
  ## Outputs:
  ## ys          y*, one column for each time tau 
  ## us          u*, one column for each time tau 
  ## xs          x*, one column for each time tau 
  ## xu          x_u, one column for each time tau 
  ## AA          The composite system matrix
  
  ## Copyright (C) 1999 by Peter J. Gawthrop
  ## 	$Id$	


  [n_x,n_u,n_y] = abcddim(A,B,C,D); # System dimensions
  no_system = n_x==0;

  [n,m] = size(A_u);		# Size of composite A_u matrix
  square = (n==m);		# Is A_u square?
  n_U = m;			# functions per input

  
  [n,m] = size(U);
  if (m != 1)
    error("U must be a column vector");
  endif
  
  if n_u>0
    if n_u!=length(U)/n_U
      error("U must be a column vector with n_u*n_U components");
    endif
  else
    n_u = length(U)/n_U;	# Deduce n_u from U if no system
  endif
  

  [n,m]=size(tau);
  if (n != 1 )
    error("tau must be a row vector of times");
  endif
  
  if square			# Then same A_u for each input
    ## Reorganise vector U into matrix Utilde  
    Utilde = [];
    for i=1:n_u
      j = (i-1)*n_U;
      range = j+1:j+n_U;
      Utilde = [Utilde; U(range,1)'];
    endfor

    ## Composite A matrix
    if no_system
      AA = A_u;
    else
      Z = zeros(n_U,n_x);
      AA = [A   B*Utilde
	    Z   A_u];
    endif
    
    xx_0 = [x_0;ones(n_U,1)];	# Composite initial condition
  else				# Different A_u on each input
    ## Reorganise vector U into matrix Utilde  
    Utilde = [];
    for i=1:n_u
      j = (i-1)*n_U;
      k = (n_u-i)*n_U;
      range = j+1:j+n_U;
      Utilde = [Utilde; zeros(1,j), U(range,1)', zeros(1,k)];
    endfor

    ## Create the full A_u matrix (AA_u) with the A_i s on the diagonal
#     AA_u = [];
#     for i = 1:n_u
#       AA_u = ppp_aug(AA_u,ppp_extract(A_u,i));
#     endfor
    AA_u = ppp_inflate(A_u);

    ## Composite A matrix
    if no_system
      AA = AA_u;
    else
      Z = zeros(n_U*n_u,n_x);
      AA = [A   B*Utilde
	    Z   AA_u];
    endif
    xx_0 = [x_0;ones(n_U*n_u,1)];	# Composite initial condition
  endif
  
  
  ## Initialise
  xs = [];			# x star
  xu = [];			# x star
  ys = [];			# y star
  us = [];			# u star
  n_xx = length(xx_0);		# Length of composite state

  ## Compute the star variables
  for t=tau
    xxt = expm(AA*t)*xx_0;	# Composite state
    xst = xxt(1:n_x);		# x star
    xut = xxt(n_x+1:n_xx);	# x star
    yst = C*xst;		# y star
    ust = Utilde*xut;		# u star

    xs = [xs xst];		# x star
    xu = [xu xut];		# x star
    ys = [ys yst];		# y star
    us = [us ust];		# u star
  endfor

endfunction

function [A,B,C,D] = rpv
% System RPV
% This system is the remotely-piloted vehicle example from the book:
% J.M Maciejowski: Multivariable Feedback Design  Addison-Wesley, 1989
% It has 6 states, 2 inputs and 2 outputs.

% P J Gawthrop Jan 1998

A = [-0.0257  -36.6170  -18.8970  -32.0900    3.2509   -0.7626
      0.0001   -1.8997    0.9831   -0.0007   -0.1780   -0.0050
      0.0123   11.7200   -2.6316    0.0009  -31.6040   22.3960
           0         0    1.0000         0         0         0
           0         0         0         0  -30.0000         0
           0         0         0         0         0  -30.0000];

B = [0     0
     0     0
     0     0
     0     0
    30     0
     0    30];

C = [0     1     0     0     0     0
     0     0     0     1     0     0];

D = zeros(2,2);

function [A,B,C,D] = tgen
% System TGEN from
% This system is the turbogenerator example from the book:
% J.M Maciejowski: Multivariable Feedback Design  Addison-Wesley, 1989
% It has 6 states, 2 inputs and 2 outputs.

% P J Gawthrop Jan 1998

A = [-18.4456    4.2263   -2.2830    0.2260    0.4220   -0.0951
      -4.0977   -6.0706    5.6825   -0.6966   -1.2246    0.2873
       1.4449    1.4336   -2.6477    0.6092    0.8979   -0.2300
      -0.0093    0.2302   -0.5002   -0.1764   -6.3152    0.1350
      -0.0464   -0.3489    0.7238    6.3117   -0.6886    0.3645
      -0.0602   -0.2361    0.2300    0.0915   -0.3214   -0.2087];

B = [-0.2748    3.1463
     -0.0501   -9.3737
     -0.1550    7.4296
      0.0716   -4.9176
     -0.0814  -10.2648
      0.0244   13.7943];

C = [0.5971   -0.7697    4.8850    4.8608   -9.8177   -8.8610
     3.1013    9.3422   -5.6000   -0.7490    2.9974   10.5719];

D = zeros(2,2);

function X = transient (t,A,x_0)

  ## usage:  L = transient (t,p,order)
  ##
  ## Computes transient response for time t with initial condition x_0

  ###############################################################
  ## Version control history
  ###############################################################
  ## $Id$
  ## $Log$
  ## Revision 1.1  1999/04/27 04:46:05  peterg
  ## Initial revision
  ##
  ## Revision 1.1  1999/04/25 23:19:40  peterg
  ## Initial revision
  ##
  ###############################################################


X=[];
  for tt=t			# Create the Transient up to highest order
    x = expm(A*tt)*x_0;
    X = [X x];
  endfor

endfunction