Overview
Comment:Putting the PPP library under cvs
Downloads: Tarball | ZIP archive | SQL archive
Timelines: family | ancestors | descendants | both | origin/master | trunk
Files: files | file ages | folders
SHA3-256: 182384e0d6a2879737978a234bc30dbb44f2b9224b88b0af1880d9809f50b064
User & Date: gawthrop@users.sourceforge.net on 2001-04-02 15:02:35
Other Links: branch diff | manifest | tags
Context
2001-04-02
17:36:20
Resolved $sys name clash when using -s check-in: 06cc77a126 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
15:02:35
Putting the PPP library under cvs check-in: 182384e0d6 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
10:57:57
Added missing ; check-in: ef3ac62298 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
Changes
Hide Diffs Side-by-Side Diffs Ignore Whitespace Patch

Added mttroot/mtt/lib/control/PPP/Beam_numpar.m version [0d62366729]. 















































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

% 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








Added mttroot/mtt/lib/control/PPP/Beam_sm.m version [cd16b034f7].









































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

% -*-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);








Added mttroot/mtt/lib/control/PPP/NMPsystem.m version [2fed1aea39].

























1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/TwoMassSpring.m version [5f26391fd5].






































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/airc.m version [c3d4271b18].































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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);










Added mttroot/mtt/lib/control/PPP/autm.m version [e5ff112d0d].












































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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);









Added mttroot/mtt/lib/control/PPP/butterworth_matrix.m version [c40a4036f0].




















1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/damped_matrix.m version [62d084883f].






























1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/laguerre_matrix.m version [2bc3da3dab].



















1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_aug.m version [85d071a163].






























1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_butter.m version [e0dda0f911].



























1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_closedloop.m version [3bb5af1be2].









































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_cost.m version [f7cb316246].


















1
2
3
4
5
6
7
8
9
10
11
12
13
14

+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex1.m version [a792430509].


































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_ex10.m version [cee25ffa57].






































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex11.m version [8afbccc82d].













































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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












Added mttroot/mtt/lib/control/PPP/ppp_ex12.m version [4b9ce5134a].















































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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
















Added mttroot/mtt/lib/control/PPP/ppp_ex13.m version [410a811ae6].





















































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_ex14m.m version [da6984e601].










































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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












Added mttroot/mtt/lib/control/PPP/ppp_ex15.m version [a26d3cfd27].

































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex16.m version [b34052bc15].











































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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
















Added mttroot/mtt/lib/control/PPP/ppp_ex17.m version [0cc0c0d748].

































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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












Added mttroot/mtt/lib/control/PPP/ppp_ex18.m version [4568857bd3].












































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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













Added mttroot/mtt/lib/control/PPP/ppp_ex19.m version [667d1947d8].












































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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














Added mttroot/mtt/lib/control/PPP/ppp_ex2.m version [163faa5b9f].




































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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









Added mttroot/mtt/lib/control/PPP/ppp_ex20.m version [bd637b24f5].













































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_ex20.new.m version [fd66e71f1b].
































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

# 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








Added mttroot/mtt/lib/control/PPP/ppp_ex21.m version [0b03160976].
















































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_ex3.m version [c1cd799f6a].


































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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









Added mttroot/mtt/lib/control/PPP/ppp_ex4.m version [f6af77ca79].



















































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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












Added mttroot/mtt/lib/control/PPP/ppp_ex5.m version [eab12a6897].






























































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex6.m version [ba0ffb029f].



























































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_ex7.m version [20afb5cf0e].









































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex8.m version [d3d175c796].

































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_ex9.m version [c71a52df94].














































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_examples.m version [e27b26eff0].


























































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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














Added mttroot/mtt/lib/control/PPP/ppp_extract.m version [b84460c15f].



































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_inflate.m version [7743bb793c].































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_input_constraint.m version [7527131a2c].











































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_lin.m version [342b9867d7].


























































































































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_lin_con_sol.m version [3f791341cb].














1
2
3
4
5
6
7
8
9
10

+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_lin_obs.m version [3637bc55b1].


























































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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

















Added mttroot/mtt/lib/control/PPP/ppp_lin_plot.m version [e00d3bd973].






























































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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









Added mttroot/mtt/lib/control/PPP/ppp_lin_sim.m version [767b01c78f].




























































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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









Added mttroot/mtt/lib/control/PPP/ppp_nlin.m version [0eacb03d2f].


































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_nlin_sim.m version [93a2ec563f].














































































































































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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













Added mttroot/mtt/lib/control/PPP/ppp_open2closed.m version [f6b3aefde7].























































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_optimise.m version [aff901223d].





























































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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













Added mttroot/mtt/lib/control/PPP/ppp_output_constraint.m version [cae34a060b].


































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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










Added mttroot/mtt/lib/control/PPP/ppp_qp.m version [e94ddba2d2].






















































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_qp_sim.m version [58fd63cdfa].













































































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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











Added mttroot/mtt/lib/control/PPP/ppp_sim.m version [a4ff8b90d8].


























































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_sm2sr.m version [b8cab9f99f].









































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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









Added mttroot/mtt/lib/control/PPP/ppp_ustar.m version [7e105f13fe].




















































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/ppp_y_u.m version [8a57fa7a93].























































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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












Added mttroot/mtt/lib/control/PPP/ppp_ystar.m version [d942259424].
































































































































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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








Added mttroot/mtt/lib/control/PPP/rpv.m version [c302718039].
































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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);










Added mttroot/mtt/lib/control/PPP/tgen.m version [37e3b79324].
































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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);










Added mttroot/mtt/lib/control/PPP/transient.m version [14fe0c3057].































1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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






















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