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 Nn + 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_u0 + [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)&&\ + (iterations1 # 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