ADDED mttroot/mtt/lib/control/PPP/Beam_numpar.m
Index: mttroot/mtt/lib/control/PPP/Beam_numpar.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/Beam_numpar.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/Beam_sm.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/Beam_sm.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/NMPsystem.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/NMPsystem.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/TwoMassSpring.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/TwoMassSpring.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/airc.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/airc.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/autm.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/autm.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/butterworth_matrix.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/butterworth_matrix.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/damped_matrix.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/damped_matrix.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/laguerre_matrix.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/laguerre_matrix.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_aug.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_aug.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_butter.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_butter.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_closedloop.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_closedloop.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_cost.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_cost.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex1.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex1.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex10.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex10.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex11.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex11.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex12.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex12.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex13.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex13.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex14m.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex14m.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex15.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex15.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex16.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex16.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex17.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex17.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex18.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex18.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex19.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex19.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex2.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex2.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex20.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex20.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex20.new.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex20.new.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex21.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex21.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex3.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex3.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex4.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex4.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex5.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex5.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex6.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex6.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex7.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex7.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex8.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex8.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ex9.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ex9.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_examples.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_examples.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_extract.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_extract.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_inflate.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_inflate.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_input_constraint.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_input_constraint.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_lin.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_lin.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_lin_con_sol.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_lin_con_sol.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_lin_obs.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_lin_obs.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_lin_plot.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_lin_plot.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_lin_sim.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_lin_sim.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_nlin.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_nlin.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_nlin_sim.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_nlin_sim.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_open2closed.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_open2closed.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_optimise.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_optimise.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_output_constraint.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_output_constraint.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_qp.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_qp.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_qp_sim.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_qp_sim.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_sim.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_sim.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_sm2sr.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_sm2sr.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ustar.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ustar.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_y_u.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_y_u.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/ppp_ystar.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/ppp_ystar.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/rpv.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/rpv.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/tgen.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/tgen.m
@@ -0,0 +1,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
Index: mttroot/mtt/lib/control/PPP/transient.m
==================================================================
--- /dev/null
+++ mttroot/mtt/lib/control/PPP/transient.m
@@ -0,0 +1,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