├── _config.yml ├── .gitignore ├── docs └── example10.pdf ├── examples ├── example10.mlx ├── example03.m ├── Burgers1DControl │ ├── openLoop.m │ ├── README.md │ └── hjbBurgers1D.m ├── example01.m ├── ChafeeInfante1DControl │ ├── openLoop.m │ ├── oned_f_int.m │ ├── oned_bilinear.m │ ├── oned_mesh.m │ ├── oned_shape.m │ ├── oned_quadrature.m │ └── ChafeeInfanteFEMControl.m ├── KuramotoSivashinsky1DControl │ ├── oned_f_int.m │ ├── openLoop.m │ ├── oned_bilinear.m │ ├── oned_plot_hermite.m │ ├── oned_mesh.m │ ├── oned_shapeherm.m │ ├── oned_quadrature.m │ └── KuramotoSivashinskyFEMControl.m ├── LICENSE ├── example02.m ├── README.md ├── example04.m ├── exampleCI.m ├── exampleKS.m ├── example06.m ├── example07.m └── example08.m ├── setNSTpath.m ├── KnownIssues ├── setPolynomialSystemsPath.m ├── setKroneckerToolsPath.m ├── testScripts ├── testPQR.m ├── testKron2CT.m ├── testLyapProduct.m ├── testAlbrekhtQQR.m └── AlbrekhtKronQQR.m ├── testCQR.m ├── testPQR.m ├── LICENSE ├── runNST3.m ├── runNSTl.m ├── runNST.m ├── testBilinear.m ├── examplesForACC.m ├── plotLyapunov2.m ├── examplesForMTNS.m ├── examplesForTAC.m ├── README.md ├── pqr.m ├── runComparisons.m ├── runNSTcomparisons.m └── cqrOdd.m /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | kronecker/tensor_recursive/* 3 | misc_code.m 4 | *mat 5 | -------------------------------------------------------------------------------- /docs/example10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jborggaard/QQR/HEAD/docs/example10.pdf -------------------------------------------------------------------------------- /examples/example10.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jborggaard/QQR/HEAD/examples/example10.mlx -------------------------------------------------------------------------------- /setNSTpath.m: -------------------------------------------------------------------------------- 1 | addpath('/Volumes/borggaard4/Research/Control/NonlinearControl/NonlinearControlExamples/nst15') -------------------------------------------------------------------------------- /KnownIssues: -------------------------------------------------------------------------------- 1 | 1. When the number of states==1, there is an issue in Lyapunov recursive 2 | (and Laplace recursive, in the calculation of d) The size command 3 | on a tensor of size (1,1,1,1,1) will not give 5 as the length gets 4 | compressed and results in a (1,1) "matrix" 5 | 6 | 2. 7 | 8 | 9 | -------------------------------------------------------------------------------- /setPolynomialSystemsPath.m: -------------------------------------------------------------------------------- 1 | % PolynomialSystems can be obtained from 2 | % https://github.com/jborggaard/PolynomialSystems 3 | % 4 | % Download PolynomialSystems, then adjust the path variable below if needed. 5 | PolynomialSystemsPath = '../PolynomialSystems'; 6 | 7 | addpath(PolynomialSystemsPath) 8 | -------------------------------------------------------------------------------- /setKroneckerToolsPath.m: -------------------------------------------------------------------------------- 1 | % The KroneckerTools can be obtained from 2 | % https://github.com/jborggaard/KroneckerTools 3 | % 4 | % Download KroneckerTools, then adjust the path variable below if needed. 5 | KroneckerToolsPath = '/Volumes/borggaard4/Software/MyPublicSoftware/KroneckerTools'; 6 | 7 | addpath([KroneckerToolsPath,'/src']) 8 | addpath([KroneckerToolsPath,'/util']) 9 | 10 | if ( exist([KroneckerToolsPath,'/tensor_recursive'],'dir') ) 11 | addpath([KroneckerToolsPath,'/tensor_recursive']) 12 | end 13 | -------------------------------------------------------------------------------- /testScripts/testPQR.m: -------------------------------------------------------------------------------- 1 | % A script to test the pqr function 2 | 3 | % 4 | %% Set problem sizes and approximation degree 5 | n = 12; 6 | m = 4; 7 | 8 | degree = 5; 9 | 10 | % 11 | %% Generate random problem 12 | A = rand(n,n); 13 | B = rand(n,m); 14 | Q = sprandsym(n,0.5,0.1,1); 15 | R = sprandsym(m,0.5,0.5,1); 16 | 17 | N = cell(1,degree); 18 | for d=1:degree 19 | N{d} = rand(n,n^d); 20 | end 21 | 22 | [k,v] = pqr(A,B,Q,R,N,degree); 23 | [kNew,vNew] = pqrNew(A,B,Q,R,N,degree); 24 | 25 | for d=1:degree 26 | fprintf('Degree %d: kError=%g, vError=%g\n',... 27 | d,norm(k{d}-kNew{d}),norm(v{d+1}-vNew{d+1})); 28 | end -------------------------------------------------------------------------------- /examples/example03.m: -------------------------------------------------------------------------------- 1 | function [A,B,Q,R,N] = example03(n,m,seed) 2 | %EXAMPLE03 Compares feedback strategies for a random, stable quadratic system 3 | 4 | if ( ~exist('n','var') ) 5 | n = 4; 6 | end 7 | 8 | if ( ~exist('m','var') ) 9 | m = 2; 10 | end 11 | 12 | if ( ~exist('seed','var') ) 13 | seed = 0; 14 | end 15 | 16 | rng(seed,'v5uniform') 17 | 18 | % x0 = zeros(n,1); u0 = zeros(m,1); %#ok 19 | 20 | % produce a random orthogonal matrix to build an SPD matrix 21 | Z = rand(n,n); [Q,~] = qr(Z); 22 | A = -Q*diag(rand(n,1))*Q'; 23 | 24 | B = rand(n,m); 25 | 26 | Q = eye(n); 27 | 28 | R = eye(m); 29 | 30 | N = rand(n,n*n); 31 | 32 | end -------------------------------------------------------------------------------- /examples/Burgers1DControl/openLoop.m: -------------------------------------------------------------------------------- 1 | % Compute Burgers equation matrices and perform open loop simulation 2 | 3 | n = 41; 4 | m = 1; % a control input that we won't use 5 | p = 2; % a controlled output that we also don't use 6 | 7 | epsilon = 0.005; 8 | tInfinity = 15; 9 | 10 | [E,A,B,C,N,zInit] = BurgersFEMControl(n,m,p); 11 | xNodes = linspace(0,1,n); 12 | 13 | % A = epsilon*(E\A); 14 | % B = E\B; 15 | % N = E\N; 16 | A = epsilon*A; 17 | options = odeset('Mass',E); 18 | 19 | zdot = @(t,z) A*z + N*kron(z,z); 20 | [T,Z] = ode23(zdot,[0 tInfinity],zInit,options); 21 | figure(10) 22 | mesh(xNodes,T,Z) 23 | xlabel('x'); ylabel('time') 24 | title('Open Loop Simulation') 25 | -------------------------------------------------------------------------------- /testCQR.m: -------------------------------------------------------------------------------- 1 | % test cqr 2 | setKroneckerToolsPath 3 | setNSTpath 4 | 5 | % define the random test problem 6 | n = 3; 7 | m = 2; 8 | 9 | degree = 7; 10 | 11 | rng(0,'v5uniform') % set random number generator for reproducable tests. 12 | 13 | A = -full( sprandsym(n,0.3,rand(n,1)) ); 14 | B = rand(n,m); 15 | 16 | N{2} = rand(n,n^2); N{2} = kronMatrixSymmetrize(N{2},n,2); 17 | N{3} = rand(n,n^3); N{3} = kronMatrixSymmetrize(N{3},n,3); 18 | 19 | Q = full( sprandsym(n,0.5,rand(n,1)) ); 20 | R = full( sprandsym(m,0.5,rand(m,1)) ); 21 | 22 | %[k,v] = cqr(A,B,Q,R,N,degree,'LyapunovRecursive',true); 23 | [k,v] = cqr(A,B,Q,R,N,degree,[],true); 24 | 25 | runNSTcomparisons 26 | 27 | N{2} = zeros(n,n^2); 28 | [k,v] = cqrOdd(A,B,Q,R,N{3},degree,[],true); 29 | 30 | runNSTcomparisons 31 | -------------------------------------------------------------------------------- /examples/example01.m: -------------------------------------------------------------------------------- 1 | function [A,B,Q,R,N,x0,u0] = example01(n,m,seed) 2 | %EXAMPLE01 Produces a repeatable random quadratic system (using a fixed rng). 3 | % 4 | % Usage: 5 | % [A,B,Q,R,N,z0,u0] = example01(n,m,seed); 6 | % 7 | % Variables: 8 | % n - state dimension (default: n=4) 9 | % m - control dimension (default: m=2) 10 | % seed - seed for the v5uniform random number generator (default=0) 11 | %% 12 | 13 | if ( ~exist('n','var') ) 14 | n = 4; 15 | end 16 | 17 | if ( ~exist('m','var') ) 18 | m = 2; 19 | end 20 | 21 | if ( ~exist('seed','var') ) 22 | seed = 0; 23 | end 24 | 25 | rng(seed,'v5uniform') 26 | 27 | x0 = zeros(n,1); u0 = zeros(m,1); 28 | A = rand(n,n); B = rand(n,m); Q = eye(n); R = eye(m); 29 | 30 | %R = rand(m,m); R=R*R'; 31 | 32 | N = rand(n,n*n); 33 | 34 | end -------------------------------------------------------------------------------- /testPQR.m: -------------------------------------------------------------------------------- 1 | % A script to test the pqr function 2 | clear A B Q R N 3 | % 4 | %% Set problem sizes and approximation degree 5 | n = 3; 6 | m = 4; 7 | 8 | degree = 5; 9 | 10 | % 11 | %% Generate random problem 12 | A = rand(n,n); 13 | B = rand(n,m); 14 | Q = sprandsym(n,0.5,0.1,1); 15 | R = sprandsym(m,0.5,0.5,1); 16 | 17 | Ndeg = 5; % number between 2 and degree 18 | N = cell(1,Ndeg); 19 | for d=2:Ndeg 20 | N{d} = rand(n,n^d); 21 | end 22 | 23 | % 24 | %% Compare with the Nonlinear Systems Toolbox 25 | [k,v] = pqr(A,B,Q,R,N,degree); 26 | runNSTcomparisons 27 | 28 | % 29 | %% Compare the new version of pqr with the previous version 30 | % degree = min(degree,5); % older versions of pqr are limited to degree=5 31 | % 32 | % [kOld,vOld] = pqrOld(A,B,Q,R,N,degree); 33 | % 34 | % for d=1:degree 35 | % fprintf('Degree %d: kError=%g, vError=%g\n',... 36 | % d,norm(k{d}-kOld{d}),norm(v{d+1}-vOld{d+1})); 37 | % end 38 | -------------------------------------------------------------------------------- /testScripts/testKron2CT.m: -------------------------------------------------------------------------------- 1 | %testKron2CT A script to help test the Kron2CT function using symbolic 2 | % expressions. This is very expensive for values of n and d larger than 3 | % 4~5. 4 | % 5 | % Author: Jeff Borggaard, Virginia Tech 6 | % 7 | % Part of the QQR library. 8 | 9 | n = 4; 10 | d = 7; 11 | 12 | x = sym('x',[n,1]); 13 | kk = x; 14 | for i=2:d 15 | kk = kron(kk,x); 16 | end 17 | 18 | S = Kron2CT(n,d); 19 | v = S*kk; 20 | 21 | test1 = (simplify( sum(v)-sum(kk) )); 22 | 23 | g = sum(v-v); 24 | if ( isequal(test1,g) ) 25 | fprintf('testKron2CT: passed for order %d and degree %d\n\n',n,d); 26 | else 27 | fprintf('testKron2CT: failed for order %d and degree %d\n\n',n,d); 28 | end 29 | 30 | % s{1} = 'ijjjk'; 31 | % s{2} = 'ijjkj'; 32 | % s{3} = 'ijkjj'; 33 | % s{4} = 'ikjjj'; 34 | % s{5} = 'kijjj'; 35 | % for j=1:5 36 | % for i=1:5 37 | % b(i+(j-1)*5,:) = [s{j}(i+1:5) s{j}(1:i)]; 38 | % end 39 | % end 40 | % b=unique(b,'rows'); 41 | % disp(b) 42 | 43 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/openLoop.m: -------------------------------------------------------------------------------- 1 | % Compute Chafee-Infante equation matrices and perform open loop simulation 2 | % 3 | % This model is very sensitive to numerical discretization choices. The 4 | % parameters here when used in the Matlab PDE toolbox produce similar 5 | % solutions. (to tInfinity=.2 shows it nicely) 6 | 7 | n = 41; 8 | m = 1; % a control input that we won't use in this verification example 9 | 10 | tInfinity = .2; 11 | a = 1e-3; % scaling parameter on initial conditions 12 | 13 | [E,A,B,B2,N3,Q,zInit] = ChafeeInfanteFEMControl(n,m); 14 | xNodes = linspace(0,1,length(zInit)); 15 | 16 | options = odeset('Mass',E,'reltol',1e-6); % very sensitive to this value 17 | 18 | %zInit = ones(size(zInit)); % validate against an ODE solution 19 | zdot = @(t,z) A*z + N3*kron(z,kron(z,z)); 20 | [T,Z] = ode15s(zdot,[0 tInfinity],a*zInit,options); 21 | figure(1) 22 | mesh(xNodes,T,Z) 23 | xlabel('x'); ylabel('time') 24 | title('Open Loop Simulation') 25 | 26 | 27 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_f_int.m: -------------------------------------------------------------------------------- 1 | function F = oned_f_int( Ff, test, w_g ) 2 | %----------------------------------------------------------------------- 3 | % oned_f_int.m - routine to compute \int{ f*test } 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: F = oned_f_int( Ff, test, w_g ) 9 | % 10 | % Variables: Ff 11 | % Function values at the quadrature points 12 | % 13 | % test 14 | % matrix of test functions evaluated at the 15 | % quadrature points (dim: n_quadrature, n_dof) 16 | % 17 | % w_g 18 | % Column vector of quadrature weights 19 | %----------------------------------------------------------------------- 20 | % original 21 | % [n_quadrature,n_dof] = size(test); 22 | 23 | % F = zeros(n_dof,1); 24 | % for j=1:n_dof 25 | % F(j) = test(:,j)' * ( w_g .* Ff ); 26 | % end 27 | 28 | % Vectorized version is more efficient (even for small vector lengths) 29 | F = test'*(w_g.*Ff); 30 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/oned_f_int.m: -------------------------------------------------------------------------------- 1 | function F = oned_f_int( Ff, test, w_g ) 2 | %----------------------------------------------------------------------- 3 | % oned_f_int.m - routine to compute \int{ f*test } 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: F = oned_f_int( Ff, test, w_g ) 9 | % 10 | % Variables: Ff 11 | % Function values at the quadrature points 12 | % 13 | % test 14 | % matrix of test functions evaluated at the 15 | % quadrature points (dim: n_quadrature, n_dof) 16 | % 17 | % w_g 18 | % Column vector of quadrature weights 19 | %----------------------------------------------------------------------- 20 | % original 21 | % [n_quadrature,n_dof] = size(test); 22 | 23 | % F = zeros(n_dof,1); 24 | % for j=1:n_dof 25 | % F(j) = test(:,j)' * ( w_g .* Ff ); 26 | % end 27 | 28 | % Vectorized version is more efficient (even for small vector lengths) 29 | F = test'*(w_g.*Ff); 30 | 31 | end 32 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/openLoop.m: -------------------------------------------------------------------------------- 1 | % Compute Kuramoto-Sivashinsky equation matrices and open loop simulation 2 | % 3 | % This model is very sensitive to numerical discretization choices. The 4 | % parameters here when used in the Matlab PDE toolbox produce similar 5 | % solutions. 6 | 7 | n = 21; 8 | m = 1; % a control input that we won't use in this verification example 9 | 10 | tInfinity = 150; 11 | L=13.5; 12 | [E,A,B,N2,Q,zInit] = KuramotoSivashinskyFEMControl(n,m,1/L^2); 13 | xNodes = linspace(0,1,n+1); 14 | 15 | % zInit(1:2:end) = L*sin(4*pi*xNodes(1:end-1)); 16 | % zInit(2:2:end) = L*4*pi*cos(4*pi*xNodes(1:end-1)); 17 | options = odeset('Mass',E,'reltol',1e-5); 18 | 19 | zdot = @(t,z) A*z + N2*kron(z,z); 20 | % zInit(1:2:end-1) = 11*sin(4*pi*xNodes(1:end-1)); 21 | % zInit(2:2:end ) = 44*pi*cos(4*pi*xNodes(1:end-1)); 22 | [T,Z] = ode23s(zdot,[0 tInfinity],zInit,options); 23 | figure(10) 24 | Zval = Z(:,1:2:end); Zval(:,end+1) = Zval(:,1); 25 | mesh(xNodes,T,Zval) 26 | xlabel('x'); ylabel('time') 27 | title('Open Loop Simulation') 28 | 29 | 30 | -------------------------------------------------------------------------------- /examples/Burgers1DControl/README.md: -------------------------------------------------------------------------------- 1 | Burgers1DControl contains files that are used to provide an autonomous quadratic control system by discretizing 2 | a distributed parameter control problem with finite elements. The order of the system (n), the number of control inputs (m), and the number of controlled outputs (p) can be specified. These correspond to the number of finite elements and the number of distributed source 3 | functions over the domain. 4 | 5 | The mathematical description of the problem is 6 | 7 | $\dot{z} = \epsilon z_{\xi\xi} - z z_\xi + \sum_{k=1}^m \chi_{[(k-1)/m,k/m]} u_k(t)$ 8 | 9 | for $\xi\in H_{periodic}^1(0,1)$ and $t>0$. The equations are simulated from the initial condition 10 | 11 | $z(0,\xi) = piecewise(sin(2\pi\xi)/2,0\leq\xi\leq 0.5, 0,0.5<\xi<1).$ 12 | 13 | As controlled outputs, we consider 14 | 15 | $y_i(t) = \int_0^1 \chi_{[(i-1)/p,i/p]}(\xi) z(t,\xi) d\xi, \quad i=1,2,\ldots,p$ 16 | 17 | When discretized with linear finite elements, the system has the form 18 | 19 | $E \dot{x} = A x + N kron(x,x) + B u, x(0)=x0.$ 20 | 21 | $y = C x$ 22 | 23 | The initial conditions are determined by Galerkin projection. 24 | 25 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Jeff Borggaard 4 | 5 | Any publications of research utilizing this software should please reference 6 | the website: https://github.com/jborggaard/QQR.git 7 | 8 | Permission is hereby granted, free of charge, to any person obtaining a copy 9 | of this software and associated documentation files (the "Software"), to deal 10 | in the Software without restriction, including without limitation the rights 11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | copies of the Software, and to permit persons to whom the Software is 13 | furnished to do so, subject to the following conditions: 14 | 15 | The above copyright notice and this permission notice shall be included in all 16 | copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | SOFTWARE. 25 | -------------------------------------------------------------------------------- /examples/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Jeff Borggaard 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | Any publications of research utilizing this software should please reference 13 | the website: https://github.com/jborggaard/QQR.git 14 | 15 | The above copyright notice and this permission notice shall be included in all 16 | copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | SOFTWARE. 25 | -------------------------------------------------------------------------------- /runNST3.m: -------------------------------------------------------------------------------- 1 | function [ka,py] = runNST3(A,B,Q,R,N2,N3,degree,Nxu) 2 | %runNST Solves the QQR problem within the Nonlinear Systems Toolbox for 3 | % development and comparisons. This is set up for either quadratic 4 | % or cubic nonlinearities and also handles the bilinear case if 5 | % Nxu is provided. 6 | 7 | % adjust the file setNSTpath.m to contain the path where you installed nst15 8 | setNSTpath 9 | 10 | n = size(A,1); 11 | m = size(B,2); 12 | 13 | tic 14 | x=sym('x',[n,1]); % state variables 15 | u=sym('u',[m,1]); % control variables 16 | f = A*x + B*u + N2*kron(x,x) + N3*kron(kron(x,x),x); 17 | 18 | % check for the bilinear case 19 | if ( nargin==8 ) 20 | f = f + Nxu*kron(x,u); 21 | end 22 | 23 | x0 = zeros(n,1); u0 = zeros(m,1); 24 | % control Lagrangian 25 | l=0.5*( x.'*Q*x + u.'*R*u ); % this must be scaled by 0.5 to compensate 26 | % for an internal calculation that doubles the 27 | % values of q and r. 28 | % l=( x.'*Q*x + u.'*R*u ); 29 | [ff,ll]=hjb_set_up(f,l,x,u,x0,u0,n,m,degree); 30 | set_up=toc; 31 | 32 | % call hjb.m to find the Taylor polynomial py of the optimal cost 33 | % to degree d+1 and the Taylor polynomial ka of the optimal feedback 34 | % to degree d. 35 | 36 | tic 37 | [ka,fk,py,lk]= hjb(ff,ll,n,m,degree); 38 | comp=toc; 39 | 40 | fprintf(' NST solution required %g (%g) seconds\n\n',... 41 | comp,comp+set_up); 42 | end 43 | 44 | -------------------------------------------------------------------------------- /examples/example02.m: -------------------------------------------------------------------------------- 1 | function [A,B,Q,R,N,zInit] = example02(n,m,epsilon,alpha) 2 | %EXAMPLE02 Compares different degree feedback strategies for the 3 | % discretized Burgers equation. The conversion to an ODE is non-optimal 4 | % but is below as performed in the ACC submission of 27 Sept 2019. 5 | % 6 | 7 | % n is the order of the state space 8 | % m is the number of equally spaced control inputs 9 | 10 | addpath('./examples/Burgers1DControl') 11 | 12 | if ( ~exist('epsilon','var') ) 13 | epsilon = 0.001; % set the viscosity parameter which controls the relative 14 | % importance of the nonlinear term 15 | end 16 | 17 | if ( ~exist('alpha','var') ) 18 | alpha = 0; % a linear reaction term 19 | end 20 | 21 | % x0 = zeros(n,1); u0 = zeros(m,1); 22 | 23 | % Get FEM model of Burgers equation on a periodic domain. The model 24 | % consists of matrices of the form 25 | % 26 | % M*\dot{z} = A*z + B*u + N*kron(z,z), 27 | 28 | [M,A,B,~,N,zInit] = BurgersFEMControl(n,m,1); 29 | 30 | % write the terms in the form \dot{x} = Ax+Bu+Nu, another way is shown 31 | % in example5 that uses change of variables, e.g. y = M^(1/2)x. Better yet, 32 | % we intend to extend the software to handle "mass matrices" 33 | 34 | A = epsilon*(M\A) + alpha*eye(n); 35 | B = M\B; 36 | N = M\N; 37 | 38 | % Here we expect the objective function to be 39 | % 1/2 \int_0^\infty \| z \|^2 + \| u \|^2 dt 40 | 41 | Q = M/2; R = speye(m)/2; 42 | 43 | end -------------------------------------------------------------------------------- /runNSTl.m: -------------------------------------------------------------------------------- 1 | function [ka,py] = runNSTl(A,B,Q,R,N,degree,Nxu) 2 | %runNST Solves the QQR problem within the Nonlinear Systems Toolbox for 3 | % development and comparisons. This is set up for either quadratic 4 | % or cubic nonlinearities and also handles the bilinear case if 5 | % Nxu is provided. 6 | 7 | % adjust the file setNSTpath.m to contain the path where you installed nst15 8 | setNSTpath 9 | setKroneckerToolsPath 10 | 11 | n = size(A,1); 12 | m = size(B,2); 13 | 14 | tic 15 | x=sym('x',[n,1]); % state variables 16 | u=sym('u',[m,1]); % control variables 17 | f = A*x + B*u; 18 | for i=2:length(N) 19 | f = f + N{i}*KroneckerPower(x,i); 20 | end 21 | 22 | % check for the bilinear case 23 | if ( nargin==8 ) 24 | f = f + Nxu*kron(x,u); 25 | end 26 | 27 | x0 = zeros(n,1); u0 = zeros(m,1); 28 | % control Lagrangian 29 | l=0.5*( x.'*Q*x + u.'*R*u ); % this must be scaled by 0.5 to compensate 30 | % for an internal calculation that doubles the 31 | % values of q and r. 32 | % l=( x.'*Q*x + u.'*R*u ); 33 | [ff,ll]=hjb_set_up(f,l,x,u,x0,u0,n,m,degree); 34 | set_up=toc; 35 | 36 | % call hjb.m to find the Taylor polynomial py of the optimal cost 37 | % to degree d+1 and the Taylor polynomial ka of the optimal feedback 38 | % to degree d. 39 | 40 | tic 41 | [ka,fk,py,lk]= hjb(ff,ll,n,m,degree); 42 | comp=toc; 43 | 44 | fprintf(' NST solution required %g (%g) seconds\n\n',... 45 | comp,comp+set_up); 46 | end 47 | 48 | -------------------------------------------------------------------------------- /testScripts/testLyapProduct.m: -------------------------------------------------------------------------------- 1 | % a script to test LyapProduct 2 | %------------------------------------------------------------------------------- 3 | 4 | m = 20; n = 3; 5 | M = rand(m,n); 6 | 7 | %------------------------------------------------------------------------------- 8 | % d = 2 case 9 | %------------------------------------------------------------------------------- 10 | d = 2; 11 | A = kron(M,eye(n))+kron(eye(n),M); 12 | v = rand(n^d,1); 13 | 14 | Mv = LyapProduct(M,v,d); 15 | pError = norm( A*v - Mv); 16 | if ( pError < n*m*eps ) 17 | fprintf('LyapProduct: test d=2 passed: error is %g\n',pError); 18 | end 19 | 20 | 21 | %------------------------------------------------------------------------------- 22 | % d = 3 case 23 | %------------------------------------------------------------------------------- 24 | d = 3; 25 | A = kron(M,eye(n^2)) + kron(eye(n),kron(M,eye(n))) + kron(eye(n^2),M); 26 | v = rand(n^d,1); 27 | 28 | Mv = LyapProduct(M,v,d); 29 | 30 | pError = norm( A*v - Mv); 31 | if ( pError < n^2*m*eps ) 32 | fprintf('LyapProduct: test d=3 passed: error is %g\n',pError); 33 | end 34 | 35 | 36 | 37 | %------------------------------------------------------------------------------- 38 | % d = 4 case 39 | %------------------------------------------------------------------------------- 40 | d = 4; 41 | A = kron( M,eye(n^3) ) + ... 42 | kron(eye(n ),kron(M,eye(n^2))) + ... 43 | kron(eye(n^2),kron(M,eye(n ))) + ... 44 | kron(eye(n^3), M ); 45 | v = rand(n^d,1); 46 | 47 | Mv = LyapProduct(M,v,d); 48 | 49 | pError = norm( A*v - Mv); 50 | if ( pError < n^d*m*eps ) 51 | fprintf('LyapProduct: test d=4 passed: error is %g\n',pError); 52 | end 53 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_bilinear.m: -------------------------------------------------------------------------------- 1 | function M = oned_bilinear( kernel, phi, test, w_g ) 2 | %----------------------------------------------------------------------- 3 | % oned_bilinear.m - routine to compute \int{ kernel*phi*test } 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: M = oned_bilinear(kernel, phi, test, w_g) 9 | % 10 | % Variables: kernel 11 | % Kernel function in the integral evaluated 12 | % at the quadrature points 13 | % 14 | % phi 15 | % matrix of element test functions evaluated 16 | % at the quadrature points (dim: n_quadrature, n_dof) 17 | % 18 | % test 19 | % matrix of test functions evaluated at the 20 | % quadrature points (dim: n_quadrature, n_test) 21 | % 22 | % w_g 23 | % Column vector of quadrature weights 24 | %----------------------------------------------------------------------- 25 | % test this 26 | % [n_quadrature,n_test] = size(test); 27 | % [n_quadrature,n_dof ] = size(phi); 28 | 29 | % M = zeros(n_test,n_dof); 30 | % for i=1:n_test 31 | % for j=1:n_dof 32 | % M(i,j) = ( kernel' .* test(:,i)' )*( phi(:,j) .* w_g ); 33 | % end 34 | % end 35 | 36 | % then test this 37 | % [n_quadrature,n_test] = size(test); 38 | % [n_quadrature,n_dof ] = size(phi); 39 | 40 | % M = zeros(n_test,n_dof); 41 | % for i=1:n_test 42 | % for j=1:n_dof 43 | % M(i,j) = test(:,i)'*( phi(:,j) .* kernel .* w_g ); 44 | % end 45 | % end 46 | 47 | % finally, try this 48 | % M = test' * ( phi .* kernel .* w_g ); 49 | 50 | % Vectorized version is more efficient (even for small vector lengths) 51 | M = test'*diag(kernel.*w_g)*phi; 52 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/oned_bilinear.m: -------------------------------------------------------------------------------- 1 | function M = oned_bilinear( kernel, phi, test, w_g ) 2 | %----------------------------------------------------------------------- 3 | % oned_bilinear.m - routine to compute \int{ kernel*phi*test } 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: M = oned_bilinear(kernel, phi, test, w_g) 9 | % 10 | % Variables: kernel 11 | % Kernel function in the integral evaluated 12 | % at the quadrature points 13 | % 14 | % phi 15 | % matrix of element test functions evaluated 16 | % at the quadrature points (dim: n_quadrature, n_dof) 17 | % 18 | % test 19 | % matrix of test functions evaluated at the 20 | % quadrature points (dim: n_quadrature, n_test) 21 | % 22 | % w_g 23 | % Column vector of quadrature weights 24 | %----------------------------------------------------------------------- 25 | % test this 26 | % [n_quadrature,n_test] = size(test); 27 | % [n_quadrature,n_dof ] = size(phi); 28 | 29 | % M = zeros(n_test,n_dof); 30 | % for i=1:n_test 31 | % for j=1:n_dof 32 | % M(i,j) = ( kernel' .* test(:,i)' )*( phi(:,j) .* w_g ); 33 | % end 34 | % end 35 | 36 | % then test this 37 | % [n_quadrature,n_test] = size(test); 38 | % [n_quadrature,n_dof ] = size(phi); 39 | 40 | % M = zeros(n_test,n_dof); 41 | % for i=1:n_test 42 | % for j=1:n_dof 43 | % M(i,j) = test(:,i)'*( phi(:,j) .* kernel .* w_g ); 44 | % end 45 | % end 46 | 47 | % finally, try this 48 | % M = test' * ( phi .* kernel .* w_g ); 49 | 50 | % Vectorized version is more efficient (even for small vector lengths) 51 | M = test'*diag(kernel.*w_g)*phi; 52 | 53 | end -------------------------------------------------------------------------------- /runNST.m: -------------------------------------------------------------------------------- 1 | function [ka,py] = runNST(A,B,Q,R,N,degree,Nxu,Nuu) 2 | %runNST Solves the QQR problem within the Nonlinear Systems Toolbox for 3 | % development and comparisons. This is set up for either quadratic 4 | % or cubic nonlinearities and also handles the bilinear case if 5 | % Nxu is provided. 6 | 7 | % adjust the file setNSTpath.m to contain the path where you installed nst15 8 | setNSTpath 9 | 10 | n = size(A,1); 11 | m = size(B,2); 12 | 13 | tic 14 | x=sym('x',[n,1]); % state variables 15 | u=sym('u',[m,1]); % control variables 16 | f = A*x + B*u; 17 | if ( iscell(N) ) 18 | xx = x; 19 | for j=2:length(N) 20 | xx = kron(xx,x); 21 | f = f + N{j}*xx; 22 | end 23 | else 24 | f = A*x + B*u + N*kron(x,x); 25 | end 26 | 27 | % check for the bilinear case 28 | if ( nargin>6 ) 29 | if (iscell(Nxu)) 30 | xu = u; 31 | for i=1:length(Nxu) 32 | xu = kron(x,xu); 33 | f = f + Nxu{i}*xu; 34 | end 35 | else 36 | f = f + Nxu*kron(x,u); 37 | end 38 | end 39 | 40 | if ( nargin>7 ) 41 | f = f + Nuu*kron(u,u); 42 | end 43 | 44 | x0 = zeros(n,1); u0 = zeros(m,1); 45 | % control Lagrangian 46 | l=0.5*( x.'*Q*x + u.'*R*u ); % this must be scaled by 0.5 to compensate 47 | % for an internal calculation that doubles the 48 | % values of q and r. 49 | % l=( x.'*Q*x + u.'*R*u ); 50 | [ff,ll]=hjb_set_up(f,l,x,u,x0,u0,n,m,degree); 51 | set_up=toc; 52 | 53 | % call hjb.m to find the Taylor polynomial py of the optimal cost 54 | % to degree d+1 and the Taylor polynomial ka of the optimal feedback 55 | % to degree d. 56 | 57 | tic 58 | [ka,fk,py,lk]= hjb(ff,ll,n,m,degree); 59 | comp=toc; 60 | 61 | fprintf(' NST solution required %g (%g) seconds\n\n',... 62 | comp,comp+set_up); 63 | end 64 | 65 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_plot_hermite.m: -------------------------------------------------------------------------------- 1 | function [] = oned_plot_hermite(fig_num,x,e_conn,w,wp,plt_str) 2 | %%------------------------------------------------------------------------------ 3 | % oned_plot_hermite - Plots oned Hermite cubic functions 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: oned_plot_hermite(fig_number,x,e_conn,val,der,plot_string) 9 | % 10 | % Variables: fig_number 11 | % figure number 12 | % x 13 | % nodal coordinates 14 | % e_conn 15 | % element connectivity 16 | % val 17 | % values of the function at the nodes 18 | % der 19 | % derivatives of the function at the nodes 20 | % plot_string 21 | % (optional) additional arguments to the plot command 22 | %% ----------------------------------------------------------------------------- 23 | if (nargin<6) 24 | plt_str='k'; 25 | end 26 | 27 | w = w(:); 28 | wp = wp(:); 29 | figure(fig_num) 30 | hold on 31 | [n_elements, tmp] = size(e_conn); 32 | 33 | r = linspace(-1,1,11); wt = zeros(size(r)); 34 | for n_el=1:n_elements 35 | nodes_local = e_conn(n_el,:); 36 | x_local = x(nodes_local,:); 37 | [x_g,w_g,phi0,phi1,p0_x,p1_x,p0_xx,p1_xx] = ... 38 | oned_shapeherm(x_local,r,wt); 39 | 40 | w_plot = phi0*w(nodes_local) + phi1*wp(nodes_local); 41 | plot(x_g,w_plot,plt_str) 42 | end 43 | hold off 44 | % up_plot = p0_x*u(nodes_local)' + p1_x*up(nodes_local)'; 45 | % plot(x_g,up_plot,'r') 46 | % 47 | % upp_plot = p0_xx*u(nodes_local)' + p1_xx*up(nodes_local)'; 48 | % plot(x_g,upp_plot,'g') 49 | 50 | end % function plot_hermite 51 | -------------------------------------------------------------------------------- /testBilinear.m: -------------------------------------------------------------------------------- 1 | % A Matlab script to test the bilinear formulation on simple examples by 2 | % comparing to NST results. 3 | 4 | setNSTpath 5 | setKroneckerToolsPath 6 | 7 | 8 | degree=5; 9 | 10 | testcase = 2; 11 | 12 | switch(testcase) 13 | 14 | case 1 15 | % Case 1: Build a small bilinear example 16 | % make the linear part controllable 17 | n=3; m=2; 18 | 19 | A = -eye(n); 20 | B = [1 0;0 2;3 1]; 21 | 22 | Q = 0.5*eye(n); 23 | R = 0.5*eye(m); 24 | 25 | % build nonlinear terms, then symmetrize those 26 | Nxx = [0 0 0 0 0 1/2 0 1/2 0; 0 1/2 0 1/2 0 0 0 0 0;0 0 0 0 0 0 0 0 0]; 27 | Nxu = [1 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 1]; 28 | Nuu = [0 0 0 0;0 1/2 1/2 0;0 0 0 0]; 29 | 30 | case 2 31 | % Case 2: generate a random test problem 32 | n=6; m=4; 33 | rng(0,'v5uniform') % set random number generator for reproducable tests. 34 | 35 | A = -full( sprandsym(n,0.3,rand(n,1)) ); 36 | B = rand(n,m); 37 | 38 | clear Nxx Nxu 39 | Nxx{2} = rand(n,n^2); Nxx{2} = kronMatrixSymmetrize(Nxx{2},n,2); 40 | Nxx{3} = rand(n,n^3); Nxx{3} = kronMatrixSymmetrize(Nxx{3},n,3); 41 | Nxu{1} = rand(n,n *m); 42 | Nxu{2} = rand(n,n^2*m); Nxu{2} = kronNxuSymmetrize(Nxu{2},n,2); 43 | Nxu{3} = rand(n,n^3*m); Nxu{3} = kronNxuSymmetrize(Nxu{3},n,3); 44 | Nxu{4} = rand(n,n^4*m); Nxu{4} = kronNxuSymmetrize(Nxu{4},n,3); 45 | Nuu = rand(n,m*m); Nuu = kronMatrixSymmetrize(Nuu,m,2); 46 | 47 | Q = full( sprandsym(n,0.5,rand(n,1)) ); 48 | R = full( sprandsym(m,0.5,rand(m,1)) ); 49 | 50 | otherwise 51 | end 52 | 53 | % 54 | %% Solve the current example using qqrBilinear 55 | tic 56 | [k,v] = pqrBilinear(A,B,Q,R,Nxx,Nxu,Nuu,degree); 57 | tpqr = toc; 58 | fprintf('The time required for pqrBilinear is: %g\n',tpqr) 59 | 60 | % 61 | %% Compare the solutions 62 | % Solve for the feedback and value function approximations with NST, 63 | % build matrices to convert Kronecker coefficients to compact Taylor format 64 | % to represent everything on the same monomial terms, then report 65 | % differences in the solutions. 66 | runNSTcomparisons 67 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # QQR 2 | Software to approximately solve the quadratic-quadratic regulator problem. The description of the algorithm is given in the paper 3 | 4 | - *The Quadratic-Quadratic Regulator Problem: Approximating feedback controls for quadratic-in-state nonlinear systems, submitted.* 5 | 6 | by Jeff Borggaard and Lizette Zietsman (full references included below) 7 | 8 | For installation and instructions on how to run qqr, please see the README.md 9 | file in the parent directory. 10 | 11 | ## Description of Examples 12 | #### example01 13 | 14 | In _example01.m_: Solves the control problem for randomly generated systems. Some systems that are randomly generated are nearly uncontrollable, yet others have coefficients that are close to zero. In either of these cases, the relative errors could be very large even though the code is running correctly. 15 | 16 | #### example02 17 | 18 | In _example02.m_: Solves a control problem using a discretization of the 1-dimensional Burgers equation (found in the Burgers1DControl directory). The control inputs are spatially distributed uniform sources. 19 | 20 | #### example03 21 | 22 | In _example03.m_: Similar to example1.m, except we force A to be negative-definite, symmetric. It should produce controllable systems and unlikely to have zero coefficients. 23 | 24 | #### example04 25 | 26 | In _example04.m_: Compare feedback strategies for the Lorenz system. 27 | 28 | #### example05 29 | 30 | In _example05.m_: Similar to example2.m, except we consider a linear reaction term and use a better change-of-variables to convert the discretized system to an explicit system of controlled differential equations. 31 | 32 | #### example06 33 | 34 | In _example06.m_: A simple first-order system where we can investigate convergence of the value function (by plotting it). 35 | 36 | #### References 37 | ``` 38 | @misc{borggaard2019quadraticquadratic, 39 | title={The Quadratic-Quadratic Regulator Problem: 40 | Approximating feedback controls for quadratic-in-state nonlinear systems}, 41 | author={Jeff Borggaard and Lizette Zietsman}, 42 | month={10}, 43 | year={2019}, 44 | eprint={1910.03396}, 45 | archivePrefix={arXiv}, 46 | primaryClass={math.OC} 47 | } 48 | ``` 49 | -------------------------------------------------------------------------------- /examplesForACC.m: -------------------------------------------------------------------------------- 1 | % This is the script used to run the testcases comparing 2 | % NST, qqr and the full Kronecker form in AlbrekhtKronQQR that were reported 3 | % in 4 | % 5 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator: 6 | % Proc. American Conference on Control, Denver, CO, 2020 (submitted). 7 | % 8 | % Testcases 1 and 2 utilize are set up to provide tables for comparison 9 | % see the comments to determine the required values of n, m, and degree. 10 | % 11 | % if testNST==true 12 | % - solutions from NST are provided in the ka and py arrays. 13 | % 14 | % if testAlbrekhtQQR==true 15 | % - solutions from qqr are provided in the kk and vv arrays. 16 | % 17 | % if testAlbrekhtKronQQR==true 18 | % - solutions from AlbrekhtKronQQR are provided in the k and v arrays. 19 | % - this can require a lot of memory and CPU time, so keep n, m, and the 20 | % degree variables small. 21 | % 22 | % Part of the QQR library. 23 | %% 24 | setKroneckerToolsPath 25 | 26 | % Set up test examples, problem dimensions (order), and degree of feedback 27 | addpath('./examples') % location of example problems 28 | addpath('./testScripts') 29 | 30 | testcase = 2; 31 | 32 | n = 12; % state dimension 33 | m = 2; % control dimension 34 | degree = 5; % degree of optimal feedback 35 | 36 | % Flag those methods used for the current test (NST is reqd for error tables) 37 | testNST = false; 38 | testFull = false; 39 | 40 | if ( testcase==1 ) 41 | %% 42 | % For the ACC submission, we chose n=6:2:20, m=1, degree=2:4 43 | % the full Kronecker solution wasn't calculated for 16:2:20 44 | [A,B,Q,R,N] = example01(n,m); 45 | 46 | elseif ( testcase==2 ) 47 | %% 48 | % For the ACC submission/final, we chose n=10:2:20, m=2, degree=2:3 49 | % the full Kronecker solution wasn't calculated for 16:2:20 50 | [A,B,Q,R,N,zInit] = example02(n,m); 51 | 52 | elseif ( testcase==3 ) 53 | %% 54 | % For the ACC final version, we chose n=6:2:20, m=1, degree=2:4 55 | % the full Kronecker solution wasn't calculated for 16:2:20 56 | [A,B,Q,R,N] = example03(n,m); 57 | 58 | end 59 | 60 | tic 61 | [k,v] = qqr(A,B,Q,R,N,degree); 62 | compQQR = toc; 63 | 64 | fprintf('\n'); 65 | fprintf(' qqr solution required %g seconds\n\n',compQQR); 66 | 67 | if ( testNST ) 68 | runNSTcomparisons 69 | end 70 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_mesh.m: -------------------------------------------------------------------------------- 1 | function [x,e_conn,index_u,index_c] = oned_mesh(xb,e_connb,rho) 2 | %%---------------------------------------------------------------------- 3 | % oned_mesh - Generate a mesh with a prescribed density. 4 | % This routine returns elements of the same type as 5 | % xb, e_connb (linear or quadratic) 6 | % 7 | % Copyright (c) 2001, Jeff Borggaard, Virginia Tech 8 | % Version: 1.0a 9 | % 10 | % Usage: [x,e_conn,index_u,index_c] = oned_mesh(xb,e_connb,rho) 11 | % 12 | % Variables: xb 13 | % nodal coordinates for a background mesh 14 | % e_connb 15 | % connectivity for a background mesh 16 | % rho 17 | % a mesh density function 18 | % (assumed piecewise constant for now) 19 | % 20 | % x 21 | % node coordinates of adapted mesh 22 | % e_conn 23 | % element connectivity of adapted mesh 24 | % index_u 25 | % node numbers of unknowns 26 | % index_c 27 | % node numbers of controls 28 | %% --------------------------------------------------------------------- 29 | 30 | dim = size(e_connb,2); 31 | dim = dim - 1; 32 | 33 | rho = rho(:); % make rho a column ;^) 34 | 35 | % make sure the number of elements is integral 36 | int_rho = ( xb(e_connb(:,end),1)-xb(e_connb(:,1),1) )'*rho; 37 | new_elem = ceil(int_rho); 38 | rho = new_elem*rho/int_rho; 39 | 40 | x_front = xb(1,1); 41 | int_rho = 0; 42 | bg_elem = 0; 43 | 44 | for k=1:new_elem 45 | if (dim == 1) 46 | e_conn(k,:) = [k, k+1]; 47 | elseif (dim == 2) 48 | e_conn(k,:) = [2*k-1, 2*k, 2*k+1]; 49 | elseif (dim == 3) 50 | e_conn(k,:) = [3*k-2, 3*k-1, 3*k, 3*k+1]; 51 | end 52 | 53 | while (int_rho<1-sqrt(eps)) 54 | bg_elem = bg_elem + 1; 55 | eint_rho = ( xb(e_connb(bg_elem,end),1)-xb(e_connb(bg_elem,1),1) )*... 56 | rho(bg_elem); 57 | int_rho = int_rho + eint_rho; 58 | end 59 | 60 | % the new endpoint is in the current background element 61 | x_t = max(x_front,xb(e_connb(bg_elem,1))); 62 | int_rho = int_rho-1; 63 | 64 | x_right = x_t + min(1,eint_rho-int_rho)/eint_rho*... 65 | ( xb(e_connb(bg_elem,end),1)-xb(e_connb(bg_elem,1),1) ); 66 | x_nodes = linspace(x_front,x_right,dim+1); 67 | x(e_conn(k,:),1) = x_nodes'; 68 | 69 | % advance the front 70 | x_front = x_right; 71 | end 72 | 73 | [n_nodes,~] = size(x); 74 | index_u = [2:n_nodes-1]; 75 | index_c = [1, n_nodes]; 76 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/oned_mesh.m: -------------------------------------------------------------------------------- 1 | function [x,e_conn,index_u,index_c] = oned_mesh(xb,e_connb,rho) 2 | %%---------------------------------------------------------------------- 3 | % oned_mesh - Generate a mesh with a prescribed density. 4 | % This routine returns elements of the same type as 5 | % xb, e_connb (linear or quadratic) 6 | % 7 | % Copyright (c) 2001, Jeff Borggaard, Virginia Tech 8 | % Version: 1.0a 9 | % 10 | % Usage: [x,e_conn,index_u,index_c] = oned_mesh(xb,e_connb,rho) 11 | % 12 | % Variables: xb 13 | % nodal coordinates for a background mesh 14 | % e_connb 15 | % connectivity for a background mesh 16 | % rho 17 | % a mesh density function 18 | % (assumed piecewise constant for now) 19 | % 20 | % x 21 | % node coordinates of adapted mesh 22 | % e_conn 23 | % element connectivity of adapted mesh 24 | % index_u 25 | % node numbers of unknowns 26 | % index_c 27 | % node numbers of controls 28 | %% --------------------------------------------------------------------- 29 | 30 | dim = size(e_connb,2); 31 | dim = dim - 1; 32 | 33 | rho = rho(:); % make rho a column ;^) 34 | 35 | % make sure the number of elements is integral 36 | int_rho = ( xb(e_connb(:,end),1)-xb(e_connb(:,1),1) )'*rho; 37 | new_elem = ceil(int_rho); 38 | rho = new_elem*rho/int_rho; 39 | 40 | x_front = xb(1,1); 41 | int_rho = 0; 42 | bg_elem = 0; 43 | 44 | for k=1:new_elem 45 | if (dim == 1) 46 | e_conn(k,:) = [k, k+1]; 47 | elseif (dim == 2) 48 | e_conn(k,:) = [2*k-1, 2*k, 2*k+1]; 49 | elseif (dim == 3) 50 | e_conn(k,:) = [3*k-2, 3*k-1, 3*k, 3*k+1]; 51 | end 52 | 53 | while (int_rho<1-sqrt(eps)) 54 | bg_elem = bg_elem + 1; 55 | eint_rho = ( xb(e_connb(bg_elem,end),1)-xb(e_connb(bg_elem,1),1) )*... 56 | rho(bg_elem); 57 | int_rho = int_rho + eint_rho; 58 | end 59 | 60 | % the new endpoint is in the current background element 61 | x_t = max(x_front,xb(e_connb(bg_elem,1))); 62 | int_rho = int_rho-1; 63 | 64 | x_right = x_t + min(1,eint_rho-int_rho)/eint_rho*... 65 | ( xb(e_connb(bg_elem,end),1)-xb(e_connb(bg_elem,1),1) ); 66 | x_nodes = linspace(x_front,x_right,dim+1); 67 | x(e_conn(k,:),1) = x_nodes'; 68 | 69 | % advance the front 70 | x_front = x_right; 71 | end 72 | 73 | [n_nodes,~] = size(x); 74 | index_u = [2:n_nodes-1]; 75 | index_c = [1, n_nodes]; 76 | 77 | end -------------------------------------------------------------------------------- /plotLyapunov2.m: -------------------------------------------------------------------------------- 1 | function [] = plotLyapunov2(v,k,A,B,N,x1Range,x2Range,indices) 2 | %plotLyapunov2 Plots contours of the value function viewed as a Lyapunov fcn. 3 | % 4 | % This function assumes we are in the n=2 case. The variables 5 | % v - coefficients of the value function 6 | % k - coefficients of the feedback control 7 | % A,B,N coefficients of a polynomial system 8 | % 9 | % Usage 10 | % x1Range = linspace(-1,1,201); 11 | % x2Range = linspace(-1,1,201); 12 | % [] = plotLyapunov2(v,k,A,B,N,x1Range,x2Range) 13 | % 14 | % This function includes an optional argument for plotting a slice of 15 | % the Lyapunov function when n>2 (indices: size(indices)=[1,2]) 16 | % 17 | % Author: Jeff Borggaard, Virginia Tech 18 | % 19 | % Part of the QQR library. 20 | %% 21 | 22 | if (nargin<8) 23 | indices = [1 2]; 24 | end 25 | 26 | N1 = length(x1Range); 27 | N2 = length(x2Range); 28 | 29 | d = length(v); % find the degree of the Lyapunov function 30 | 31 | V = zeros(N1,N2); 32 | for n1=1:N1 33 | for n2=1:N2 34 | %V(n1,n2) = vFun(v,x1Range(n1),x2Range(n2)); 35 | [V(n1,n2),D(n1,n2)] = dFun(v,k,A,B,N,x1Range(n1),x2Range(n2),indices); 36 | end 37 | end 38 | 39 | figure 40 | v1 = -0.01; v2 = max(max(V)); 41 | contourf(x1Range,x2Range,V',linspace(v1,v2,41)) 42 | hold on 43 | contour(x1Range,x2Range,V',[0 0],'k-.','LineWidth',4) 44 | var1 = sprintf('Variable %d',indices(1)); 45 | var2 = sprintf('Variable %d',indices(2)); 46 | xlabel(var1); ylabel(var2) 47 | title('Value Function') 48 | 49 | figure 50 | d1 = min(min(D)); d2 = 0.001; 51 | contourf(x1Range,x2Range,D',linspace(d1,d2,41)) 52 | hold on 53 | contour(x1Range,x2Range,D',[0 0],'k-.','LineWidth',4) 54 | xlabel(var1); ylabel(var2) 55 | title('Derivative Along Solutions') 56 | 57 | figure 58 | surf(x1Range,x2Range,D') 59 | end 60 | 61 | function V = vFun(v,x1,x2) 62 | z = length(v); 63 | x = [x1;x2]; 64 | xp = kron(x,x); 65 | V = v{2}*xp; 66 | for d=3:z 67 | xp = kron(xp,x); 68 | V = V + v{d}*xp; 69 | end 70 | end 71 | 72 | function [V,D] = dFun(v,k,A,B,N,x1,x2,indices) 73 | d = length(v); 74 | n = size(A,1); 75 | 76 | x{1} = zeros(n,1); 77 | x{1}(indices) = [x1;x2]; 78 | for i=2:max(d,3) 79 | x{i} = kron(x{1},x{i-1}); 80 | end 81 | 82 | Kx = k{1}*x{1}; 83 | for i=2:d-1 84 | Kx = Kx + k{i}*x{i}; 85 | end 86 | 87 | if ( iscell(N) ) 88 | nd = length(N); 89 | f = A*x{1} + B*Kx; 90 | for n=2:nd 91 | f = f + N{n}*x{n}; 92 | end 93 | else 94 | f = A*x{1} + B*Kx + N*x{2}; 95 | end 96 | 97 | fx = f; 98 | V = 0; 99 | D = 0; 100 | for i=2:d 101 | V = V + v{i}*x{i}; 102 | fx = kron(fx,x{1}) + kron(x{i-1},f); 103 | D = D + v{i}*fx; 104 | end 105 | end 106 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_shapeherm.m: -------------------------------------------------------------------------------- 1 | function [x_g,w_g,phi0,phi1,p0_x,p1_x,p0_xx,p1_xx] = ... 2 | oned_shapeherm(x,r,w) 3 | %----------------------------------------------------------------------- 4 | % oned_shapeherm.m - computes test functions and derivatives on a 5 | % (C1+) Hermite element given element coordinates 6 | % and Gauss points. 7 | % 8 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 9 | % Version: 1.3 10 | % 11 | % Usage: [x_g,w_g,phi0,phi1,p0_x,p1_x,p0_xx,p1_xx] = ... 12 | % oned_shapeherm(x_local,r,w) 13 | % 14 | % Variables: x_local 15 | % Coordinates of the element nodes 16 | % r 17 | % Coordinates of Gauss points in (-1,1) 18 | % w 19 | % Gauss weights associated with r 20 | % 21 | % x_g 22 | % Coordinates of Gauss points in the element 23 | % w_g 24 | % Gauss weights scaled by the element Jacobian 25 | % phi* 26 | % Value of element shape functions at x_g 27 | % p*_x 28 | % First spatial derivatives of phi 29 | % p*_xx 30 | % Second spatial derivatives of phi 31 | %----------------------------------------------------------------------- 32 | 33 | % [n,t1] = size(x); % n = 2, representing the endpoints of the interval 34 | % t1 had better be 1, since this is ONED_shapeherm 35 | rule = length(r); % derive the order of the quadrature rule 36 | 37 | % Transform coordinates for linear elements 38 | len = ( x(2,1)-x(1,1) ); 39 | c0 = len/2; 40 | c1 = ( x(2,1)+x(1,1) )/2; 41 | 42 | x_g = c0*r + c1; 43 | 44 | phi0(:,1) = ( 1-r ).^2 .* ( .25*r + .50); 45 | phi0(:,2) = ( 1+r ).^2 .* (-.25*r + .50); 46 | 47 | phi1(:,1) = ( 1-r ).^2 .* ( .125*r + .125)*len; 48 | phi1(:,2) = ( 1+r ).^2 .* ( .125*r - .125)*len; 49 | 50 | p0_r(:,1) = ( 1-r ) .* (-.75*r - .75); 51 | p0_r(:,2) = ( 1+r ) .* (-.75*r + .75); 52 | 53 | p1_r(:,1) = ( 1-r ) .* (-.375*r - .125)*len; 54 | p1_r(:,2) = ( 1+r ) .* ( .375*r - .125)*len; 55 | 56 | dxdr = c0; 57 | djac = dxdr; 58 | drdx = 1./djac; 59 | 60 | 61 | p0_x(:,1) = p0_r(:,1).*drdx; 62 | p0_x(:,2) = p0_r(:,2).*drdx; 63 | 64 | p1_x(:,1) = p1_r(:,1).*drdx; 65 | p1_x(:,2) = p1_r(:,2).*drdx; 66 | 67 | p0_rr(:,1) = 1.5*r; 68 | p0_rr(:,2) =-1.5*r; 69 | 70 | p1_rr(:,1) = (.75*r - .25)*len; 71 | p1_rr(:,2) = (.75*r + .25)*len; 72 | 73 | p0_xx(:,1) = p0_rr(:,1)*drdx^2; 74 | p0_xx(:,2) = p0_rr(:,2)*drdx^2; 75 | 76 | p1_xx(:,1) = p1_rr(:,1)*drdx^2; 77 | p1_xx(:,2) = p1_rr(:,2)*drdx^2; 78 | 79 | w_g = djac.*w; 80 | 81 | 82 | end 83 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/oned_shape.m: -------------------------------------------------------------------------------- 1 | function [x_g,w_g,phi,p_x,p_xx] = oned_shape(x,r,w) 2 | %----------------------------------------------------------------------- 3 | % oned_shape.m - computes test functions and derivatives for a Lagrange 4 | % C0 element given element coordinates and Gauss points. 5 | % (assumes all nodes are uniformly distributed in the 6 | % element) 7 | % 8 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 9 | % Version: 1.3 10 | % 11 | % Usage: [x_g,w_g,phi,p_x,p_xx] = oned_shape(x,r,w) 12 | % 13 | % Variables: x 14 | % Coordinates of the element nodes 15 | % r 16 | % Coordinates of Gauss points in (-1,1) 17 | % w 18 | % Gauss weights associated with r 19 | % 20 | % x_g 21 | % Coordinates of Gauss points in the element 22 | % w_g 23 | % Gauss weights scaled by the element Jacobian 24 | % phi 25 | % Value of element shape functions at x_g 26 | % p_x 27 | % First spatial derivatives of phi 28 | % p_xx 29 | % Second spatial derivatives of phi 30 | %----------------------------------------------------------------------- 31 | [n,t1] = size(x); % t1, the dimension, had better be 1 32 | 33 | if (n==2) 34 | % Transform coordinates for linear elements 35 | c0 = ( x(n,1)-x(1,1) )/2; 36 | c1 = ( x(n,1)+x(1,1) )/2; 37 | 38 | x_g = c0*r + c1; 39 | 40 | % defined backwards to help Matlab create the proper sized array 41 | phi(:,2) = ( 1+r )/2; 42 | phi(:,1) = ( 1-r )/2; 43 | 44 | p_x(:,2) = .5*ones(size(r))/c0; 45 | p_x(:,1) =-p_x(:,2); 46 | 47 | djac = c0; 48 | 49 | w_g = djac*w; 50 | 51 | if (nargout == 5) 52 | p_xx = zeros(length(r),2); 53 | end 54 | 55 | elseif (n==3) 56 | % Transform coordinates for quadratic elements 57 | c0 = ( x(n,1)-x(1,1) )/2; 58 | c1 = ( x(n,1)+x(1,1) )/2; 59 | 60 | x_g = c0*r + c1; 61 | 62 | % defined backwards to help Matlab create the proper sized array 63 | phi(:,3) = .5*r.*( r+1 ); 64 | phi(:,2) =-( r+1 ).*( r-1 ); 65 | phi(:,1) = .5*r.*( r-1 ); 66 | 67 | p_x(:,3) = ( r+.5 )/c0; 68 | p_x(:,2) =-2*r/c0; 69 | p_x(:,1) = ( r-.5 )/c0; 70 | 71 | djac = c0; 72 | 73 | w_g = djac*w; 74 | 75 | if (nargout == 5) 76 | p_xx(:,3) = ones(size(r))/c0^2; 77 | p_xx(:,2) =-2*p_xx(:,3); 78 | p_xx(:,1) = p_xx(:,3); 79 | end 80 | 81 | elseif (n==4) 82 | % Transform coordinates for (nonconforming) cubic elements 83 | c0 = ( x(n,1)-x(1,1) )/2; 84 | c1 = ( x(n,1)+x(1,1) )/2; 85 | 86 | x_g = c0*r + c1; 87 | 88 | r2 = r.*r; 89 | r3 = r.*r2; 90 | 91 | % defined backwards to help Matlab create the proper sized array 92 | phi(:,4) = 9*( r3+r2-r/9-1/9 )/16; 93 | phi(:,3) =-27*( r3+r2/3-r-1/3 )/16; 94 | phi(:,2) = 27*( r3-r2/3-r+1/3 )/16; 95 | phi(:,1) =- 9*( r3-r2-r/9+1/9 )/16; 96 | 97 | p_r(:,4) = 9*( 3*r2+2*r-1/9 )/16; 98 | p_r(:,3) =-27*( 3*r2+2*r/3-1 )/16; 99 | p_r(:,2) = 27*( 3*r2-2*r/3-1 )/16; 100 | p_r(:,1) =- 9*( 3*r2-2*r-1/9 )/16; 101 | 102 | p_rr(:,4) = 9*( 6*r+2 )/16; 103 | p_rr(:,3) =-27*( 6*r+2/3 )/16; 104 | p_rr(:,2) = 27*( 6*r-2/3 )/16; 105 | p_rr(:,1) =- 9*( 6*r-2 )/16; 106 | 107 | dxdr = p_r*x(:,1); 108 | djac = dxdr; 109 | drdx = 1./djac; 110 | 111 | p_x(:,4) = p_r(:,4).*drdx; 112 | p_x(:,3) = p_r(:,3).*drdx; 113 | p_x(:,2) = p_r(:,2).*drdx; 114 | p_x(:,1) = p_r(:,1).*drdx; 115 | w_g = djac.*w; 116 | else 117 | error('Elements higher than cubic not currently supported') 118 | keyboard 119 | end 120 | 121 | end -------------------------------------------------------------------------------- /examplesForMTNS.m: -------------------------------------------------------------------------------- 1 | % This is the script used to run the testcases for qqr and NST 2 | % as well as closed-loop simulations that were reported in 3 | % 4 | % Borggaard and Zietsman, The Polynomial-Quadratic Regulator 5 | % Proc. Mathematical Theory of Networks and Systems (accepted). 6 | % 7 | % The final version utilizes _testcases_ 4, 5, and 8. 8 | % 9 | % Change the value of _testcase_ below. Values can be: 10 | % 11 | % testcase = 3 - random, stable quadratic system (verification study) 12 | % 13 | % testcase = 4 - Lorenz equations 14 | % 15 | % testcase = 5 - Burgers equation 16 | % 17 | % testcase = 8 - a ring of van der Pol oscillators 18 | % 19 | % if testNST=true 20 | % - solutions from NST are provided in the ka and py arrays for 21 | % code verification. 22 | % 23 | % Part of the QQR library @ https://github.com/jborggaard/QQR 24 | %% 25 | setKroneckerToolsPath 26 | 27 | % Set up test examples, problem dimensions (order), and degree of feedback 28 | 29 | addpath('./examples') 30 | 31 | testcase = 4; % 4, 5, and 8 (substantial CPU to run testcase 8) 32 | 33 | % Flag those methods used for the current test (NST is reqd for errors) 34 | testNST = false; 35 | testFull = false; 36 | 37 | if ( testcase==3 ) 38 | %% 39 | % To test the code, we chose n=10:2:20, m=2, degree=2:3 40 | 41 | n = 2; % state dimension 42 | m = 1; % control dimension 43 | degree = 5; % degree of optimal feedback 44 | 45 | [A,B,Q,R,N] = example03(n,m); 46 | 47 | tic 48 | [k,v] = qqr(A,B,Q,R,N,degree); 49 | compQQR = toc; 50 | 51 | fprintf('\n'); 52 | fprintf(' qqr solution required %g seconds\n\n',compQQR); 53 | 54 | elseif ( testcase==4 ) 55 | %% 56 | % For the MTNS submission, described in Section 5.1 57 | % Produces the values in Table 1 in the PQR paper. 58 | % 59 | % Test the QQR algorithm on a well-known low-dimensional problem (Lorenz) 60 | % 61 | % This example calls qqr internally and n=3,m=1 must be specified as the 62 | % problem dimensions for the runNSTcomparisons script. 63 | 64 | n = 3; % state dimension 65 | m = 1; % control dimension 66 | degree = 7; % degree of optimal feedback 67 | 68 | example04 69 | 70 | elseif ( testcase==5 ) 71 | %% 72 | % For the MTNS submission, described in Section 5.3 73 | % Produces the values in Table 5 in the PQR paper. 74 | % 75 | % A Burgers equation example with closed-loop simulations and a better 76 | % change of variable to remove the mass matrix. 77 | 78 | n = 16; % state dimension (values 16 and 20 in the paper) 79 | m = 3; % control dimension 80 | degree = 5; % degree of optimal feedback 81 | 82 | setParams = true; %#ok (mlint doesn't read scripts) 83 | example05 84 | setParams = false; 85 | 86 | testNST = false; % NST comparisons are meaningless since we've scaled 87 | % the v and k solutions. 88 | 89 | elseif ( testcase==6 ) 90 | % A one-dimensional example where we also compute the stabilizability 91 | % radius. 92 | n = 8; % state dimension 93 | m = 2; % control dimension 94 | degree = 5; % degree of optimal feedback 95 | 96 | example06 97 | 98 | elseif ( testcase==8 ) 99 | % For the MTNS submission, described in Section 5.2 100 | % Produces values reported in Tables 2-4 in the PQR paper. 101 | % 102 | % A ring of van der Pol oscillators to test feedback controls in a system 103 | % with a cubic nonlinearity. Cidx is a list of the oscillators that 104 | % are equiped with a controller. 105 | g = 4; % number of van der Pol oscillators ( n=2*g ) 106 | Cidx = [1 2]; 107 | m = length(Cidx); % control dimension 108 | degree = 7; % degree of optimal feedback 109 | 110 | example08 % table 2 111 | 112 | g = 8; 113 | Cidx = [1 2]; 114 | m = length(Cidx); 115 | degree = 5; 116 | 117 | example08a % table 3 118 | 119 | %% 120 | g = 8; 121 | % Cidx = [1 2 3 4]; % run this section over all the cases below 122 | Cidx = [1 2 3 5]; 123 | % Cidx = [1 2 3 6]; 124 | % Cidx = [1 2 4 5]; 125 | % Cidx = [1 2 4 6]; 126 | % Cidx = [1 2 4 7]; 127 | % Cidx = [1 2 5 6]; 128 | m = length(Cidx); 129 | degree = 5; 130 | 131 | example08 % table 4 132 | end 133 | 134 | 135 | if ( testNST ) 136 | runNSTcomparisons 137 | end 138 | -------------------------------------------------------------------------------- /examplesForTAC.m: -------------------------------------------------------------------------------- 1 | % This is the script used to run the testcases for qqr and NST 2 | % as well as closed-loop simulations that were reported in 3 | % 4 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator 5 | % IEEE Transactions on Automatic Control (submitted). 6 | % - testcases 3, 4, 5, and 6. 7 | % 8 | % testcase 3 - random, stable quadratic system 9 | % 10 | % testcase 4 - Lorenz equations 11 | % 12 | % testcase 5 - Burgers equation 13 | % 14 | % testcase 6 - simple first-order problem 15 | % 16 | % testcase 8 - connected van der Pol oscillators 17 | % 18 | % if testNST==true 19 | % - solutions from NST are provided in the ka and py arrays. 20 | % 21 | % if testAlbrekhtQQR==true 22 | % - solutions from qqr are provided in the kk and vv arrays. 23 | % 24 | % if testAlbrekhtKronQQR==true 25 | % - solutions from AlbrekhtKronQQR are provided in the k and v arrays. 26 | % - this can require a lot of memory and CPU time, so keep n, m, and the 27 | % degree variables small. 28 | % 29 | % Part of the QQR library. 30 | %% 31 | setKroneckerToolsPath 32 | 33 | % Set up test examples, problem dimensions (order), and degree of feedback 34 | 35 | addpath('./examples') 36 | 37 | testcase = 7; 38 | 39 | % Flag those methods used for the current test (NST is reqd for errors) 40 | testNST = false; 41 | testFull = false; 42 | 43 | if ( testcase==1 ) 44 | %% 45 | % For the ACC submission, we chose n=6:2:20, m=1, degree=2:4 46 | % the full Kronecker solution wasn't calculated for 16:2:20 47 | 48 | n = 6; % state dimension 49 | m = 2; % control dimension 50 | degree = 5; % degree of optimal feedback 51 | 52 | [A,B,Q,R,N] = example01(n,m); 53 | 54 | tic 55 | [k,v] = qqr(A,B,Q,R,N,degree); 56 | compQQR = toc; 57 | 58 | fprintf('\n'); 59 | fprintf(' qqr solution required %g seconds\n\n',compQQR); 60 | 61 | elseif ( testcase==2 ) 62 | %% 63 | % For the ACC submission, we chose n=10:2:20, m=2, degree=2:3 64 | % the full Kronecker solution wasn't calculated for 16:2:20 65 | 66 | n = 8; % state dimension 67 | m = 2; % control dimension 68 | degree = 5; % degree of optimal feedback 69 | 70 | [A,B,Q,R,N,zInit] = example02(n,m); 71 | 72 | tic 73 | [k,v] = qqr(A,B,Q,R,N,degree); 74 | compQQR = toc; 75 | 76 | fprintf('\n'); 77 | fprintf(' qqr solution required %g seconds\n\n',compQQR); 78 | 79 | elseif ( testcase==3 ) 80 | %% 81 | % For the TAC submission, we chose n=10:2:20, m=2, degree=2:3 82 | % the full Kronecker solution wasn't calculated 83 | 84 | n = 8; % state dimension 85 | m = 2; % control dimension 86 | degree = 4; % degree of optimal feedback 87 | 88 | [A,B,Q,R,N] = example03(n,m); 89 | 90 | tic 91 | [k,v] = qqr(A,B,Q,R,N,degree); 92 | compQQR = toc; 93 | 94 | fprintf('\n'); 95 | fprintf(' qqr solution required %g seconds\n\n',compQQR); 96 | 97 | elseif ( testcase==4 ) 98 | %% 99 | % Test the QQR algorithm on a well-known low-dimensional problem (Lorenz) 100 | % 101 | % This example calls qqr internally and n=3,m=1 must be specified as the 102 | % problem dimensions for the runNSTcomparisons script. 103 | 104 | n = 3; % state dimension 105 | m = 1; % control dimension 106 | degree = 5; % degree of optimal feedback 107 | 108 | example04 109 | 110 | elseif ( testcase==5 ) 111 | %% 112 | % A Burgers equation example with closed-loop simulations and a better 113 | % change of variable to remove the mass matrix. 114 | 115 | n = 8; % state dimension 116 | m = 2; % control dimension 117 | degree = 5; % degree of optimal feedback 118 | 119 | example05 120 | 121 | elseif ( testcase==6 ) 122 | % A one-dimensional example where we also compute the stabilizability 123 | % radius. 124 | n = 8; % state dimension 125 | m = 2; % control dimension 126 | degree = 5; % degree of optimal feedback 127 | 128 | example06 129 | 130 | elseif ( testcase==7 ) 131 | 132 | example07 133 | 134 | elseif ( testcase==8 ) 135 | % A ring of van der Pol oscillators to test feedback controls in a system 136 | % with a cubic nonlinearity. 137 | No = 4; % number of van der Pol oscillators ( n=2*No ) 138 | m = 2; % control dimension 139 | degree = 5; % degree of optimal feedback 140 | 141 | example08 142 | 143 | elseif ( testcase==9 ) 144 | % Comparison to a published example. 145 | 146 | example09 147 | 148 | end 149 | 150 | 151 | if ( testNST ) 152 | runNSTcomparisons 153 | end 154 | -------------------------------------------------------------------------------- /examples/Burgers1DControl/hjbBurgers1D.m: -------------------------------------------------------------------------------- 1 | function [] = hjbBurgers1D(n,m,d) 2 | % Solve the quadratic-in-state control problem associated with Burgers equation 3 | % \dot{z} = \epsilon z_xx + h(x) u(t) + \frac{1}{2} ( z^2 )_x 4 | % with z=0 on boundaries. Use AlbrechtQQR to approximate the HJB solution. 5 | % 6 | addpath('../..') 7 | 8 | if ( nargin==0 ) 9 | n = 14; 10 | m = 6; % number of equally spaced control inputs 11 | p = 1; % number of controlled outputs 12 | d = 3; % degree of optimal feedback 13 | end 14 | 15 | epsilon = 0.005; 16 | 17 | tInfinity = 15; 18 | 19 | [M,A,B,~,N,zInit] = BurgersFEMControl(n,m,p); 20 | xNodes = linspace(0,1,n); 21 | 22 | save('zInit.mat','zInit') 23 | 24 | % write the quadratic term in Kronecker product form 25 | % N = zeros(n,n*n); 26 | % for i=1:n 27 | % tmp = NN(:,:,i)'; 28 | % N(i,:) = tmp(:)'; 29 | % end 30 | 31 | % The nonlinear control performs better when the initial condition is 32 | % closer to 0... 33 | zInit = 1*zInit; 34 | 35 | % % at this point, we take a shortcut and lump the mass matrix. this 36 | % % produces a diagonal matrix with values 1/n. thus, we eliminate the 37 | % % mass matrix from the equations by multiplying A, B, and N by n. 38 | % A = A*n; 39 | % B = B*n; 40 | % N = N*n; 41 | A = epsilon*(M\A); 42 | B = M\B; 43 | N = M\N; 44 | 45 | Q = M/2; R = speye(m)/2; 46 | 47 | % Simulate the open loop system and compute its optimal cost 48 | zdot = @(t,z) [ A*z(1:end-1) + N*kron(z(1:end-1),z(1:end-1));... 49 | z(1:end-1)'*Q*z(1:end-1) ]; 50 | [T,Z] = ode23(zdot,[0 tInfinity],[zInit;0]); 51 | figure(10) 52 | mesh(xNodes,T,Z(:,1:end-1)) 53 | xlabel('x'); ylabel('time') 54 | title('Open Loop Simulation') 55 | savefig(['openLoopn=',int2str(n),'.fig']); 56 | 57 | fprintf('Open Loop Cost (0,T) is %g\n\n',Z(end,end)); 58 | 59 | 60 | % Approximate the feedback using Albrecht's method. 61 | [k,v] = qqr(A,B,Q,R,N,d); 62 | 63 | if ( d>=1 ) 64 | % Simulate the closed-loop system with degree 1 feedback 65 | k1 = k{1}; 66 | F = @(z) A*z + N*kron(z,z); 67 | ell = @(z,u) z.'*Q*z + u.'*R*u; 68 | computeU1 = @(z) k1*z; 69 | zdotCL1 = @(t,z) [ F(z(1:end-1)) + B*computeU1(z(1:end-1)); ... 70 | ell(z(1:end-1), computeU1(z(1:end-1))) ]; 71 | [T,Z] = ode23(zdotCL1,[0 tInfinity],[zInit;0]); 72 | figure(1) 73 | mesh(xNodes,T,Z(:,1:end-1)) 74 | xlabel('x'); ylabel('time') 75 | titleString = sprintf('Closed Loop Simulation: Order %d',1); 76 | title(titleString) 77 | savefig(['closedLoopn=',int2str(n),'m=',int2str(m),'d=',int2str(1),'.fig']); 78 | fprintf('Order %d Closed Loop Cost (0,T) is %14.8e\n\n',1,Z(end,end)); 79 | U1 = zeros(m,length(T)); 80 | for i=1:length(T) 81 | U1(:,i) = computeU1(Z(i,1:end-1)'); 82 | end 83 | figure(81) 84 | plot(T,U1(1,:),T,U1(2,:),T,U1(3,:),T,U1(4,:),T,U1(5,:),T,U1(6,:)); 85 | for i=1:m 86 | legStr{i} = sprintf('u_{%02d}',i); 87 | end 88 | legend(legStr) 89 | title('Optimal feedback control inputs, d=1') 90 | end 91 | 92 | if ( d>=2 ) 93 | % Simulate the closed-loop system with degree 2 feedback 94 | k2 = k{2}; 95 | computeU2 = @(z) k1*z + k2*kron(z,z); 96 | 97 | zdotCL2 = @(t,z) [ F(z(1:end-1)) + B*computeU2(z(1:end-1)); ... 98 | ell(z(1:end-1), computeU2(z(1:end-1))) ]; 99 | [T,Z] = ode23(zdotCL2,[0 tInfinity],[zInit;0]); 100 | figure(2) 101 | mesh(xNodes,T,Z(:,1:end-1)) 102 | xlabel('x'); ylabel('time') 103 | titleString = sprintf('Closed Loop Simulation: Order %d',2); 104 | title(titleString) 105 | savefig(['closedLoopn=',int2str(n),'m=',int2str(m),'d=',int2str(2),'.fig']); 106 | fprintf('Order %d Closed Loop Cost (0,T) is %14.8e\n\n',2,Z(end,end)); 107 | U2 = zeros(m,length(T)); 108 | for i=1:length(T) 109 | U2(:,i) = computeU2(Z(i,1:end-1)'); 110 | end 111 | figure(82) 112 | plot(T,U2(1,:),T,U2(2,:),T,U2(3,:),T,U2(4,:),T,U2(5,:),T,U2(6,:)); 113 | legend(legStr) 114 | title('Optimal feedback control inputs, d=2') 115 | end 116 | 117 | if ( d>=3 ) 118 | % Simulate the closed-loop system with degree 3 feedback 119 | k3 = k{3}; 120 | computeU3 = @(z) k1*z + k2*kron(z,z) + k3*kron(z,kron(z,z)); 121 | 122 | zdotCL3 = @(t,z) [ F(z(1:end-1))+ B*computeU3(z(1:end-1)); ... 123 | ell(z(1:end-1), computeU3(z(1:end-1))) ]; 124 | [T,Z] = ode23(zdotCL3,[0 tInfinity],[zInit;0]); 125 | figure(3) 126 | mesh(xNodes,T,Z(:,1:end-1)) 127 | xlabel('x'); ylabel('time') 128 | titleString = sprintf('Closed Loop Simulation: Order %d',3); 129 | title(titleString) 130 | savefig(['closedLoopn=',int2str(n),'m=',int2str(m),'d=',int2str(3),'.fig']); 131 | fprintf('Order %d Closed Loop Cost (0,T) is %14.8e\n\n',3,Z(end,end)); 132 | 133 | U3 = zeros(m,length(T)); 134 | for i=1:length(T) 135 | U3(:,i) = computeU3(Z(i,1:end-1)'); 136 | end 137 | figure(83) 138 | plot(T,U3(1,:),T,U3(2,:),T,U3(3,:),T,U3(4,:),T,U3(5,:),T,U3(6,:)); 139 | legend(legStr) 140 | title('Optimal feedback control inputs, d=3') 141 | 142 | end 143 | 144 | 145 | filename = ['HJBBurgersD',num2str(d),'N',num2str(n),'M',num2str(m),'.mat']; 146 | save(filename,'v','k') 147 | 148 | 149 | end % function hjbBurgersNST 150 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/oned_quadrature.m: -------------------------------------------------------------------------------- 1 | function [r,w] = oned_quadrature(rule) 2 | %----------------------------------------------------------------------- 3 | % oned_gauss.m - calculate Gauss integration points on (-1,1) 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: [r,w] = oned_quadrature(rule) 9 | % 10 | % Variables: rule 11 | % Number of Gauss points: 12 | % r 13 | % Gauss points located between (-1,1) 14 | % w 15 | % Gauss weights corresponding to r 16 | %----------------------------------------------------------------------- 17 | 18 | r = zeros(rule,1); 19 | w = zeros(rule,1); 20 | 21 | if (rule == 1) % up to order 1 polynomials exact 22 | r(1) = 0; 23 | w(1) = 2; 24 | 25 | elseif (rule == 2) % up to order 3 polynomials exact 26 | r(1) =-1.0d0 / sqrt(3.0d0); 27 | r(2) =-r(1); 28 | w(1) = 1.0; 29 | w(2) = 1.0; 30 | 31 | elseif (rule == 3) % up to order 5 polynomials exact 32 | r(1) =-sqrt(3.0d0/5.0d0); 33 | r(2) = 0.0; 34 | r(3) =-r(1); 35 | w(1) = 5.0d0 / 9.0d0; 36 | w(2) = 8.0d0 / 9.0d0; 37 | w(3) = w(1); 38 | 39 | elseif (rule == 4) % up to order 7 polynomials exact 40 | r(1) =-sqrt((3.0d0+2.0*sqrt(6.0d0/5.0d0))/7.0d0); 41 | r(2) =-sqrt((3.0d0-2.0*sqrt(6.0d0/5.0d0))/7.0d0); 42 | r(3) =-r(2); 43 | r(4) =-r(1); 44 | w(1) = 0.5d0 - 1.0d0 / ( 6.0d0 * sqrt(6.0d0/5.0d0) ); 45 | w(2) = 0.5d0 + 1.0d0 / ( 6.0d0 * sqrt(6.0d0/5.0d0) ); 46 | w(3) = w(2); 47 | w(4) = w(1); 48 | 49 | elseif (rule == 5) % up to order 9 polynomials exact 50 | r(1) =-sqrt(5.0d0+4.0d0*sqrt(5.0d0/14.0d0)) / 3.0d0; 51 | r(2) =-sqrt(5.0d0-4.0d0*sqrt(5.0d0/14.0d0)) / 3.0d0; 52 | r(3) = 0.0d0; 53 | r(4) =-r(2); 54 | r(5) =-r(1); 55 | w(1) = 161.0d0/450.0d0-13.0d0/(180.d0*sqrt(5.0d0/14.0d0)); 56 | w(2) = 161.0d0/450.0d0+13.0d0/(180.d0*sqrt(5.0d0/14.0d0)); 57 | w(3) = 128.0d0/225.0d0; 58 | w(4) = w(2); 59 | w(5) = w(1); 60 | 61 | elseif (rule == 6) 62 | r(1) = -0.2386191860831969; 63 | r(2) = -0.6612093864662645; 64 | r(3) = -0.9324695142031521; 65 | r(4) = - r(1); 66 | r(5) = - r(2); 67 | r(6) = - r(3); 68 | w(1) = 0.4679139345726910; 69 | w(2) = 0.3607615730481386; 70 | w(3) = 0.1713244923791704; 71 | w(4) = w(1); 72 | w(5) = w(2); 73 | w(6) = w(3); 74 | 75 | elseif (rule == 7) 76 | r(1) = -0.9491079123427585; 77 | r(2) = -0.7415311855993945; 78 | r(3) = -0.4058451513773972; 79 | r(4) = 0.0000000000000000; 80 | r(5) = - r(3); 81 | r(6) = - r(2); 82 | r(7) = - r(1); 83 | w(1) = 0.1294849661688697; 84 | w(2) = 0.2797053914892766; 85 | w(3) = 0.3818300505051189; 86 | w(4) = 0.4179591836734694; 87 | w(5) = w(3); 88 | w(6) = w(2); 89 | w(7) = w(1); 90 | 91 | elseif (rule == 8) 92 | r(1) = -0.9602898564975363; 93 | r(2) = -0.7966664774136267; 94 | r(3) = -0.5255324099163290; 95 | r(4) = -0.1834346424956498; 96 | r(5) = - r(4); 97 | r(6) = - r(3); 98 | r(7) = - r(2); 99 | r(8) = - r(1); 100 | w(1) = 0.1012285362903763; 101 | w(2) = 0.2223810344533745; 102 | w(3) = 0.3137066458778873; 103 | w(4) = 0.3626837833783620; 104 | w(5) = w(4); 105 | w(6) = w(3); 106 | w(7) = w(2); 107 | w(8) = w(1); 108 | 109 | elseif (rule == 9) 110 | r(1) = -0.9681602395076261; 111 | r(2) = -0.8360311073266358; 112 | r(3) = -0.6133714327005904; 113 | r(4) = -0.3242534234038089; 114 | r(5) = 0.0000000000000000; 115 | r(6) = - r(4); 116 | r(7) = - r(3); 117 | r(8) = - r(2); 118 | r(9) = - r(1); 119 | w(1) = 0.0812743883615744; 120 | w(2) = 0.1806481606948574; 121 | w(3) = 0.2606106964029354; 122 | w(4) = 0.3123470770400029; 123 | w(5) = 0.3302393550012598; 124 | w(6) = w(4); 125 | w(7) = w(3); 126 | w(8) = w(2); 127 | w(9) = w(1); 128 | 129 | elseif (rule == 10) 130 | r( 1) = -0.9739065285171717; 131 | r( 2) = -0.8650633666889845; 132 | r( 3) = -0.6794095682990244; 133 | r( 4) = -0.4333953941292472; 134 | r( 5) = -0.1488743389816312; 135 | r( 6) = - r(5); 136 | r( 7) = - r(4); 137 | r( 8) = - r(3); 138 | r( 9) = - r(2); 139 | r(10) = - r(1); 140 | w( 1) = 0.0666713443086881; 141 | w( 2) = 0.1494513491505806; 142 | w( 3) = 0.2190863625159820; 143 | w( 4) = 0.2692667193099963; 144 | w( 5) = 0.2955242247147529; 145 | w( 6) = w(5); 146 | w( 7) = w(4); 147 | w( 8) = w(3); 148 | w( 9) = w(2); 149 | w(10) = w(1); 150 | 151 | elseif (rule == 11) 152 | r( 1) = -0.9782286581460570; 153 | r( 2) = -0.8870625997680953; 154 | r( 3) = -0.7301520055740494; 155 | r( 4) = -0.5190961292068118; 156 | r( 5) = -0.2695431559523450; 157 | r( 6) = 0.0000000000000000; 158 | r( 7) = - r(5); 159 | r( 8) = - r(4); 160 | r( 9) = - r(3); 161 | r(10) = - r(2); 162 | r(11) = - r(1); 163 | w( 1) = 0.0556685671161737; 164 | w( 2) = 0.1255803694649046; 165 | w( 3) = 0.1862902109277343; 166 | w( 4) = 0.2331937645919905; 167 | w( 5) = 0.2628045445102467; 168 | w( 6) = 0.2729250867779006; 169 | w( 7) = w(5); 170 | w( 8) = w(4); 171 | w( 9) = w(3); 172 | w(10) = w(2); 173 | w(11) = w(1); 174 | 175 | else 176 | error('Quadrature rule not supported') 177 | keyboard 178 | end 179 | 180 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/oned_quadrature.m: -------------------------------------------------------------------------------- 1 | function [r,w] = oned_quadrature(rule) 2 | %----------------------------------------------------------------------- 3 | % oned_gauss.m - calculate Gauss integration points on (-1,1) 4 | % 5 | % Copyright (c) 2013, Jeff Borggaard, Virginia Tech 6 | % Version: 1.3 7 | % 8 | % Usage: [r,w] = oned_quadrature(rule) 9 | % 10 | % Variables: rule 11 | % Number of Gauss points: 12 | % r 13 | % Gauss points located between (-1,1) 14 | % w 15 | % Gauss weights corresponding to r 16 | %----------------------------------------------------------------------- 17 | 18 | r = zeros(rule,1); 19 | w = zeros(rule,1); 20 | 21 | if (rule == 1) % up to order 1 polynomials exact 22 | r(1) = 0; 23 | w(1) = 2; 24 | 25 | elseif (rule == 2) % up to order 3 polynomials exact 26 | r(1) =-1.0d0 / sqrt(3.0d0); 27 | r(2) =-r(1); 28 | w(1) = 1.0; 29 | w(2) = 1.0; 30 | 31 | elseif (rule == 3) % up to order 5 polynomials exact 32 | r(1) =-sqrt(3.0d0/5.0d0); 33 | r(2) = 0.0; 34 | r(3) =-r(1); 35 | w(1) = 5.0d0 / 9.0d0; 36 | w(2) = 8.0d0 / 9.0d0; 37 | w(3) = w(1); 38 | 39 | elseif (rule == 4) % up to order 7 polynomials exact 40 | r(1) =-sqrt((3.0d0+2.0*sqrt(6.0d0/5.0d0))/7.0d0); 41 | r(2) =-sqrt((3.0d0-2.0*sqrt(6.0d0/5.0d0))/7.0d0); 42 | r(3) =-r(2); 43 | r(4) =-r(1); 44 | w(1) = 0.5d0 - 1.0d0 / ( 6.0d0 * sqrt(6.0d0/5.0d0) ); 45 | w(2) = 0.5d0 + 1.0d0 / ( 6.0d0 * sqrt(6.0d0/5.0d0) ); 46 | w(3) = w(2); 47 | w(4) = w(1); 48 | 49 | elseif (rule == 5) % up to order 9 polynomials exact 50 | r(1) =-sqrt(5.0d0+4.0d0*sqrt(5.0d0/14.0d0)) / 3.0d0; 51 | r(2) =-sqrt(5.0d0-4.0d0*sqrt(5.0d0/14.0d0)) / 3.0d0; 52 | r(3) = 0.0d0; 53 | r(4) =-r(2); 54 | r(5) =-r(1); 55 | w(1) = 161.0d0/450.0d0-13.0d0/(180.d0*sqrt(5.0d0/14.0d0)); 56 | w(2) = 161.0d0/450.0d0+13.0d0/(180.d0*sqrt(5.0d0/14.0d0)); 57 | w(3) = 128.0d0/225.0d0; 58 | w(4) = w(2); 59 | w(5) = w(1); 60 | 61 | elseif (rule == 6) 62 | r(1) = -0.2386191860831969; 63 | r(2) = -0.6612093864662645; 64 | r(3) = -0.9324695142031521; 65 | r(4) = - r(1); 66 | r(5) = - r(2); 67 | r(6) = - r(3); 68 | w(1) = 0.4679139345726910; 69 | w(2) = 0.3607615730481386; 70 | w(3) = 0.1713244923791704; 71 | w(4) = w(1); 72 | w(5) = w(2); 73 | w(6) = w(3); 74 | 75 | elseif (rule == 7) 76 | r(1) = -0.9491079123427585; 77 | r(2) = -0.7415311855993945; 78 | r(3) = -0.4058451513773972; 79 | r(4) = 0.0000000000000000; 80 | r(5) = - r(3); 81 | r(6) = - r(2); 82 | r(7) = - r(1); 83 | w(1) = 0.1294849661688697; 84 | w(2) = 0.2797053914892766; 85 | w(3) = 0.3818300505051189; 86 | w(4) = 0.4179591836734694; 87 | w(5) = w(3); 88 | w(6) = w(2); 89 | w(7) = w(1); 90 | 91 | elseif (rule == 8) 92 | r(1) = -0.9602898564975363; 93 | r(2) = -0.7966664774136267; 94 | r(3) = -0.5255324099163290; 95 | r(4) = -0.1834346424956498; 96 | r(5) = - r(4); 97 | r(6) = - r(3); 98 | r(7) = - r(2); 99 | r(8) = - r(1); 100 | w(1) = 0.1012285362903763; 101 | w(2) = 0.2223810344533745; 102 | w(3) = 0.3137066458778873; 103 | w(4) = 0.3626837833783620; 104 | w(5) = w(4); 105 | w(6) = w(3); 106 | w(7) = w(2); 107 | w(8) = w(1); 108 | 109 | elseif (rule == 9) 110 | r(1) = -0.9681602395076261; 111 | r(2) = -0.8360311073266358; 112 | r(3) = -0.6133714327005904; 113 | r(4) = -0.3242534234038089; 114 | r(5) = 0.0000000000000000; 115 | r(6) = - r(4); 116 | r(7) = - r(3); 117 | r(8) = - r(2); 118 | r(9) = - r(1); 119 | w(1) = 0.0812743883615744; 120 | w(2) = 0.1806481606948574; 121 | w(3) = 0.2606106964029354; 122 | w(4) = 0.3123470770400029; 123 | w(5) = 0.3302393550012598; 124 | w(6) = w(4); 125 | w(7) = w(3); 126 | w(8) = w(2); 127 | w(9) = w(1); 128 | 129 | elseif (rule == 10) 130 | r( 1) = -0.9739065285171717; 131 | r( 2) = -0.8650633666889845; 132 | r( 3) = -0.6794095682990244; 133 | r( 4) = -0.4333953941292472; 134 | r( 5) = -0.1488743389816312; 135 | r( 6) = - r(5); 136 | r( 7) = - r(4); 137 | r( 8) = - r(3); 138 | r( 9) = - r(2); 139 | r(10) = - r(1); 140 | w( 1) = 0.0666713443086881; 141 | w( 2) = 0.1494513491505806; 142 | w( 3) = 0.2190863625159820; 143 | w( 4) = 0.2692667193099963; 144 | w( 5) = 0.2955242247147529; 145 | w( 6) = w(5); 146 | w( 7) = w(4); 147 | w( 8) = w(3); 148 | w( 9) = w(2); 149 | w(10) = w(1); 150 | 151 | elseif (rule == 11) 152 | r( 1) = -0.9782286581460570; 153 | r( 2) = -0.8870625997680953; 154 | r( 3) = -0.7301520055740494; 155 | r( 4) = -0.5190961292068118; 156 | r( 5) = -0.2695431559523450; 157 | r( 6) = 0.0000000000000000; 158 | r( 7) = - r(5); 159 | r( 8) = - r(4); 160 | r( 9) = - r(3); 161 | r(10) = - r(2); 162 | r(11) = - r(1); 163 | w( 1) = 0.0556685671161737; 164 | w( 2) = 0.1255803694649046; 165 | w( 3) = 0.1862902109277343; 166 | w( 4) = 0.2331937645919905; 167 | w( 5) = 0.2628045445102467; 168 | w( 6) = 0.2729250867779006; 169 | w( 7) = w(5); 170 | w( 8) = w(4); 171 | w( 9) = w(3); 172 | w(10) = w(2); 173 | w(11) = w(1); 174 | 175 | else 176 | error('Quadrature rule not supported') 177 | keyboard 178 | end 179 | 180 | end 181 | -------------------------------------------------------------------------------- /testScripts/testAlbrekhtQQR.m: -------------------------------------------------------------------------------- 1 | % This is the script used to run the testcases for AlbrechtQQR that were 2 | % reported in 3 | % 4 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator 5 | % IEEE Transactions on Automatic Control (submitted). 6 | % - testcases 1, 3, 4, and 5 7 | % 8 | % testcases 1 and 3 provide tables for comparison 9 | % 10 | % testcases 4 and 5 provide closed-loop validation 11 | % 12 | % solutions from AlbrechtQQR are provided in the kk and vv arrays. 13 | % 14 | % if testNST==true 15 | % - solutions from NST are provided in the ka and py arrays. 16 | % 17 | % Author: Jeff Borggaard, Virginia Tech 18 | % 19 | % Part of the QQR library. 20 | %% 21 | % Set up test examples, problem dimensions (order), and degree of feedback 22 | % in this code block, then run the script. 23 | 24 | testcase=1; 25 | 26 | n = 5; % state dimension 27 | m = 3; % control dimension 28 | degree = 3; % degree of optimal feedback 29 | 30 | % Flag whether or not NST is to be computed (reqd for error calculations) 31 | testNST = true; 32 | 33 | if ( testcase==1 ) 34 | %% 35 | example01 36 | 37 | elseif ( testcase==2 ) 38 | %% 39 | % For the ACC submission, we chose n=6:2:20, m=1, degree=2:4 40 | example02 41 | 42 | elseif ( testcase==3 ) 43 | %% 44 | % For the ACC submission, we chose n=10:2:20, m=2, degree=2:3 45 | example03 46 | 47 | end 48 | 49 | %% 50 | if ( testNST ) 51 | % adjust the script below to contain the path where you install nst15 52 | setNSTpath 53 | 54 | tic 55 | x=sym('x',[n,1]); % state variables 56 | u=sym('u',[m,1]); % control variables 57 | f = A*x + B*u + N*kron(x,x); 58 | 59 | % control Lagrangian 60 | l=0.5*( x'*Q*x + u'*R*u ); 61 | [ff,ll]=hjb_set_up(f,l,x,u,x0,u0,n,m,degree); 62 | set_up=toc; 63 | 64 | % call hjb.m to find the Taylor polynomial py of the optimal cost 65 | % to degree d+1 and the Taylor polynomial ka of the optimal feedback 66 | % to degree d. 67 | tic 68 | [ka,fk,py,lk]= hjb(ff,ll,n,m,degree); 69 | comp=toc; 70 | py = 2*py; 71 | 72 | fprintf(' NST solution required %g (%g) seconds\n',comp,comp+set_up); 73 | end % if testNST 74 | 75 | %% 76 | if ( degree>1 ) 77 | tic 78 | [kk,vv] = AlbrechtQQR(A,B,Q,R,N,degree); 79 | comp = toc; 80 | 81 | disp('') 82 | fprintf(' AlbrechtQQR solution required %g seconds\n\n',comp) 83 | kk2 = kk{2}; 84 | vv3 = vv{3}; 85 | 86 | if ( testNST ) 87 | ka2 = ka(:,n+1:n+n*(n+1)/2); 88 | py3 = py( (n*(n+1)/2)+1 : (n*(n+1)/2)+n*(n+1)*(n+2)/6); 89 | 90 | tic; 91 | S2 = Kron2CT(n,2); 92 | S3 = Kron2CT(n,3); 93 | CTtime = toc; 94 | % fprintf('Kron to CT mappings (2+3) required %g seconds\n',CTtime) 95 | 96 | e_k2 = norm( ka2-kk2*S2' ); 97 | e_p3 = norm( py3-(vv3*S3') ); 98 | fprintf('testAlbrekhtQQR: The relative error in k^[2] is %g\n',e_k2/norm(ka2)); 99 | fprintf('testAlbrekhtQQR: The relative error in v^[3] is %g\n',e_p3/norm(py3)); 100 | end 101 | 102 | k2Rk1 = kk{2}.'*R*kk{1}; k1Rk2 = k2Rk1.'; 103 | HJB1_residual = LyapProduct((A+B*kk{1}).',vv{3}.',3) ... 104 | + LyapProduct((N+B*kk{2}).',vv{2}.',2) ... 105 | + k2Rk1(:) + k1Rk2(:); 106 | fprintf('testAlbrekhtQQR: The HJB residual for (k^[2],v^[3]) is %g\n\n',... 107 | norm(HJB1_residual) ) 108 | end 109 | 110 | %% 111 | if ( degree>2 ) 112 | tic 113 | kk3 = kk{3}; 114 | vv4 = vv{4}; 115 | 116 | if ( testNST ) 117 | ka3 = ka(:,n+n*(n+1)/2+1:n+n*(n+1)/2+n*(n+1)*(n+2)/6); 118 | py4 = py((n*(n+1)/2+n*(n+1)*(n+2)/6)+1:(n*(n+1)/2+n*(n+1)*(n+2)/6)+n*(n+1)*(n+2)*(n+3)/24); 119 | 120 | tic; 121 | S4 = Kron2CT(n,4); 122 | CTtime = toc; 123 | % fprintf('CT to Kron mappings (4) require %g seconds\n',CTtime) 124 | 125 | e_k3 = norm( ka3-kk3*S3' ); 126 | e_p4 = norm( py4-(vv4*S4') ); 127 | fprintf('tensor: The relative error in k^[3] is %g\n',e_k3/norm(ka3)); 128 | fprintf('tensor: The relative error in v^[4] is %g\n',e_p4/norm(py4)); 129 | 130 | k2Rk1 = kk{2}.'*R*kk{1}; k1Rk2 = k2Rk1.'; 131 | HJB1_residual = LyapProduct((A+B*kk{1}).',vv{3}.',3) ... 132 | + LyapProduct((N+B*kk{2}).',vv{2}.',2) ... 133 | + (kron(kk{1}.',kk{2}.')+kron(kk{2}.',kk{1}.'))*R(:); 134 | end 135 | end 136 | 137 | %% 138 | if ( degree>3 ) 139 | tic 140 | kk4 = kk{4}; 141 | vv5 = vv{5}; 142 | 143 | if ( testNST ) 144 | ka4 = ka(:,n+n*(n+1)/2+n*(n+1)*(n+2)/6+1:n+n*(n+1)/2+n*(n+1)*(n+2)/6+n*(n+1)*(n+2)*(n+3)/24); 145 | py5 = py((n*(n+1)/2+n*(n+1)*(n+2)/6)+n*(n+1)*(n+2)*(n+3)/24+1:(n*(n+1)/2+n*(n+1)*(n+2)/6)+n*(n+1)*(n+2)*(n+3)/24+n*(n+1)*(n+2)*(n+3)*(n+4)/120); 146 | 147 | tic 148 | S5 = Kron2CT(n,5); 149 | CTtime = toc; 150 | % fprintf('CT to Kron mappings (4) require %g seconds\n',CTtime) 151 | 152 | e_k4 = norm( ka4-kk4*S4' ); 153 | e_p5 = norm( py5 - vv5*S5' ); 154 | fprintf('tensor: The relative error in k^[4] is %g\n',e_k4/norm(ka4)); 155 | fprintf('tensor: The relative error in v^[5] is %g\n',e_p5/norm(py5)); 156 | 157 | end 158 | end 159 | 160 | %% 161 | if ( degree>4 ) 162 | tic 163 | kk5 = kk{5}; 164 | vv6 = vv{6}; 165 | 166 | if ( testNST ) 167 | ka5 = ka(:,n+n*(n+1)/2+n*(n+1)*(n+2)/6+n*(n+1)*(n+2)*(n+3)/24+1:end); 168 | py5 = py((n*(n+1)/2+n*(n+1)*(n+2)/6)+n*(n+1)*(n+2)*(n+3)/24+n*(n+1)*(n+2)*(n+3)*(n+4)/120+1:end); 169 | 170 | %tic 171 | %S6 = Kron2CT(n,6); 172 | %CTtime = toc; 173 | % fprintf('CT to Kron mappings (4) require %g seconds\n',CTtime) 174 | 175 | e_k5 = norm( ka5-kk5*S5' ); 176 | %e_p5 = norm( py5 - vv5*S5' ); 177 | fprintf('tensor: The relative error in k^[5] is %g\n',e_k5/norm(ka5)); 178 | %fprintf('tensor: The relative error in v^[5] is %g\n',e_p5/norm(py5)); 179 | 180 | end 181 | end 182 | % sometimes these errors are high, but the relative error is then low. 183 | % possibly due to factors like nearly singular R, nearly uncontrollable. 184 | % at other times, we are computing a relative error for a quantity 185 | % that should be zero. 186 | -------------------------------------------------------------------------------- /examples/ChafeeInfante1DControl/ChafeeInfanteFEMControl.m: -------------------------------------------------------------------------------- 1 | function [M,A,B1,B2,N,Q,zInit] = ChafeeInfanteFEMControl(n,m,alpha,nu,alpha3) 2 | %CHAFFEE_INFANTE A control problem for the Chafee-Infante equations. 3 | % 4 | % A Chafee-Infante control problem is to find u that minimizes 5 | % 6 | % J(u) = \int_0^\infty \| z \|^2 + u'*R*u dt 7 | % 8 | % subject to 9 | % 10 | % z_t = \nu z_xx + \alpha z - \alpha_3 z^3 + \sum_{i=1}^m \chi_i(x) u_i(t) 11 | % 12 | % and 13 | % 14 | % z_x(0,t)=0=z_x(1,t), z(x,0) = z_0(x) = a*cos(3*pi*x) 15 | % 16 | % 17 | % Given a state dimension (n) and a control dimension (m, default=1), 18 | % we produce a discretized control problem of the form: 19 | % 20 | % M \dot{x} = A*x + N3*(kron(x,kron(x,x))) + B*u 21 | % 22 | % J(u) = \int_0^\infty x'*Q*x + u'*R*u 23 | % 24 | % Note that a change of variables should be called by the calling routing 25 | % so that the mass matrix does not appear in the final discretized equation. 26 | % 27 | % Usage: 28 | % [M,A,B,N3,Q,z0] = ChafeeInfante(n,m,a,alpha,nu,alpha3) 29 | % 30 | % Default values: m=1, a=1, alpha=100, nu=1, alpha3=1. 31 | % (see Lunasin and Titi, 2017) 32 | % 33 | %% 34 | 35 | idx3 = @(j,k,l) (j-1)*n^2 + (k-1)*n + l; 36 | 37 | if ( nargin<2 ) 38 | m = 1; 39 | end 40 | 41 | if ( nargin<3 ) 42 | alpha = 100; 43 | nu = 1; 44 | alpha3 = 1; 45 | end 46 | % a = 1; 47 | 48 | [x,e_conn] = oned_mesh([0; 1],[1 2],n); 49 | % [x,e_conn] = oned_mesh([0; 0.5; 1],[1 2 3],n); 50 | 51 | [n_nodes , ~ ] = size(x ); 52 | [n_elements, nel_dof ] = size(e_conn); 53 | 54 | ide = 1:n_nodes; % setting equation numbers (Neumann bcs.) 55 | 56 | n_gauss = 5; 57 | [r,wt] = oned_quadrature(n_gauss); 58 | n_equations = n_nodes; 59 | 60 | one = ones(n_gauss,1); 61 | nu_g = nu*one; 62 | 63 | II = zeros(n_elements*nel_dof^2,1); 64 | JJ = zeros(n_elements*nel_dof^2,1); 65 | AA = zeros(n_elements*nel_dof^2,1); 66 | MM = zeros(n_elements*nel_dof^2,1); 67 | B1 = zeros(n_equations,m); 68 | B2 = zeros(n_equations,2); 69 | z0 = zeros(n_equations,1); 70 | 71 | IIn = zeros(6*n_elements*nel_dof^4,1); 72 | JJn = zeros(6*n_elements*nel_dof^4,1); 73 | NN = zeros(6*n_elements*nel_dof^4,1); 74 | 75 | n_triplets = 0; 76 | n_tripletsn = 0; 77 | 78 | b_loc = zeros(nel_dof,m); 79 | for n_el=1:n_elements 80 | % compute value of each test function and spatial derivatives 81 | % at the integration points (x_g - Gauss points, wt_g - Gauss weights) 82 | nodes_local = e_conn(n_el,:); 83 | x_local = x(nodes_local,:); 84 | [x_g, wt_g, phi, p_x] = oned_shape(x_local,r,wt); 85 | 86 | M_loc = oned_bilinear( one, phi, phi, wt_g ); 87 | A_loc = -oned_bilinear( nu_g, p_x, p_x, wt_g ) + alpha*M_loc; 88 | 89 | for mm = 1:m 90 | b_loc(:,mm) = oned_f_int(chi(x_g,mm,m),phi,wt_g); 91 | end 92 | 93 | z_loc = zZero(x_g); 94 | z0_loc = oned_f_int( z_loc, phi, wt_g ); 95 | %--------------------------------------------------------------------------- 96 | % Assemble contributions into the system matrices 97 | %--------------------------------------------------------------------------- 98 | for n_t=1:nel_dof 99 | n_test = ide(nodes_local(n_t)); 100 | 101 | for n_u=1:nel_dof 102 | n_unk = ide(nodes_local(n_u)); 103 | 104 | % A(n_unk,n_test) = A(n_unk,n_test) + A_loc(n_u); 105 | n_triplets = n_triplets + 1; 106 | II(n_triplets) = n_unk; 107 | JJ(n_triplets) = n_test; 108 | AA(n_triplets) = A_loc(n_t,n_u); 109 | MM(n_triplets) = M_loc(n_t,n_u); 110 | end 111 | 112 | for mm=1:m 113 | B1(n_test,mm) = B1(n_test,mm) + b_loc(n_t,mm); 114 | end 115 | 116 | z0(n_test) = z0(n_test) + z0_loc(n_t); 117 | 118 | % assemble the -z^3 term with symmetries 119 | for nj=1:nel_dof 120 | j = ide(nodes_local(nj)); 121 | % for nk=nj:nel_dof 122 | for nk=1:nel_dof 123 | k = ide(nodes_local(nk)); 124 | % for nl=nk:nel_dof 125 | for nl=1:nel_dof 126 | l = ide(nodes_local(nl)); 127 | % tmp = -sum(phi(:,n_t).*phi(:,nj).*phi(:,nk).*phi(:,nl).*wt_g(:))/6; 128 | tmp = -alpha3*sum(phi(:,n_t).*phi(:,nj).*phi(:,nk).*phi(:,nl).*wt_g(:)); 129 | 130 | n_tripletsn = n_tripletsn+1; 131 | IIn(n_tripletsn) = n_test; 132 | JJn(n_tripletsn) = idx3(j,k,l); 133 | NN(n_tripletsn) = tmp; 134 | 135 | % n_tripletsn = n_tripletsn+1; 136 | % IIn(n_tripletsn) = n_test; 137 | % JJn(n_tripletsn) = idx3(j,l,k); 138 | % NN(n_tripletsn) = tmp; 139 | % 140 | % n_tripletsn = n_tripletsn+1; 141 | % IIn(n_tripletsn) = n_test; 142 | % JJn(n_tripletsn) = idx3(k,j,l); 143 | % NN(n_tripletsn) = tmp; 144 | % 145 | % n_tripletsn = n_tripletsn+1; 146 | % IIn(n_tripletsn) = n_test; 147 | % JJn(n_tripletsn) = idx3(k,l,j); 148 | % NN(n_tripletsn) = tmp; 149 | % 150 | % n_tripletsn = n_tripletsn+1; 151 | % IIn(n_tripletsn) = n_test; 152 | % JJn(n_tripletsn) = idx3(l,j,k); 153 | % NN(n_tripletsn) = tmp; 154 | % 155 | % n_tripletsn = n_tripletsn+1; 156 | % IIn(n_tripletsn) = n_test; 157 | % JJn(n_tripletsn) = idx3(l,k,j); 158 | % NN(n_tripletsn) = tmp; 159 | end 160 | end 161 | end 162 | end 163 | end 164 | 165 | II = II(1:n_triplets); 166 | JJ = JJ(1:n_triplets); 167 | A = sparse( II, JJ, AA(1:n_triplets), n_equations, n_equations ); 168 | M = sparse( II, JJ, MM(1:n_triplets), n_equations, n_equations ); 169 | 170 | N = sparse( IIn(1:n_tripletsn), JJn(1:n_tripletsn), NN(1:n_tripletsn), ... 171 | n_equations, n_equations^3 ); 172 | 173 | Q = M; 174 | 175 | B2(1,1) = -1; 176 | B2(end,2) = 1; 177 | 178 | zInit = M\z0; 179 | end 180 | 181 | function [b_loc] = chi(x_local,mm,m) 182 | % The characteristic function over the interval ( (mm-1)/m, mm/m ). 183 | n = length(x_local); 184 | b_loc = zeros(n,1); 185 | 186 | for i=1:n 187 | if ( x_local(i)>(mm-1)/m && x_local(i)1 ) 65 | %=========================================================================== 66 | % Compute the degree=2 feedback solution 67 | %=========================================================================== 68 | % AA = 3*( kron( kron( (A+B*K1)',speye(n)),speye(n)) + kron( speye(n), kron( (A+B*K1)',speye(n))) + kron( kron( speye(n),speye(n) ),(A+B*K1)') ); 69 | % bb = -3*( kron( N*kron(eye(n),eye(n)), eye(n)) + kron(eye(n),N*kron(eye(n),eye(n))))'*v2; 70 | tic 71 | ABKT = (A+B*K1).'; 72 | AA = ( kron( ABKT, eye(n^2) ) + ... 73 | kron( eye(n ), kron( ABKT, eye(n ) ) ) + ... 74 | kron( eye(n^2), ABKT ) ); 75 | bb = -( kron( N, eye(n) ) + kron( eye(n),N ) ).'*v2; 76 | v3 = AA\bb; 77 | 78 | S = Kron2CT(n,2); 79 | C = CT2Kron(n,2); 80 | 81 | res = zeros(n*n,m); 82 | for i=1:m 83 | GG = ( kron( B(:,i).',eye(n^2) ) + ... 84 | kron( eye(n ), kron(B(:,i).',eye(n ) ) ) + ... 85 | kron( eye(n^2), B(:,i).' ) ); 86 | GG = C*S*GG; 87 | res(:,i) = -GG*v3; 88 | end 89 | 90 | v{3} = v3.'; 91 | k{2} = 0.5*(R\res.'); 92 | K2 = k{2}; 93 | end 94 | 95 | if ( degree>2 ) 96 | %=========================================================================== 97 | % Compute the degree=3 feedback solution 98 | %=========================================================================== 99 | AA = ( kron( ABKT, eye(n^3) ) + ... 100 | kron( eye(n ), kron( ABKT, eye(n^2) ) ) + ... 101 | kron( eye(n^2), kron( ABKT, eye(n ) ) ) + ... 102 | kron( eye(n^3), ABKT ) ); 103 | 104 | BK2NT = (B*K2+N).'; 105 | bb = -( kron( BK2NT, eye(n^2) ) + ... 106 | kron( kron( eye(n ), BK2NT), eye(n) ) + ... 107 | kron( eye(n^2), BK2NT ) )*v3 ... 108 | - kron(K2.',K2.')*r2 ; 109 | 110 | v4 = AA\bb; 111 | 112 | S = Kron2CT(n,3); 113 | C = CT2Kron(n,3); 114 | 115 | res = zeros(n*n*n,m); 116 | for i=1:m 117 | % GG = ( kron(B(:,i)',kron(eye(n),kron(eye(n),eye(n)))) + kron(eye(n),kron(B(:,i)',kron(eye(n),eye(n)))) + kron(eye(n),kron(eye(n),kron(B(:,i)',eye(n)))) + kron(eye(n),kron(eye(n),kron(eye(n),B(:,i)'))) ); 118 | GG = ( kron( B(:,i).', eye(n^3) ) + ... 119 | kron( eye(n ), kron(B(:,i).', eye(n^2) ) ) + ... 120 | kron( eye(n^2), kron(B(:,i).', eye(n ) ) ) + ... 121 | kron( eye(n^3), B(:,i).' ) ); 122 | GG = C*S*GG; 123 | res(:,i) = -GG*v4; 124 | end 125 | 126 | v{4} = v4.'; 127 | k{3} = 0.5*(R\res.'); 128 | K3 = k{3}; 129 | end 130 | 131 | if ( degree>3 ) 132 | %=========================================================================== 133 | % Compute the degree=4 feedback solution 134 | %=========================================================================== 135 | AA = ( kron( ABKT, eye(n^4) ) + ... 136 | kron( eye(n ), kron( ABKT, eye(n^3) ) ) + ... 137 | kron( eye(n^2), kron( ABKT, eye(n^2) ) ) + ... 138 | kron( eye(n^3), kron( ABKT, eye(n ) ) ) + ... 139 | kron( eye(n^4), ABKT ) ); 140 | 141 | bb = -( kron( BK2NT, eye(n^3) ) + ... 142 | kron( kron( eye(n ), BK2NT ), eye(n^2) ) + ... 143 | kron( kron( eye(n^2), BK2NT ), eye(n) ) + ... 144 | kron( eye(n^3), BK2NT ) )*v4 ... 145 | -( kron( (B*K3 ).', eye(n^2) ) + ... 146 | kron( kron( eye(n ), (B*K3 ).' ), eye(n ) ) + ... 147 | kron( eye(n^2), (B*K3 ).' ) )*v3 ... 148 | -( kron(K2.',K3.') + kron(K3.',K2.') )*r2 ; 149 | 150 | v5 = AA\bb; 151 | 152 | S = Kron2CT(n,4); 153 | C = CT2Kron(n,4); 154 | 155 | res = zeros(n*n*n*n,m); 156 | for i=1:m 157 | % GG = ( kron(B(:,i)',kron(eye(n),kron(eye(n),eye(n)))) + kron(eye(n),kron(B(:,i)',kron(eye(n),eye(n)))) + kron(eye(n),kron(eye(n),kron(B(:,i)',eye(n)))) + kron(eye(n),kron(eye(n),kron(eye(n),B(:,i)'))) ); 158 | GG = ( kron( B(:,i).',eye(n^4) ) + ... 159 | kron( eye(n ),kron(B(:,i).',eye(n^3) ) ) + ... 160 | kron( eye(n^2),kron(B(:,i).',eye(n^2) ) ) + ... 161 | kron( eye(n^3),kron(B(:,i).',eye(n ) ) ) + ... 162 | kron( eye(n^4), B(:,i).' ) ); 163 | GG = C*S*GG; 164 | res(:,i) = -GG*v5; 165 | end 166 | 167 | v{5} = v5.'; 168 | k{4} = 0.5*(R\res.'); 169 | K4 = k{4}; 170 | end 171 | 172 | if ( degree>4 ) 173 | warning('Only controls of degree <=4 have been implemented so far') 174 | end 175 | 176 | end 177 | 178 | -------------------------------------------------------------------------------- /examples/example04.m: -------------------------------------------------------------------------------- 1 | %function [A,B,Q,R,N] = example04() 2 | %EXAMPLE04 Compares feedback strategies for the Lorenz equations 3 | % 4 | 5 | fprintf('example04: setting degree to %d\n',degree); 6 | 7 | sigma = 10; rho = 28; beta = 8/3; 8 | 9 | A = [-sigma sigma 0 ; ... 10 | rho -1 0 ; ... 11 | 0 0 -beta ]; 12 | B = [1;0;0]; %B = [1;0;1]; 13 | N = zeros(3,9); 14 | N(2,3)=-0.5; N(2,7)=-0.5; % -x1 x3 term 15 | N(3,2)= 0.5; N(3,4)= 0.5; % x1 x2 term 16 | 17 | Q = eye(3); R = 1; 18 | 19 | [k,v] = qqr(A,B,Q,R,N,degree); 20 | 21 | v2 = v{2}; 22 | % x0 = [1;1;1]; 23 | x0 = [10;10;10]; 24 | T = 500; 25 | % Open loop 26 | rhs_open = @(t,x) [A*x(1:3) + N*kron(x(1:3),x(1:3)); ... 27 | x(1:3).'*Q*x(1:3)]; 28 | 29 | [t,x] = ode15s( rhs_open, [0 T], [x0;0] ); 30 | figure(10); hold on 31 | plot3(x(1,1),x(1,2),x(1,3),'*') 32 | plot3(x(:,1),x(:,2),x(:,3)) 33 | view([1 1 1]) 34 | 35 | % Linear feedback 36 | T = 1; 37 | APBK = A + B*k{1}; 38 | rhs_k1 = @(t,x) [APBK*x(1:3) + N*kron(x(1:3),x(1:3)); ... 39 | x(1:3).'*(Q+k{1}.'*R*k{1})*x(1:3)]; 40 | 41 | [t1,x1] = ode23s( rhs_k1, [0 T], [x0;0] ); 42 | figure(11); hold on 43 | plot3(x1(1,1),x1(1,2),x1(1,3),'*') 44 | plot3(x1(:,1),x1(:,2),x1(:,3),'r') 45 | plot3(0,0,0,'o') 46 | view([1 1 1]) 47 | 48 | c2 = v2*kron(x0,x0); 49 | fprintf('approx. regulator cost to v2: %g cost to T: %g\n',c2,x1(end,4)) 50 | 51 | % Quadratic feedback 52 | v3 = v{3}; 53 | NPBK2 = N + B*k{2}; 54 | u2 = @(x) k{1}*x + k{2}*kron(x,x); 55 | rhs_k2 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)); 56 | x(1:3).'*Q*x(1:3) + u2(x(1:3)).'*R*u2(x(1:3))]; 57 | 58 | [t2,x2] = ode23s( rhs_k2, [0 T], [x0;0] ); 59 | figure(12); hold on 60 | plot3(x2(1,1),x2(1,2),x2(1,3),'*') 61 | plot3(x2(:,1),x2(:,2),x2(:,3),'b') 62 | plot3(0,0,0,'o') 63 | view([1 1 1]) 64 | 65 | c3 = c2 + v3*kron(x0,kron(x0,x0)); 66 | fprintf('approx. regulator cost to v3: %g cost to T: %g\n',c3,x2(end,4)) 67 | 68 | figure(21) 69 | u1 = k{1}*x1(:,1:3).'; 70 | 71 | u2 = k{1}*x2(:,1:3).'; 72 | for i=1:length(u2) 73 | u2(i) = u2(i) + k{2}*kron(x2(i,1:3).',x2(i,1:3).'); 74 | end 75 | plot(t1,u1,'r',t2,u2,'b') 76 | 77 | 78 | % Cubic feedback 79 | v4 = v{4}; 80 | u3 = @(x) k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 81 | rhs_k3 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)) + ... 82 | B*k{3}*kron(x(1:3),kron(x(1:3),x(1:3))); 83 | x(1:3).'*Q*x(1:3) + u3(x(1:3)).'*R*u3(x(1:3))]; 84 | 85 | [t3,x3] = ode23s( rhs_k3, [0 T], [x0;0] ); 86 | figure(13); hold on 87 | plot3(x3(1,1),x3(1,2),x3(1,3),'*') 88 | plot3(x3(:,1),x3(:,2),x3(:,3),'k') 89 | plot3(0,0,0,'o') 90 | view([1 1 1]) 91 | 92 | c4 = c3 + v4*kron(x0,kron(x0,kron(x0,x0))); 93 | fprintf('approx. regulator cost to v4: %g cost to T: %g\n',c4,x3(end,4)) 94 | 95 | % Quartic feedback 96 | v5 = v{5}; 97 | u3_4 = @(x) k{3}*kron(kron(x,x),x) + ... 98 | k{4}*kron(kron(kron(x,x),x),x); 99 | u4 = @(x) k{1}*x + k{2}*kron(x,x) + u3_4(x); 100 | rhs_k4 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)) + ... 101 | B*u3_4(x(1:3)); 102 | x(1:3).'*Q*x(1:3) + u4(x(1:3)).'*R*u4(x(1:3))]; 103 | 104 | [t4,x4] = ode23s( rhs_k4, [0 T], [x0;0] ); 105 | figure(14); hold on 106 | plot3(x4(1,1),x4(1,2),x4(1,3),'*') 107 | plot3(x4(:,1),x4(:,2),x4(:,3),'k') 108 | plot3(0,0,0,'o') 109 | view([1 1 1]) 110 | 111 | c5 = c4 + v5*kron(x0,kron(x0,kron(x0,kron(x0,x0)))); 112 | fprintf('approx. regulator cost to v5: %g cost to T: %g\n',c5,x4(end,4)) 113 | 114 | % Quintic feedback 115 | v6 = v{6}; 116 | u3_5 = @(x) k{3}*kron(kron(x,x),x) + ... 117 | k{4}*kron(kron(kron(x,x),x),x) + ... 118 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 119 | u5 = @(x) k{1}*x + k{2}*kron(x,x) + u3_5(x); 120 | rhs_k5 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)) + ... 121 | B*u3_5(x(1:3)); 122 | x(1:3).'*Q*x(1:3) + u5(x(1:3)).'*R*u5(x(1:3))]; 123 | 124 | [t5,x5] = ode23s( rhs_k5, [0 T], [x0;0] ); 125 | figure(15); hold on 126 | plot3(x5(1,1),x5(1,2),x5(1,3),'*') 127 | plot3(x5(:,1),x5(:,2),x5(:,3),'k') 128 | plot3(0,0,0,'o') 129 | view([1 1 1]) 130 | 131 | c6 = c5 + v6*kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,x0))))); 132 | fprintf('approx. regulator cost to v6: %g cost to T: %g\n',c6,x5(end,4)) 133 | 134 | 135 | % Hexic feedback 136 | v7 = v{7}; 137 | u3_6 = @(x) k{3}*kron(kron(x,x),x) + ... 138 | k{4}*kron(kron(kron(x,x),x),x) + ... 139 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 140 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x); 141 | u6 = @(x) k{1}*x + k{2}*kron(x,x) + u3_6(x); 142 | rhs_k6 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)) + ... 143 | B*u3_6(x(1:3)); 144 | x(1:3).'*Q*x(1:3) + u6(x(1:3)).'*R*u6(x(1:3))]; 145 | 146 | [t6,x6] = ode23s( rhs_k6, [0 T], [x0;0] ); 147 | figure(16); hold on 148 | plot3(x6(1,1),x6(1,2),x6(1,3),'*') 149 | plot3(x6(:,1),x6(:,2),x6(:,3),'k') 150 | plot3(0,0,0,'o') 151 | view([1 1 1]) 152 | 153 | c7 = c6 + v7*kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,x0)))))); 154 | fprintf('approx. regulator cost to v7: %g cost to T: %g\n',c7,x6(end,4)) 155 | 156 | 157 | % Septic feedback 158 | v8 = v{8}; 159 | u3_7 = @(x) k{3}*kron(kron(x,x),x) + ... 160 | k{4}*kron(kron(kron(x,x),x),x) + ... 161 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 162 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x) + ... 163 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 164 | u7 = @(x) k{1}*x + k{2}*kron(x,x) + u3_7(x); 165 | rhs_k7 = @(t,x) [APBK*x(1:3) + NPBK2*kron(x(1:3),x(1:3)) + ... 166 | B*u3_7(x(1:3)); 167 | x(1:3).'*Q*x(1:3) + u7(x(1:3)).'*R*u7(x(1:3))]; 168 | 169 | [t7,x7] = ode23s( rhs_k6, [0 T], [x0;0] ); 170 | figure(17); hold on 171 | plot3(x7(1,1),x7(1,2),x7(1,3),'*') 172 | plot3(x7(:,1),x7(:,2),x7(:,3),'k') 173 | plot3(0,0,0,'o') 174 | view([1 1 1]) 175 | 176 | c8 = c7 + v8*kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,kron(x0,x0))))))); 177 | fprintf('approx. regulator cost to v8: %g cost to T: %g\n',c8,x7(end,4)) 178 | 179 | 180 | % figure(31) 181 | % plot(t1,x1(:,1),'r',t2,x2(:,1),'b',t3,x3(:,1),'k') 182 | % legend('degree 1','degree 2','degree 3') 183 | % xlabel('time'); ylabel('x_1'); title('evolution of state 1') 184 | % 185 | % figure(32) 186 | % plot(t1,x1(:,2),'r',t2,x2(:,2),'b',t3,x3(:,2),'k') 187 | % legend('degree 1','degree 2','degree 3') 188 | % xlabel('time'); ylabel('x_2'); title('evolution of state 2') 189 | % 190 | % figure(33) 191 | % plot(t1,x1(:,3),'r',t2,x2(:,3),'b',t3,x3(:,3),'k') 192 | % legend('degree 1','degree 2','degree 3') 193 | % xlabel('time'); ylabel('x_3'); title('evolution of state 3') 194 | 195 | figure(41) 196 | pu1 = k{1}*x1(:,1:3).'; 197 | 198 | pu2 = k{1}*x2(:,1:3).'; 199 | for i=1:length(pu2) 200 | pu2(i) = pu2(i) + k{2}*kron(x2(i,1:3).',x2(i,1:3).'); 201 | end 202 | 203 | pu3 = k{1}*x3(:,1:3).'; 204 | for i=1:length(pu3) 205 | pu3(i) = pu3(i) + k{2}*kron(x3(i,1:3).',x3(i,1:3).') ... 206 | + k{3}*kron(x3(i,1:3).',kron(x3(i,1:3).',x3(i,1:3).')); 207 | end 208 | 209 | pu4 = k{1}*x4(:,1:3).'; 210 | for i=1:length(pu4) 211 | pu4(i) = pu4(i) + k{2}*kron(x4(i,1:3).',x4(i,1:3).') ... 212 | + k{3}*kron(x4(i,1:3).',kron(x4(i,1:3).',x4(i,1:3).')) ... 213 | + k{4}*kron(x4(i,1:3).',kron(x4(i,1:3).',kron(x4(i,1:3).',x4(i,1:3).'))); 214 | end 215 | 216 | plot(t1,pu1,'r',t2,pu2,'b',t3,pu3,'k',t4,pu4,'c') 217 | legend('degree 1','degree 2','degree 3','degree 4') 218 | xlabel('time'); ylabel('control input'); title('Evolution of the control') 219 | 220 | figure(51) 221 | plot(t1,x1(:,4),t2,x2(:,4),t3,x3(:,4),t4,x4(:,4),t5,x5(:,4)) 222 | legend('degree 1','degree 2','degree 3','degree 4','degree 5') 223 | xlabel('time'); ylabel('accumulated cost'); title('Value function') 224 | %end 225 | 226 | -------------------------------------------------------------------------------- /examples/exampleCI.m: -------------------------------------------------------------------------------- 1 | % exampleCI, feedback control of the discretized Chafee-Infante equation 2 | % using a distributed (B1) or Neumann boundary control (B2). 3 | % 4 | 5 | [E,A,B1,B2,N3,Q,zInit] = ChafeeInfanteFEMControl(n,m,alpha,nu,alpha3); 6 | 7 | zInit = a*zInit; 8 | 9 | xNodes = linspace(0,1,n+1); 10 | 11 | sqM = sqrtm(full(E)); 12 | sqMinv = inv(sqM); 13 | 14 | Ac = sqMinv*A*sqMinv; %#ok 15 | if ( strcmp(controlType,'source') ) 16 | Bc = sqMinv*B1; %#ok % use source terms 17 | elseif ( strcmp(controlType,'boundary') ) 18 | Bc = sqMinv*B2; %#ok % use Neumann (m=2) 19 | end 20 | 21 | Nc = sqMinv*kroneckerRight(N3,sqMinv); %#ok 22 | 23 | Qc = q0*eye(n+1); R = r0*eye(m); 24 | 25 | x0 = sqM*zInit; 26 | 27 | tic 28 | [k,v] = cqrOdd(Ac,Bc,Qc,R,Nc,degree,false); 29 | toc 30 | 31 | c2 = v{2}*kron(x0,x0); 32 | fprintf('linear feedback cost: %g\n',c2) 33 | 34 | if (degree>2) 35 | c4 = c2 + v{4}*KroneckerPower(x0,4); 36 | fprintf('cubic feedback cost: %g\n',c4) 37 | end 38 | if (degree>4) 39 | c6 = c4 + v{6}*KroneckerPower(x0,6); 40 | fprintf('quintic feedback cost: %g\n',c6) 41 | end 42 | if (degree>6) 43 | c8 = c6 + v{8}*KroneckerPower(x0,8); 44 | fprintf('septic feedback cost: %g\n',c8) 45 | end 46 | fprintf('\n') 47 | 48 | %% 49 | % Now verify the quality of the feedback laws 50 | tInfinity = 5; 51 | options = odeset('reltol',1e-9); 52 | 53 | runOpen = false; 54 | if ( runOpen ) 55 | %--------------------------------------------------------------------------- 56 | % Open loop simulation 57 | %--------------------------------------------------------------------------- 58 | tic 59 | rhs_open = @(t,x) [ Ac*x(1:end-1) + Nc*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1));... 60 | x(1:end-1).'*Qc*x(1:end-1) ]; 61 | 62 | [T,Z] = ode15s(rhs_open,linspace(0,tInfinity,101),[x0;0],options); 63 | figure(10) 64 | openLoopCost = Z(end,end); 65 | 66 | Zval = Z(:,1:end-1)*sqMinv; %#ok 67 | mesh(xNodes,T,Zval) 68 | xlabel('z'); ylabel('time') 69 | title('Open Loop Simulation') 70 | 71 | fprintf('Open Loop Cost (0,T) is %15.10f\n\n',openLoopCost); 72 | toc 73 | end 74 | 75 | runClosed = true; 76 | if ( runClosed ) 77 | %----------------------------------------------------------------------------- 78 | % Linear feedback 79 | %----------------------------------------------------------------------------- 80 | APBK = Ac + Bc*k{1}; 81 | computeU1 = @(x) k{1}*x; 82 | rhs_k1 = @(t,x) vertcat( APBK*x(1:end-1) + Nc*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)),... 83 | x(1:end-1).'*Qc*x(1:end-1) +... 84 | computeU1(x(1:end-1)).'*R*computeU1(x(1:end-1)) ); 85 | 86 | [t1,z1] = ode15s( rhs_k1, linspace(0,tInfinity,101), [x0;0], options ); 87 | figure(11) 88 | closedLoopCost1 = z1(end,end); 89 | 90 | Zval = z1(:,1:end-1)*sqMinv; %#ok 91 | surf(xNodes,t1,Zval) 92 | xlabel('z'); ylabel('time') 93 | title('Closed-Loop Simulation with k^{[1]}') 94 | 95 | fprintf('Approx regulator cost to v^[2]: %15.10f\n',c2) 96 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost1); 97 | 98 | 99 | if ( degree>2 ) 100 | %--------------------------------------------------------------------------- 101 | % Cubic feedback 102 | %--------------------------------------------------------------------------- 103 | NPBK2 = Bc*k{2}; 104 | NPBK3 = Nc + Bc*k{3}; 105 | computeU3 = @(x) kronPolyEval(k,x,3);%k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 106 | 107 | rhs_k3 = @(t,x) vertcat( APBK*x(1:end-1) + ... 108 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 109 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)), ... 110 | x(1:end-1).'*Qc*x(1:end-1) + ... 111 | computeU3(x(1:end-1)).'*R*computeU3(x(1:end-1)) ); 112 | 113 | [t3,z3] = ode15s( rhs_k3, linspace(0,tInfinity,101), [x0;0], options ); 114 | 115 | figure(13) 116 | closedLoopCost3 = z3(end,end); 117 | 118 | Zval = z3(:,1:end-1)*sqMinv; %#ok 119 | surf(xNodes,t3,Zval) 120 | xlabel('z'); ylabel('time') 121 | title('Closed-Loop Simulation with k^{[3]}') 122 | 123 | fprintf('Approx regulator cost to v^[4]: %15.10f\n',c4) 124 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost3); 125 | end 126 | 127 | if ( degree>4 ) 128 | %----------------------------------------------------------------------------- 129 | % Quintic feedback 130 | %----------------------------------------------------------------------------- 131 | computeU5 = @(x) k{1}*x + ... 132 | k{2}*kron(x,x) + ... 133 | k{3}*kron(kron(x,x),x) + ... 134 | k{4}*kron(kron(kron(x,x),x),x) + ... 135 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 136 | computeU4_5 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 137 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 138 | rhs_k5 = @(t,x) [ APBK*x(1:end-1) + ... 139 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 140 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 141 | Bc*computeU4_5(x(1:end-1)); ... 142 | x(1:end-1).'*Qc*x(1:end-1) + ... 143 | computeU5(x(1:end-1)).'*R*computeU5(x(1:end-1)) ]; 144 | 145 | [t5,z5] = ode15s( rhs_k5, linspace(0,tInfinity,101), [x0;0], options ); 146 | 147 | figure(15) 148 | closedLoopCost5 = z5(end,end); 149 | 150 | Zval = z5(:,1:end-1)*sqMinv; %#ok 151 | surf(xNodes,t5,Zval) 152 | xlabel('z'); ylabel('time') 153 | title('Closed-Loop Simulation with k^{[5]}') 154 | 155 | fprintf('Approx regulator cost to v^[6]: %15.10f\n',c6) 156 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost5); 157 | 158 | end 159 | 160 | if ( degree>6 ) 161 | %----------------------------------------------------------------------------- 162 | % Septic feedback 163 | %----------------------------------------------------------------------------- 164 | computeU7 = @(x) k{1}*x + ... 165 | k{2}*kron(x,x) + ... 166 | k{3}*kron(kron(x,x),x) + ... 167 | k{4}*kron(kron(kron(x,x),x),x) + ... 168 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 169 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x)+ ... 170 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 171 | computeU4_7 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 172 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 173 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x)+ ... 174 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 175 | rhs_k7 = @(t,x) [ APBK*x(1:end-1) + ... 176 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 177 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 178 | Bc*computeU4_7(x(1:end-1)); ... 179 | x(1:end-1).'*Qc*x(1:end-1) + ... 180 | computeU7(x(1:end-1)).'*R*computeU7(x(1:end-1)) ]; 181 | 182 | [t7,z7] = ode15s( rhs_k7, linspace(0,tInfinity,101), [x0;0], options ); 183 | 184 | figure(15) 185 | closedLoopCost7 = z7(end,end); 186 | 187 | Zval = z7(:,1:end-1)*sqMinv; %#ok 188 | surf(xNodes,t7,Zval) 189 | xlabel('z'); ylabel('time') 190 | title('Closed-Loop Simulation with k^{[7]}') 191 | 192 | fprintf('Approx regulator cost to v^[6]: %15.10f\n',c8) 193 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost7); 194 | 195 | end 196 | 197 | end 198 | 199 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QQR [![DOI](https://zenodo.org/badge/211403941.svg)](https://zenodo.org/badge/latestdoi/211403941) 2 | Software to approximately solve the quadratic-quadratic regulator (QQR) and polynomial-quadratic regulator (PQR) problems. The description of the algorithms are provided in the papers 3 | 4 | - *The Quadratic-Quadratic Regulator Problem: Approximating feedback controls for quadratic-in-state nonlinear systems, American Control Conference, 2020. 5 | https://arxiv.org/abs/1910.03396* 6 | 7 | by Jeff Borggaard and Lizette Zietsman 8 | 9 | and 10 | 11 | - *On Approximating Polynomial-Quadratic Regulator Problems, Mathematical Theory of Networks and Systems, 2020 (accepted). https://arxiv.org/abs/2009.11068* and appeared in IFAC-PapersOnLine, 54(9), 329-334, 2021. 12 | 13 | by Jeff Borggaard and Lizette Zietsman. 14 | 15 | ## Installation Notes 16 | Download and untar, or clone this and the KroneckerTools repositories: 17 | ``` 18 | git clone https://www.github.com/jborggaard/KroneckerTools 19 | git clone https://www.github.com/jborggaard/QQR 20 | ``` 21 | 22 | Optional: 23 | Get the Matlab functions for efficiently solving linear systems with a special Kronecker sum structure (laplace-like structure) at https://anchp.epfl.ch/index-html/software/misc and detailed in the preprint: _Recursive blocked algorithms for linear systems with Kronecker product structure_, by Minhong Chen and Daniel Kressner. Place the directory "tensor_recursive" within the kronecker directory. 24 | 25 | Optional: 26 | Tests to match our Quadratic-Quadratic Regulator paper can be performed by requesting the Nonlinear Systems Toolbox from Art Krener. If this is the case, adjust the path in **setNSTpath.m** and set _testNST=true_ inside the **tests_ACC.m** script. 27 | 28 | The installation can be tested in Matlab (we used R2019b) by typing 29 | ``` 30 | >> examplesForACC 31 | ``` 32 | 33 | A stand-alone test (setting testNST=false) can be found in the examples directory 34 | The details of some of our functions and test examples are provided below. 35 | 36 | ## Quickstart 37 | 38 | For a quickstart, open the live script example10.mlx in the examples directory 39 | using Matlab2022b or later. 40 | 41 | ## How to use qqr 42 | 43 | If A is n-by-n, B is n-by-m, Q is n-by-n, R is m-by-m, and N is n-by-n^2 , with [A,B] a controllable pair, we can compute the coefficients of the feedback laws and the value function in Matlab as 44 | ``` 45 | >> [k,v] = qqr(A,B,Q,R,N,degree); 46 | ``` 47 | The variable _k_ is a cell array with _k{1}_ being m-by-n and is the usual first-degree feedback law computed by _lqr_, _k{2}_ is m-by-n^2 , up to _k{degree}_ which is m-by-n^degree . The feedback control is then 48 | ``` 49 | >> u = k{1}*x + k{2}*kron(x,x) + ... + k{degree}*kron(kron(... ,x),x); 50 | ``` 51 | This can be simplified using the function **kronPolyEval.m** from the KroneckerTools repository as 52 | ``` 53 | >> u = kronPolyEval(k,x,degree); 54 | ``` 55 | 56 | The variable _v_ is a cell array with _v{2}_ being n-by-n^2 , up to _v{degree+1}_ which is n-by-n^degree+1 . These are coefficients of the polynomial approximation to the value function. From an initial _x0_, we can compute the approximation to the value function as 57 | ``` 58 | >> J = v{2}*kron(x0,x0) + ... + v{degree+1}*kron(kron(... ,x0),x0); 59 | ``` 60 | or _J = kronPolyEval(v,x0,degree+1);_ (kronPolyEval skips the empty matrix v{1}=[]). 61 | 62 | For details on how to run **qqr**, one can also type 63 | ``` 64 | >> help qqr 65 | ``` 66 | 67 | for examples how to run **qqr** see the live script 68 | ``` 69 | >> addpath('examples') 70 | >> open example10.mlx 71 | ``` 72 | as well as 73 | ``` 74 | example4.m and example5.m 75 | ``` 76 | in the examples directory. 77 | 78 | ## How to use cqr and pqr 79 | 80 | With the same assumptions on A,B,Q,R as in _qqr_, but we now allow for higher degree polynomial terms by defining N to be a cell array _N{2}_ is n-by-n^2, _N{3}_ is n-by n^3, etc. (the _N{1}_ term is ignored). The controlled polynomial system is then described as either 81 | ``` 82 | \dot{x} = A*x + B*u + N{2}*kron(x,x) + N{3}*kron(kron(x,x),x) 83 | ``` 84 | 85 | ``` 86 | >> [k,v] = cqr(A,B,Q,R,N,degree); 87 | ``` 88 | or 89 | ``` 90 | \dot{x} = A*x + B*u + N{2}*kron(x,x) + N{3}*kron(kron(x,x),x) + ... N{p}*kron(...kron(x,x),x) 91 | ``` 92 | 93 | ``` 94 | >> [k,v] = pqr(A,B,Q,R,N,degree); 95 | ``` 96 | The description of the feedback coefficients _k_ and value function coefficients _v_ are exactly as in _qqr_ above. 97 | 98 | For details on how to run **cqr** or **pqr**, one can also type 99 | ``` 100 | >> help cqr 101 | >> help pqr 102 | ``` 103 | 104 | A special version of cqr is available when N{2}=0 (in this case, the feedback 105 | terms are all odd. This version, cqrOdd is called with N{3} (not the cell array). 106 | ``` 107 | >> [k,v] = cqrOdd(A,B,Q,R,N{3},degree) 108 | ``` 109 | (degree should be odd in this case). 110 | 111 | ## Description of Files 112 | #### setKroneckerToolsPath 113 | 114 | Defines the path where functions for working with Kronecker product expressions as well as the optional tensor_recursive solver is stored. The default settings may work but can be changed if the solver directories are located elsewhere. 115 | 116 | #### setNSTpath 117 | 118 | Defines the path where the Nonlinear Systems Toolbox by Krener is located. This is optional and only used for testing and debugging. 119 | 120 | #### AlbrechtKronQQR 121 | 122 | Builds and solves the full Kronecker product form of the polynomial approximation to the HJB equation. Schur decomposition of A+Bk{1} should be performed to produce an upper triangular system. This would still be an O(n^2degree ) algorithm and prohibitively expensive. This file is only included for archival reasons. 123 | 124 | #### pqr 125 | 126 | Solves the polynomial-quadratic regulator problem. 127 | 128 | #### cqr 129 | 130 | Solves the cubic-quadratic regulator problem (a front end to pqr). 131 | 132 | #### qqr 133 | 134 | Solves the quadratic-quadratic regulator problem (a front end to pqr). 135 | 136 | #### cqrOdd 137 | 138 | A special case that exploits computation when the right-hand-size polynomial only includes odd degree terms. 139 | 140 | 141 | ## Examples 142 | 143 | A number of examples can be found in the _examples_ directory. More are available in the **PolynomialSystems** repository. 144 | 145 | ### example01.m 146 | 147 | Solves the control problem for randomly generated systems. Some systems are nearly uncontrollable, others have near zero coefficients, so relative errors could be large. 148 | 149 | ### example02.m 150 | 151 | Solves a control problem using a discretization of the 1-dimensional Burgers equation (found in the included Burgers1DControl directory). The control inputs are spatially distributed uniform sources. 152 | 153 | ### example03.m 154 | 155 | Similar to example1.m, except we force A to be negative-definite, symmetric. 156 | 157 | ### example04.m 158 | 159 | Compare feedback strategies for the Lorenz system. 160 | 161 | ### example05.m 162 | 163 | Similar to example2.m, except we consider a linear reaction term and use a better change-of-variables to convert the discretized system to an explicit system of controlled differential equations. 164 | 165 | ### example06.m 166 | 167 | A simple first-order system where we can investigate convergence of the value function (by plotting it). 168 | 169 | ### example07.m 170 | 171 | A first order cubic control system introduced in Sakamoto and van der Schaft, 2007. 172 | 173 | ### example08.m 174 | 175 | A ring of van der Pol oscillators. This example can generate arbitrarily large systems of cubic equations. The region of attraction varies with the degree of feedback control. 176 | 177 | 178 | ### References 179 | ``` 180 | @misc{borggaard2019quadraticquadratic, 181 | title={The Quadratic-Quadratic Regulator Problem: 182 | Approximating feedback controls for quadratic-in-state nonlinear systems}, 183 | author={Jeff Borggaard and Lizette Zietsman}, 184 | booktitle={Proceedings of the 2020 American Control Conference}, 185 | year={2020}, 186 | eprint={1910.03396}, 187 | archivePrefix={arXiv}, 188 | primaryClass={math.OC} 189 | } 190 | ``` 191 | 192 | ``` 193 | @misc{borggaard2020polynomialquadratic, 194 | title={On Approximating Polynomial-Quadratic Regulator Problems}, 195 | author={Jeff Borggaard and Lizette Zietsman}, 196 | year={2020}, 197 | eprint={2009.11068}, 198 | archivePrefix={arXiv}, 199 | primaryClass{math.OC} 200 | } 201 | ``` 202 | -------------------------------------------------------------------------------- /pqr.m: -------------------------------------------------------------------------------- 1 | function [k,v] = pqr(A,B,Q,R,N,degree,solver,verbose) 2 | %PQR Albrecht's approximation to the polynomial-quadratic-regulator problem 3 | % A polynomial system is provided in Kronecker product form 4 | % \dot{x} = A*x + B*u + N{2}*kron(x,x) + N{3}*kron(kron(x,x),x) + ..., 5 | % with running cost 6 | % \ell(x,u) = x'*Q*x + u'*R*u 7 | % 8 | % note: Terms in the cell array, N, are polynomial nonlinear terms and 9 | % *NOT* the bilinear term found in lqr! 10 | % 11 | % This function returns an approximation to the HJB equations for computing 12 | % the optimal feedback control up to "degree" (a natural number < 5). 13 | % 14 | % The output is a polynomial approximation to the value function v 15 | % and the feedback control k. Generally, 16 | % 17 | % v(x) = v2*kron(x,x) + ... 18 | % v3*kron(kron(x,x),x) + ... 19 | % v4*kron(kron(kron(x,x),x),x) + ... 20 | % and 21 | % 22 | % k(x) = k1*x + ... 23 | % k2*kron(x,x) + ... 24 | % k3*kron(kron(x,x),x) + ... 25 | % 26 | % The elements of v and k are returned in a cell array: 27 | % v{2} = v2, v{3} = v3, etc. and k{1} = k1, k{2} = k2, etc. 28 | % 29 | % Usage: [k,v] = pqr(A,B,Q,R,N,degree,solver); 30 | % 31 | % Inputs: 32 | % A 33 | % system matrix of size [n,n] 34 | % B 35 | % control inputs matrix of size [n,m] (A,B) is a controllable pair 36 | % Q 37 | % a symmetric, positive-semidefinite matrix of size [n,n] 38 | % R 39 | % a symmetric, positive-definite matrix of size [m,m] 40 | % N 41 | % a cell array of matrices that describe higher-order polynomial terms 42 | % when expressed in Kronecker form 43 | % N{1} is unused 44 | % N{2} is of size [n,n^2] 45 | % N{3} is of size [n,n^3] 46 | % etc. up to the degree of the system 47 | % degree 48 | % the degree of the computed control law 49 | % solver 50 | % (optional) an argument to override the automatic selection of 51 | % linear solvers for the Kronecker sum systems 52 | % 'LaplaceRecursive' 53 | % use the tensor_recursive solver by Chen and Kressner if present 54 | % 'LyapunovRecursive' 55 | % uses a modified version of the above solver if present 56 | % 'BartelsStewart' (default) 57 | % solve using an n-Way generalization of the Bartels-Stewart 58 | % algorithm as described in the reference given below. 59 | % 60 | % Outputs: 61 | % k 62 | % a cell array of the polynomial feedback terms 63 | % term k{l} is of size [m,n^l], l=1,...,degree 64 | % v 65 | % a cell array of the value function terms 66 | % term v{l+1} is of size [1,n^(l+1)], l=1,...,degree 67 | % 68 | % 69 | % The construction of the Kronecker system from Al'Brecht's expansion and 70 | % its solution is found using an N-Way version of the Bartels-Stewart alg. 71 | % cf., 72 | % 73 | % Borggaard and Zietsman, On Approximating Polynomial-Quadratic Regulator 74 | % Problems, IFAC-PapersOnLine, 54(9), 329-334. 75 | % 76 | % Details about how to run this function, including necessary libraries 77 | % and example scripts, can be found at https://github.com/jborggaard/QQR 78 | % 79 | % Author: Jeff Borggaard, Virginia Tech 80 | % 81 | % Part of the QQR library. 82 | %% 83 | 84 | setKroneckerToolsPath 85 | 86 | if ( nargin<8 ) 87 | verbose = true; % a flag for more detailed output 88 | end 89 | 90 | % some input consistency checks: A nxn, B nxm, Q nxn SPSD, R mxm SPD 91 | n = size(A,1); 92 | m = size(B,2); 93 | 94 | if ( nargin>=6 ) 95 | classes = {'numeric'}; 96 | attributesA = {'size',[n,n]}; validateattributes(A,classes,attributesA); 97 | attributesB = {'size',[n,m]}; validateattributes(B,classes,attributesB); 98 | attributesQ = {'size',[n,n]}; validateattributes(Q,classes,attributesQ); 99 | attributesR = {'size',[m,m]}; validateattributes(R,classes,attributesR); 100 | else 101 | error('pqr: expects at least 6 inputs'); 102 | end 103 | 104 | if ( nargin==5 ) 105 | degree = 2; 106 | end 107 | 108 | % input consistency check on dimensions of entries of the cell array N 109 | degN = length(N); 110 | if ( degN<2 ) 111 | error('pqr: assumes at least quadratic nonlinearities') 112 | end 113 | for p=2:degN % check dimensions of N 114 | attributesN = {'size',[n,n^p]};validateattributes(N{p},classes,attributesN); 115 | end 116 | 117 | % define the vec function for readability 118 | vec = @(X) X(:); 119 | 120 | %============================================================================= 121 | % Define the linear solver 122 | %============================================================================= 123 | if ( ~exist('solver','var') ) 124 | if ( exist('./kronecker/tensor_recursive/lyapunov_recursive.m','file') ... 125 | && n>1 ) 126 | % lyapunov_recursive exists and is applicable 127 | solver = 'LyapunovRecursive'; 128 | elseif ( exist('./kronecker/tensor_recursive/laplace_recursive.m','file')... 129 | && n>1 ) 130 | % laplace_recursive exists and is applicable 131 | solver = 'LaplaceRecursive'; 132 | else 133 | % either n=1 (which could also be treated separately) or testing N-Way 134 | % this is also the default solver. 135 | solver = 'BartelsStewart'; 136 | end 137 | end 138 | 139 | v = cell(1,degree+1); 140 | k = cell(1,degree); 141 | 142 | 143 | comp = zeros(1,degree); % store computational times 144 | %============================================================================= 145 | % Compute the degree=1 feedback solution 146 | %============================================================================= 147 | tic 148 | [KK,PP] = lqr(full(A),full(B),full(Q),full(R)); 149 | 150 | k{1} =-KK; 151 | v{2} = vec(PP); 152 | 153 | comp(1) = toc; 154 | 155 | % Define the required n-Way Lyapunov matrices for all solvers 156 | ABKT = (A+B*k{1}).'; 157 | Al = cell(1,degree); 158 | for d=1:degree+1 159 | Al{d} = ABKT; 160 | end 161 | 162 | %============================================================================= 163 | % Compute the degree=2 feedback solution 164 | %============================================================================= 165 | bb = -LyapProduct(N{2}.',v{2},2); 166 | 167 | v{3} = solveKroneckerSystem(Al,bb,n,3,solver); 168 | v{3} = real(v{3}(:)); 169 | 170 | v{3} = kronMonomialSymmetrize(v{3},n,3); 171 | 172 | res = zeros(n*n,m); 173 | for i=1:m 174 | res(:,i) = -LyapProduct(B(:,i).',v{3},3); 175 | end 176 | 177 | k{2} = 0.5*(R\res.'); 178 | 179 | comp(2) = toc; 180 | 181 | %============================================================================= 182 | % Compute higher degree feedback terms 183 | %============================================================================= 184 | BKN = cell(1,degree); 185 | for d=3:degree 186 | %=========================================================================== 187 | % Compute the degree d feedback solution 188 | %=========================================================================== 189 | tic 190 | 191 | if (degN>d-1) 192 | bb = -LyapProduct(N{d}.',v{2},2); 193 | else 194 | bb = zeros(n^(d+1),1); 195 | end 196 | 197 | if (degN>d-2) 198 | BKN{d-1} = B*k{d-1}+N{d-1}; 199 | else 200 | BKN{d-1} = B*k{d-1}; 201 | end 202 | 203 | for i=3:d 204 | bb = bb - LyapProduct(BKN{d+2-i}.',v{i},i); 205 | end 206 | 207 | for i=2:(d/2) 208 | tmp = k{i}.'*R*k{d+1-i}; 209 | bb = bb - vec(tmp) - vec(tmp.'); 210 | end 211 | 212 | if (mod(d,2)) % if d is odd 213 | tmp = k{(d+1)/2}.'*R*k{(d+1)/2}; 214 | bb = bb - vec(tmp); 215 | end 216 | 217 | v{d+1} = solveKroneckerSystem(Al,bb,n,d+1,solver); 218 | v{d+1} = real(v{d+1}(:)); 219 | 220 | v{d+1} = kronMonomialSymmetrize(v{d+1},n,d+1); 221 | 222 | res = zeros(n^d,m); 223 | for i=1:m 224 | res(:,i) = -LyapProduct(B(:,i).',v{d+1},d+1); 225 | end 226 | 227 | k{d} = 0.5*(R\res.'); 228 | 229 | comp(d) = toc; 230 | end 231 | 232 | for d=2:degree+1 233 | v{d} = v{d}.'; 234 | end 235 | 236 | if ( verbose ) 237 | for i=1:degree 238 | fprintf('pqr: CPU time for degree %d controls: %g\n',i,comp(i)); 239 | end 240 | end 241 | end 242 | 243 | 244 | function [v] = solveKroneckerSystem(Al,bb,n,degree,solver) 245 | 246 | if ( strcmp(solver,'LyapunovRecursive') ) 247 | v = lyapunov_recursive(Al,reshape(bb,n*ones(1,degree))); 248 | 249 | elseif ( strcmp(solver,'LaplaceRecursive') ) 250 | v = laplace_recursive(Al,reshape(bb,n*ones(1,degree))); 251 | 252 | else 253 | v = KroneckerSumSolver(Al,bb,degree); 254 | 255 | end 256 | 257 | end % function solveKroneckerSystem 258 | -------------------------------------------------------------------------------- /examples/exampleKS.m: -------------------------------------------------------------------------------- 1 | % example KS, feedback control of the discretized periodic 2 | % Kuramoto-Sivashinsky equations using distributed control. 3 | % 4 | 5 | [E,A,B,N2,Q,zInit] = KuramotoSivashinskyFEMControl(n,m,1/L^2); 6 | 7 | xNodes = linspace(0,1,n+1); 8 | 9 | sqM = sqrtm(full(E)); 10 | sqMinv = inv(sqM); 11 | 12 | Ac = sqMinv*A*sqMinv; %#ok 13 | Bc = sqMinv*B; %#ok 14 | Nc = kroneckerRight(sqMinv*N2,sqMinv); %#ok 15 | 16 | Qc = eye(2*n); R = r0*eye(m); 17 | 18 | x0 = sqM*zInit; % change of variable to remove mass matrix E 19 | 20 | tic 21 | [k,v] = qqr(Ac,Bc,Qc,R,Nc,degree,false); 22 | toc 23 | save('qqr_KS.mat','k','v') 24 | 25 | c2 = v{2}*kron(x0,x0); 26 | fprintf('linear feedback cost: %g\n',c2) 27 | 28 | if (degree>1) 29 | c3 = c2 + v{3}*kron(kron(x0,x0),x0); 30 | fprintf('quadratic feedback cost: %g\n',c3) 31 | end 32 | if (degree>2) 33 | c4 = c3 + v{4}*kron(kron(kron(x0,x0),x0),x0); 34 | fprintf('cubic feedback cost: %g\n',c4) 35 | end 36 | if (degree>3) 37 | c5 = c4 + v{5}*kron(kron(kron(kron(x0,x0),x0),x0),x0); 38 | fprintf('quartic feedback cost: %g\n',c5) 39 | end 40 | if (degree>4) 41 | c6 = c5 + v{6}*kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0); 42 | fprintf('quintic feedback cost: %g\n',c6) 43 | end 44 | fprintf('\n') 45 | 46 | % Now verify the quality of the feedback laws 47 | %tInfinity = 120; 48 | %tPlot = linspace(0,120,1201); 49 | 50 | runOpen = true; 51 | if ( runOpen ) 52 | %--------------------------------------------------------------------------- 53 | % Open loop simulation 54 | %--------------------------------------------------------------------------- 55 | tic 56 | rhs_open = @(t,x) [ Ac*x(1:end-1) + Nc*kron(x(1:end-1),x(1:end-1));... 57 | x(1:end-1).'*Qc*x(1:end-1) ]; 58 | 59 | options = odeset('reltol',1e-3); 60 | 61 | [T,Z] = ode23s(rhs_open,[0 tInfinity],[x0;0],options); 62 | figure(10) 63 | openLoopCost = Z(end,end); 64 | 65 | Z = Z(:,1:end-1)*sqMinv; %#ok % 66 | Zval = Z(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); % periodicity for plot 67 | mesh(xNodes,T,Zval) 68 | xlabel('z'); ylabel('time') 69 | title('Open Loop Simulation') 70 | 71 | fprintf('Open Loop Cost (0,T) is %15.10f\n\n',openLoopCost); 72 | toc 73 | end 74 | 75 | runClosed = true; 76 | if ( runClosed ) 77 | %----------------------------------------------------------------------------- 78 | % Linear feedback 79 | %----------------------------------------------------------------------------- 80 | APBK = Ac + Bc*k{1}; 81 | computeU1 = @(x) k{1}*x; 82 | rhs_k1 = @(t,x) vertcat( APBK*x(1:end-1) + Nc*kron(x(1:end-1),x(1:end-1)),... 83 | x(1:end-1).'*Qc*x(1:end-1) +... 84 | computeU1(x(1:end-1)).'*R*computeU1(x(1:end-1)) ); 85 | 86 | [t1,z1] = ode23s( rhs_k1, [0 tInfinity], [x0;0], options ); 87 | figure(11); hold on 88 | closedLoopCost1 = z1(end,end); 89 | 90 | Zz = z1(:,1:end-1)*sqMinv; %#ok 91 | Zval = Zz(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); 92 | mesh(xNodes,t1,Zval) 93 | xlabel('z'); ylabel('time') 94 | title('Closed-Loop Simulation with k^{[1]}') 95 | 96 | c2 = v{2}*kron(x0,x0); 97 | fprintf('Approx regulator cost to v^[2]: %15.10f\n',c2) 98 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost1); 99 | 100 | if ( degree>1 ) 101 | %--------------------------------------------------------------------------- 102 | % Quadratic feedback 103 | %--------------------------------------------------------------------------- 104 | NPBK2 = Nc + Bc*k{2}; 105 | computeU2 = @(x) k{1}*x + k{2}*kron(x,x); 106 | rhs_k2 = @(t,x) vertcat( APBK*x(1:end-1) + NPBK2*kron(x(1:end-1),x(1:end-1)), ... 107 | x(1:end-1).'*Qc*x(1:end-1) + ... 108 | computeU2(x(1:end-1)).'*R*computeU2(x(1:end-1)) ); 109 | 110 | [t2,z2] = ode23s( rhs_k2, [0 tInfinity], [x0;0], options ); 111 | 112 | figure(12); hold on 113 | closedLoopCost2 = z2(end,end); 114 | 115 | Zz = z2(:,1:end-1)*sqMinv; %#ok 116 | Zval = Zz(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); 117 | surf(xNodes,t2,Zval) 118 | xlabel('z'); ylabel('time') 119 | title('Closed-Loop Simulation with k^{[2]}') 120 | 121 | c3 = c2 + v{3}*kron(x0,kron(x0,x0)); 122 | fprintf('Approx regulator cost to v^[3]: %15.10f\n',c3) 123 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost2); 124 | 125 | end 126 | 127 | if ( degree>2 ) 128 | %--------------------------------------------------------------------------- 129 | % Cubic feedback 130 | %--------------------------------------------------------------------------- 131 | NPBK3 = Bc*k{3}; 132 | computeU3 = @(x) k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 133 | 134 | rhs_k3 = @(t,x) vertcat( APBK*x(1:end-1) + ... 135 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 136 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)), ... 137 | x(1:end-1).'*Qc*x(1:end-1) + ... 138 | computeU3(x(1:end-1)).'*R*computeU3(x(1:end-1)) ); 139 | 140 | [t3,z3] = ode23s( rhs_k3, [0 tInfinity], [x0;0], options ); 141 | 142 | figure(13); hold on 143 | closedLoopCost3 = z3(end,end); 144 | 145 | Zz = z3(:,1:end-1)*sqMinv; %#ok 146 | Zval = Zz(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); 147 | surf(xNodes,t3,Zval) 148 | xlabel('z'); ylabel('time') 149 | title('Closed-Loop Simulation with k^{[3]}') 150 | 151 | c4 = c3 + v{4}*kron(kron(kron(x0,x0),x0),x0); 152 | fprintf('Approx regulator cost to v^[4]: %15.10f\n',c4) 153 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost3); 154 | end 155 | 156 | if ( degree>3 ) 157 | %--------------------------------------------------------------------------- 158 | % Quartic feedback 159 | %--------------------------------------------------------------------------- 160 | computeU4 = @(x) k{1}*x + ... 161 | k{2}*kron(x,x) + ... 162 | k{3}*kron(kron(x,x),x) + ... 163 | k{4}*kron(kron(kron(x,x),x),x); 164 | 165 | computeU4_4 = @(x) k{4}*kron(kron(kron(x,x),x),x); 166 | 167 | rhs_k4 = @(t,x) vertcat( APBK*x(1:end-1) + ... 168 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 169 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 170 | Bc*computeU4_4(x(1:end-1)) , ... 171 | x(1:end-1).'*Qc*x(1:end-1) + ... 172 | computeU4(x(1:end-1)).'*R*computeU4(x(1:end-1)) ); 173 | 174 | [t4,z4] = ode23s( rhs_k4, [0 tInfinity], [x0;0], options ); 175 | 176 | figure(14); hold on 177 | closedLoopCost4 = z4(end,end); 178 | 179 | Zz = z4(:,1:end-1)*sqMinv; %#ok 180 | Zval = Zz(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); 181 | surf(xNodes,t4,Zval) 182 | xlabel('z'); ylabel('time') 183 | title('Closed-Loop Simulation with k^{[4]}') 184 | 185 | c5 = c4 + v{5}*kron(kron(kron(kron(x0,x0),x0),x0),x0); 186 | fprintf('Approx regulator cost to v^[5]: %15.10f\n',c5) 187 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost4); 188 | 189 | end 190 | 191 | if ( degree>4 ) 192 | %----------------------------------------------------------------------------- 193 | % Quintic feedback 194 | %----------------------------------------------------------------------------- 195 | computeU5 = @(x) k{1}*x + ... 196 | k{2}*kron(x,x) + ... 197 | k{3}*kron(kron(x,x),x) + ... 198 | k{4}*kron(kron(kron(x,x),x),x) + ... 199 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 200 | computeU4_5 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 201 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 202 | rhs_k5 = @(t,x) [ APBK*x(1:end-1) + ... 203 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 204 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 205 | Bc*computeU4_5(x(1:end-1)); ... 206 | x(1:end-1).'*Qc*x(1:end-1) + ... 207 | computeU5(x(1:end-1)).'*R*computeU5(x(1:end-1)) ]; 208 | 209 | [t5,z5] = ode23s( rhs_k5, [0 tInfinity], [x0;0], options ); 210 | 211 | figure(15); hold on 212 | closedLoopCost5 = z5(end,end); 213 | 214 | Zz = z5(:,1:end-1)*sqMinv; %#ok 215 | Zval = Zz(:,1:2:end-1); Zval(:,end+1) = Zval(:,1); 216 | surf(xNodes,t5,Zval) 217 | xlabel('z'); ylabel('time') 218 | title('Closed-Loop Simulation with k^{[5]}') 219 | 220 | c6 = c5 + v{6}*kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0); 221 | fprintf('Approx regulator cost to v^[6]: %15.10f\n',c6) 222 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',closedLoopCost5); 223 | 224 | end 225 | 226 | end 227 | 228 | -------------------------------------------------------------------------------- /examples/example06.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------------------------------------------- 2 | %EXAMPLE06 Compares feedback strategies for a simple first-order problem. 3 | % 4 | % n is the order of the system 5 | % m is the number of equally spaced control inputs 6 | % degree is the maximum degree to be tested. 7 | %------------------------------------------------------------------------------- 8 | %% 9 | n = 1; 10 | m = 1; 11 | degree = 5; 12 | fprintf('example06: the maximum degree is %d\n',degree); 13 | 14 | A = -0.1; 15 | N = -4.0; 16 | B = 1.0; 17 | Q = 1.0; 18 | R = 1.0; 19 | 20 | T = 30; % this is T=\infty... 21 | x0 = 0.125; 22 | 23 | tic 24 | [k,v] = qqr(A,B,Q,R,N,degree); 25 | toc 26 | 27 | disp([A B Q R N]) 28 | 29 | options = odeset('AbsTol',1e-7); 30 | 31 | runOpen = false; 32 | if ( runOpen ) 33 | %--------------------------------------------------------------------------- 34 | % Open loop simulation 35 | %--------------------------------------------------------------------------- 36 | tic 37 | rhs_open = @(t,x) [ A*x(1:end-1) + N*kron(x(1:end-1),x(1:end-1));... 38 | x(1:end-1).'*Q*x(1:end-1) ]; 39 | 40 | [t,x] = ode15s( rhs_open, [0 T], [x0;0], options ); 41 | figure(1); hold on 42 | plot(t,x) 43 | xlabel('time'); ylabel('x') 44 | title('Open Loop Simulation') 45 | 46 | fprintf('Open Loop Cost (0,T) is %g\n\n',x(end,end)); 47 | toc 48 | end 49 | 50 | runClosed = false; 51 | if ( runClosed ) 52 | %----------------------------------------------------------------------------- 53 | % Linear feedback 54 | %----------------------------------------------------------------------------- 55 | v2 = v{2}; 56 | APBK = A + B*k{1}; 57 | computeU1 = @(x) k{1}*x; 58 | rhs_k1 = @(t,x) [ APBK*x(1:end-1) + N*kron(x(1:end-1),x(1:end-1)); ... 59 | x(1:end-1).'*Q*x(1:end-1) + ... 60 | computeU1(x(1:end-1)).'*R*computeU1(x(1:end-1)) ]; 61 | 62 | [t1,x1] = ode23s( rhs_k1, [0 T], [x0;0], options ); 63 | figure(10); hold on 64 | plot(t1,x1(:,1)) 65 | xlabel('x'); ylabel('time') 66 | title('Closed-Loop Simulation with k^{[1]}') 67 | 68 | c2 = v2*kron(x0,x0); 69 | fprintf('Approx regulator cost to v^[2]: %g\n',c2) 70 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x1(end,end)); 71 | 72 | if ( degree>1 ) 73 | %--------------------------------------------------------------------------- 74 | % Quadratic feedback 75 | %--------------------------------------------------------------------------- 76 | v3 = v{3}; 77 | NPBK2 = N + B*k{2}; 78 | computeU2 = @(x) k{1}*x + k{2}*kron(x,x); 79 | rhs_k2 = @(t,x) [ APBK*x(1:end-1) + NPBK2*kron(x(1:end-1),x(1:end-1)); ... 80 | x(1:end-1).'*Q*x(1:end-1) + ... 81 | computeU2(x(1:end-1)).'*R*computeU2(x(1:end-1)) ]; 82 | 83 | [t2,x2] = ode23s( rhs_k2, [0 T], [x0;0], options ); 84 | 85 | figure(20); hold on 86 | plot(t2,x2(:,1:end-1)) 87 | xlabel('x'); ylabel('time') 88 | title('Closed-Loop Simulation with k^{[2]}') 89 | 90 | c3 = c2 + v3*kron(x0,kron(x0,x0)); 91 | fprintf('Approx regulator cost to v^[3]: %g\n',c3) 92 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x2(end,end)); 93 | 94 | end 95 | 96 | if ( degree>2 ) 97 | %--------------------------------------------------------------------------- 98 | % Cubic feedback 99 | %--------------------------------------------------------------------------- 100 | v4 = v{4}; 101 | computeU3 = @(x) k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 102 | 103 | rhs_k3 = @(t,x) [ APBK*x(1:end-1) + ... 104 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 105 | B*k{3}*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)); ... 106 | x(1:end-1).'*Q*x(1:end-1) + ... 107 | computeU3(x(1:end-1)).'*R*computeU3(x(1:end-1)) ]; 108 | 109 | [t3,x3] = ode23s( rhs_k3, [0 T], [x0;0], options ); 110 | 111 | figure(30); hold on 112 | plot(t3,x3(:,1:end-1)) 113 | xlabel('x'); ylabel('time') 114 | title('Closed-Loop Simulation with k^{[3]}') 115 | 116 | c4 = c3 + v4*kron(kron(kron(x0,x0),x0),x0); 117 | fprintf('Approx regulator cost to v^[4]: %g\n',c4) 118 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x3(end,end)); 119 | end 120 | 121 | 122 | if ( degree>3 ) 123 | %--------------------------------------------------------------------------- 124 | % Quartic feedback 125 | %--------------------------------------------------------------------------- 126 | v5 = v{5}; 127 | computeU4 = @(x) k{1}*x + ... 128 | k{2}*kron(x,x) + ... 129 | k{3}*kron(kron(x,x),x) + ... 130 | k{4}*kron(kron(kron(x,x),x),x); 131 | 132 | computeU3_4 = @(x) k{3}*kron(kron(x,x),x) + ... 133 | k{4}*kron(kron(kron(x,x),x),x); 134 | 135 | rhs_k4 = @(t,x) [ APBK*x(1:end-1) + ... 136 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 137 | B*computeU3_4(x(1:end-1)) ; ... 138 | x(1:end-1).'*Q*x(1:end-1) + ... 139 | computeU4(x(1:end-1)).'*R*computeU4(x(1:end-1)) ]; 140 | 141 | [t4,x4] = ode23s( rhs_k4, [0 T], [x0;0], options ); 142 | 143 | figure(40); hold on 144 | plot(t4,x4(:,1:end-1)) 145 | xlabel('x'); ylabel('time') 146 | title('Closed-Loop Simulation with k^{[4]}') 147 | 148 | c5 = c4 + v5*kron(kron(kron(kron(x0,x0),x0),x0),x0); 149 | fprintf('Approx regulator cost to v^[5]: %g\n',c5) 150 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x4(end,end)); 151 | 152 | end 153 | 154 | if ( degree>4 ) 155 | %--------------------------------------------------------------------------- 156 | % Quintic feedback 157 | %--------------------------------------------------------------------------- 158 | v6 = v{6}; 159 | computeU5 = @(x) k{1}*x + ... 160 | k{2}*kron(x,x) + ... 161 | k{3}*kron(kron(x,x),x) + ... 162 | k{4}*kron(kron(kron(x,x),x),x) + ... 163 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 164 | computeU3_5 = @(x) k{3}*kron(kron(x,x),x) + ... 165 | k{4}*kron(kron(kron(x,x),x),x) + ... 166 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 167 | rhs_k5 = @(t,x) [ APBK*x(1:end-1) + ... 168 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 169 | B*computeU3_5(x(1:end-1)); ... 170 | x(1:end-1).'*Q*x(1:end-1) + ... 171 | computeU5(x(1:end-1)).'*R*computeU5(x(1:end-1)) ]; 172 | 173 | [t5,x5] = ode23s( rhs_k5, [0 T], [x0;0], options ); 174 | 175 | figure(50); hold on 176 | plot(t5,x5(:,1:end-1)) 177 | xlabel('x'); ylabel('time') 178 | title('Closed-Loop Simulation with k^{[5]}') 179 | 180 | c6 = c5 + v6*kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0); 181 | fprintf('Approx regulator cost to v^[6]: %g\n',c6) 182 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x5(end,end)); 183 | 184 | end 185 | end % runClosed 186 | 187 | figure 188 | x0 = linspace(-.1,.1,201); 189 | c2 = v{2}*x0.^2; 190 | c3 = c2 + v{3}*x0.^3; 191 | c4 = c3 + v{4}*x0.^4; 192 | c5 = c4 + v{5}*x0.^5; 193 | c6 = c5 + v{6}*x0.^6; 194 | plot(x0,c2,x0,c3,x0,c4,x0,c5,x0,c6) 195 | legend('v2','v3','v4','v5','v6') 196 | 197 | % TO DO: add_derivatives_along_solutions 198 | 199 | % 200 | % [t3,x3] = ode23s( rhs_k3, [0 T], x0 ); 201 | % figure(13); hold on 202 | % plot3(x3(1,1),x3(1,2),x3(1,3),'*') 203 | % plot3(x3(:,1),x3(:,2),x3(:,3),'k') 204 | % plot3(0,0,0,'o') 205 | % view([1 1 1]) 206 | % 207 | % c4 = c3 + v4*kron(x0,kron(x0,kron(x0,x0))); 208 | % fprintf('approx. regulator cost to k3: %g\n',c4) 209 | % 210 | % figure(31) 211 | % plot(t1,x1(:,1),'r',t2,x2(:,1),'b',t3,x3(:,1),'k') 212 | % legend('degree 1','degree 2','degree 3') 213 | % figure(32) 214 | % plot(t1,x1(:,2),'r',t2,x2(:,2),'b',t3,x3(:,2),'k') 215 | % legend('degree 1','degree 2','degree 3') 216 | % figure(33) 217 | % plot(t1,x1(:,3),'r',t2,x2(:,3),'b',t3,x3(:,3),'k') 218 | % legend('degree 1','degree 2','degree 3') 219 | % 220 | % figure(41) 221 | % u1 = k{1}*x1.'; 222 | % 223 | % u2 = k{1}*x2.'; 224 | % for i=1:length(u2) 225 | % u2(i) = u2(i) + k{2}*kron(x2(i,:).',x2(i,:).'); 226 | % end 227 | % 228 | % u3 = k{1}*x3.'; 229 | % for i=1:length(u3) 230 | % u3(i) = u3(i) + k{2}*kron(x3(i,:).',x3(i,:).') ... 231 | % + k{3}*kron(x3(i,:).',kron(x3(i,:).',x3(i,:).')); 232 | % end 233 | % 234 | % plot(t1,u1,'r',t2,u2,'b',t3,u3,'k') 235 | % legend('degree 1','degree 2','degree 3') 236 | % 237 | % %end 238 | % 239 | -------------------------------------------------------------------------------- /runComparisons.m: -------------------------------------------------------------------------------- 1 | % This is the script used to perform testcase comparisons for qqr, NST, and 2 | % optionally, the full Kronecker form, for the QQR that were reported in 3 | % 4 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator: 5 | % Proc. American Conference on Control, Denver, CO, 2020 (submitted). 6 | % and 7 | % 8 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator 9 | % IEEE Transactions on Automatic Control (submitted). 10 | % 11 | % Solutions from NST are provided in the ka and py arrays. 12 | % 13 | % if testFull==true 14 | % - solutions from AlbrekhtKronQQR are provided in the kF and vF arrays. 15 | % - this can require a lot of memory and CPU time, so keep n, m, and the 16 | % degree variables small. It has not yet been optimized to build an 17 | % upper triangular system but primarily used for testing and debugging. 18 | %% 19 | % Variables: n, m, degree, A, B, N, Q, R and 20 | % Flag: testFull must be specified 21 | % 22 | % We assume that [k,v] = qqr(A,B,Q,R,N, degree) has already been called 23 | % and the problem sizes are small enough so NST is feasible. 24 | % 25 | % Author: Jeff Borggaard, Virginia Tech 26 | % 27 | % Part of the QQR library. 28 | %% 29 | 30 | verbose = true; % writes out Kron<->CT mapping times. 31 | 32 | setKroneckerToolsPath 33 | setNSTpath 34 | addpath('./testScripts') 35 | 36 | % this script is fairly useless without the NST solution 37 | [ka,py] = runNST(A,B,Q,R,N,degree); 38 | py = 2*py; % NST assumes an additional factor of 1/2 that we don't. 39 | 40 | %% Calculate via the full Kronecker product formula 41 | if ( exist('testFull','var') && testFull ) 42 | tic 43 | [kF,vF] = AlbrekhtKronQQR(A,B,Q,R,N,min(degree,4)); 44 | comp = toc; 45 | disp('') 46 | fprintf('AlbrekhtKronQQR solution required %g seconds\n',comp) 47 | end 48 | 49 | 50 | if ( degree>1 ) 51 | tic; 52 | C2 = CT2Kron(n,2); 53 | S2 = Kron2CT(n,2); 54 | 55 | C3 = CT2Kron(n,3); 56 | S3 = Kron2CT(n,3); 57 | CTtime = toc; 58 | if ( verbose ) 59 | fprintf('CT to Kron mappings (2+3) required %g seconds\n',CTtime) 60 | end 61 | 62 | k2 = k{2}; 63 | v3 = v{3}; 64 | 65 | idx1 = n; 66 | idx2 = idx1 + n*(n+1)/2; 67 | idx3 = idx2 + n*(n+1)*(n+2)/6; 68 | idx4 = idx3 + n*(n+1)*(n+2)*(n+3)/24; 69 | idx5 = idx4 + n*(n+1)*(n+2)*(n+3)*(n+4)/120; 70 | idx6 = idx5 + n*(n+1)*(n+2)*(n+3)*(n+4)*(n+5)/720; 71 | idx7 = idx6 + n*(n+1)*(n+2)*(n+3)*(n+4)*(n+5)*(n+6)/5040; 72 | 73 | ka2 = ka(:,idx1 +1:idx2 ); 74 | py3 = py( idx2-idx1+1:idx3-idx1); 75 | 76 | e_k2 = norm( ka2 - k2*S2' ); 77 | fprintf('NST: The norm of k^[2] is %g\n',norm(ka2)); 78 | fprintf('NST: The norm of v^[3] is %g\n',norm(py3)); 79 | fprintf('tensor: The relative error in k^[2] is %g\n',e_k2/norm(ka2)); 80 | e_p3 = norm( py3 - v3*S3' ); 81 | fprintf('tensor: The relative error in v^[3] is %g\n\n',e_p3/norm(py3)); 82 | 83 | if ( testFull ) 84 | vF2 = vF{2}; 85 | vF3 = vF{3}; 86 | 87 | kF1 = kF{1}; 88 | kF2 = kF{2}; 89 | 90 | % Convert to compact Taylor format for comparison with the NST toolbox 91 | % solution. 92 | e_k2 = norm( ka2 - kF2*S2' ); 93 | fprintf('FullKr: The relative error in k^[2] is %g\n',e_k2/norm(ka2)); 94 | 95 | e_p3 = norm( py3 - vF3*S3' ); 96 | fprintf('FullKr: The relative error in v^[3] is %g\n\n',e_p3/norm(py3)); 97 | end 98 | 99 | end 100 | 101 | if ( degree>2 ) 102 | 103 | tic; 104 | C4 = CT2Kron(n,4); 105 | S4 = Kron2CT(n,4); 106 | CTtime = toc; 107 | if ( verbose ) 108 | fprintf('CT to Kron mappings (4) require %g seconds\n',CTtime) 109 | end 110 | 111 | % if ( testTensor ) 112 | % Al{4} = ABKT; 113 | % r2 = R(:)/2; 114 | % bb = -( kron( (B*kk2+N).', eye(n^2) ) + ... 115 | % kron( kron( eye(n ), (B*kk2+N).'), eye(n) ) + ... 116 | % kron( eye(n^2), (B*kk2+N).' ) )*vv3(:) ... 117 | % - kron(kk2.',kk2.')*r2 ; 118 | % v4 = lyapunov_recursive(Al,reshape(bb,n,n,n,n)); 119 | % comp = comp+toc; 120 | % fprintf(' tensorized solution required %g seconds\n',comp); 121 | % 122 | % res = zeros(n*n*n,m); 123 | % for i=1:m 124 | % GG = ( kron( B(:,i).', eye(n^3) ) + ... 125 | % kron( eye(n ), kron(B(:,i).', eye(n^2) ) ) + ... 126 | % kron( eye(n^2), kron(B(:,i).', eye(n ) ) ) + ... 127 | % kron( eye(n^3), B(:,i).' ) ); 128 | % GG = C3*S3*GG; 129 | % res(:,i) = -GG*vv4(:); 130 | % end 131 | % k3 = R\res.'; 132 | % end 133 | 134 | k3 = k{3}; 135 | v4 = v{4}; 136 | 137 | ka3 = ka(:,idx2 +1:idx3 ); 138 | py4 = py( idx3-idx1+1:idx4-idx1); 139 | fprintf('NST: The norm of k^[3] is %g\n',norm(ka3)); 140 | fprintf('NST: The norm of v^[4] is %g\n',norm(py4)); 141 | 142 | e_k3 = norm( ka3 - k3*S3' ); 143 | fprintf('tensor: The relative error in k^[3] is %g\n',e_k3/norm(ka3)); 144 | e_p4 = norm( py4 - v4*S4' ); 145 | fprintf('tensor: The relative error in v^[4] is %g\n\n',e_p4/norm(py4)); 146 | 147 | 148 | if ( testFull ) 149 | kF3 = kF{3}; 150 | vF4 = vF{4}; 151 | 152 | % Convert to compact Taylor format for comparison with the NST toolbox 153 | % solution. 154 | e_k3 = norm( ka3 - kF3*S3' ); 155 | fprintf('FullKr: The relative error in k^[3] is %g\n',e_k3/norm(ka3)); 156 | 157 | e_p4 = norm( py4 - vF4*S4' ); 158 | fprintf('FullKr: The relative error in v^[4] is %g\n\n',e_p4/norm(py4)); 159 | end 160 | end 161 | 162 | if ( degree>3 ) 163 | 164 | tic; 165 | S5 = Kron2CT(n,5); 166 | CTtime = toc; 167 | if ( verbose ) 168 | fprintf('CT to Kron mappings (5) require %g seconds\n',CTtime) 169 | end 170 | 171 | % if ( testTensor ) 172 | % Al{5} = ABKT; 173 | % bb = -( kron( (B*kk2+N).', eye(n^3) ) + ... 174 | % kron( kron( eye(n ), (B*kk2+N).' ), eye(n^2) ) + ... 175 | % kron( kron( eye(n^2), (B*kk2+N).' ), eye(n) ) + ... 176 | % kron( eye(n^3), (B*kk2+N).' ) )*vv4(:) ... 177 | % -( kron( (B*kk3 ).', eye(n^2) ) + ... 178 | % kron( kron( eye(n ), (B*kk3 ).' ), eye(n ) ) + ... 179 | % kron( eye(n^2), (B*kk3 ).' ) )*vv3(:) ... 180 | % -( kron(kk2.',kk3.') + kron(kk3.',kk2.') )*r2 ; 181 | % vv5 = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n)); 182 | % comp = comp+toc; 183 | % fprintf(' tensorized solution required %g seconds\n',comp); 184 | % 185 | % res = zeros(n*n*n,m); 186 | % for i=1:m 187 | % GG = ( kron( B(:,i).',eye(n^4) ) + ... 188 | % kron( eye(n ),kron(B(:,i).',eye(n^3) ) ) + ... 189 | % kron( eye(n^2),kron(B(:,i).',eye(n^2) ) ) + ... 190 | % kron( eye(n^3),kron(B(:,i).',eye(n ) ) ) + ... 191 | % kron( eye(n^4), B(:,i).' ) ); 192 | % GG = C4*S4*GG; 193 | % res(:,i) = -GG*v5; 194 | % end 195 | % kk4 = R\res.'; 196 | % end 197 | 198 | k4 = k{4}; 199 | v5 = v{5}; 200 | 201 | ka4 = ka(:,idx3 +1:idx4 ); 202 | py5 = py( idx4-idx1+1:idx5-idx1); 203 | fprintf('NST: The norm of k^[4] is %g\n',norm(ka4)); 204 | fprintf('NST: The norm of v^[5] is %g\n',norm(py5)); 205 | 206 | e_k4 = norm( ka4 - k4*S4' ); 207 | fprintf('tensor: The relative error in k^[4] is %g\n',e_k4/norm(ka4)); 208 | 209 | e_p5 = norm( py5 - v5*S5' ); 210 | fprintf('tensor: The relative error in v^[5] is %g\n\n',e_p5/norm(py5)); 211 | 212 | if ( testFull ) 213 | kF4 = kF{4}; 214 | vF5 = vF{5}; 215 | 216 | % Convert to compact Taylor format for comparison with the NST toolbox 217 | % solution. 218 | e_k4 = norm( ka4 - kF4*S4' ); 219 | fprintf('FullKr: The relative error in k^[4] is %g\n',e_k4/norm(ka4)); 220 | 221 | e_p5 = norm( py5 - vF5*S5' ); 222 | fprintf('FullKr: The relative error in v^[5] is %g\n\n',e_p5/norm(py5)); 223 | end 224 | end 225 | 226 | if ( degree>4 ) 227 | tic; 228 | S6 = Kron2CT(n,6); 229 | CTtime = toc; 230 | if ( verbose ) 231 | fprintf('CT to Kron mappings (6) require %g seconds\n',CTtime) 232 | end 233 | 234 | k5 = k{5}; 235 | v6 = v{6}; 236 | 237 | ka5 = ka(:,idx4 +1:idx5 ); 238 | py6 = py( idx5-idx1+1:idx6-idx1); 239 | fprintf('NST: The norm of k^[5] is %g\n',norm(ka5)); 240 | fprintf('NST: The norm of v^[6] is %g\n',norm(py6)); 241 | 242 | e_k5 = norm( ka5 - k5*S5' ); 243 | fprintf('tensor: The relative error in k^[5] is %g\n',e_k5/norm(ka5)); 244 | 245 | e_p6 = norm( py6 - v6*S6' ); 246 | fprintf('tensor: The relative error in v^[6] is %g\n\n',e_p6/norm(py6)); 247 | end 248 | 249 | if ( degree>5 ) 250 | tic; 251 | S7 = Kron2CT(n,7); 252 | CTtime = toc; 253 | if ( verbose ) 254 | fprintf('CT to Kron mappings (7) require %g seconds\n',CTtime) 255 | end 256 | 257 | k6 = k{6}; 258 | v7 = v{7}; 259 | 260 | ka6 = ka(:,idx5 +1:idx6 ); 261 | py7 = py( idx6-idx1+1:idx7-idx1); 262 | fprintf('NST: The norm of k^[6] is %g\n',norm(ka6)); 263 | fprintf('NST: The norm of v^[7] is %g\n',norm(py7)); 264 | 265 | e_k6 = norm( ka6 - k6*S6' ); 266 | fprintf('tensor: The relative error in k^[6] is %g\n',e_k6/norm(ka6)); 267 | 268 | e_p7 = norm( py7 - v7*S7' ); 269 | fprintf('tensor: The relative error in v^[7] is %g\n\n',e_p7/norm(py7)); 270 | end 271 | 272 | if ( degree>6 ) 273 | 274 | k7 = k{7}; 275 | 276 | ka7 = ka(:,idx6 +1:idx7 ); 277 | fprintf('NST: The norm of k^[7] is %g\n',norm(ka7)); 278 | 279 | e_k7 = norm( ka7 - k7*S7' ); 280 | fprintf('tensor: The relative error in k^[7] is %g\n\n',e_k7/norm(ka7)); 281 | end 282 | 283 | 284 | % sometimes these errors are high, but the relative error is then low. 285 | % possibly due to factors like nearly singular R, nearly uncontrollable 286 | % (or extensions of this notion to the higher degree case?) 287 | % other times, we are computing a relative error for a quantity 288 | % that should be zero. 289 | -------------------------------------------------------------------------------- /runNSTcomparisons.m: -------------------------------------------------------------------------------- 1 | % This is the script used to perform testcase comparisons for qqr, NST, and 2 | % optionally, the full Kronecker form, for the QQR that were reported in 3 | % 4 | % Borggaard and Zietsman, The Quadratic-Quadratic Regulator: 5 | % Proc. American Conference on Control, Denver, CO, 2020. 6 | % Available at arxiv.org/abs/1910.03396 7 | % 8 | % This can also be used to verify the QQR software. 9 | % 10 | % Solutions from NST are provided in the ka and py arrays. 11 | % 12 | % if testFull==true 13 | % - solutions from AlbrekhtKronQQR are provided in the kF and vF arrays. 14 | % - this can require a lot of memory and CPU time, so keep n, m, and the 15 | % degree variables small. It has not yet been optimized to build an 16 | % upper triangular system but primarily used for testing and debugging. 17 | %% 18 | % Variables: n, m, degree, A, B, N, Q, R and 19 | % Flag: testFull must be specified 20 | % 21 | % We assume that [k,v] = qqr(A,B,Q,R,N, degree) has already been called 22 | % and the problem sizes are small enough so NST is feasible. 23 | % 24 | % Author: Jeff Borggaard, Virginia Tech 25 | % 26 | % Part of the QQR library. 27 | %% 28 | 29 | verbose = true; % writes out Kron<->CT mapping times. 30 | 31 | setKroneckerToolsPath 32 | setNSTpath 33 | addpath('./testScripts') 34 | 35 | % this script is fairly useless without the NST solution 36 | if ( exist('Nxu','var') ) % the qbqr case 37 | [ka,py] = runNST(A,B,Q,R,Nxx,degree,Nxu,Nuu); 38 | elseif (exist('N2','var') && exist('N3','var')) % the cqr case 39 | [ka,py] = runNST3(A,B,Q,R,N2,N3,degree); 40 | elseif (iscell(N)) % the pqr case 41 | [ka,py] = runNSTl(A,B,Q,R,N,degree); 42 | else % the qqr case 43 | [ka,py] = runNST(A,B,Q,R,N,degree); 44 | end 45 | 46 | py = 2*py; % NST assumes an additional factor of 1/2 that we don't. 47 | 48 | %% Calculate via the full Kronecker product formula 49 | if ( exist('testFull','var') && testFull ) 50 | tic 51 | [kF,vF] = AlbrekhtKronQQR(A,B,Q,R,N,min(degree,4)); 52 | comp = toc; 53 | disp('') 54 | fprintf('AlbrekhtKronQQR solution required %g seconds\n',comp) 55 | end 56 | 57 | 58 | if ( degree>1 ) 59 | tic; 60 | C2 = CT2Kron(n,2); 61 | S2 = Kron2CT(n,2); 62 | 63 | C3 = CT2Kron(n,3); 64 | S3 = Kron2CT(n,3); 65 | CTtime = toc; 66 | if ( verbose ) 67 | fprintf('CT to Kron mappings (2+3) required %g seconds\n',CTtime) 68 | end 69 | 70 | k2 = k{2}; 71 | v3 = v{3}; 72 | 73 | idx1 = n; 74 | idx2 = idx1 + n*(n+1)/2; 75 | idx3 = idx2 + n*(n+1)*(n+2)/6; 76 | idx4 = idx3 + n*(n+1)*(n+2)*(n+3)/24; 77 | idx5 = idx4 + n*(n+1)*(n+2)*(n+3)*(n+4)/120; 78 | idx6 = idx5 + n*(n+1)*(n+2)*(n+3)*(n+4)*(n+5)/720; 79 | idx7 = idx6 + n*(n+1)*(n+2)*(n+3)*(n+4)*(n+5)*(n+6)/5040; 80 | 81 | ka2 = ka(:,idx1 +1:idx2 ); 82 | py3 = py( idx2-idx1+1:idx3-idx1); 83 | 84 | e_k2 = norm( ka2 - k2*S2' ); 85 | fprintf('NST: The norm of k^[2] is %g\n',norm(ka2)); 86 | fprintf('NST: The norm of v^[3] is %g\n',norm(py3)); 87 | fprintf('tensor: The relative error in k^[2] is %g\n',e_k2/norm(ka2)); 88 | e_p3 = norm( py3 - v3*S3' ); 89 | fprintf('tensor: The relative error in v^[3] is %g\n\n',e_p3/norm(py3)); 90 | 91 | if ( exist('testFull','var') && testFull ) 92 | vF2 = vF{2}; 93 | vF3 = vF{3}; 94 | 95 | kF1 = kF{1}; 96 | kF2 = kF{2}; 97 | 98 | % Convert to compact Taylor format for comparison with the NST toolbox 99 | % solution. 100 | e_k2 = norm( ka2 - kF2*S2' ); 101 | fprintf('FullKr: The relative error in k^[2] is %g\n',e_k2/norm(ka2)); 102 | 103 | e_p3 = norm( py3 - vF3*S3' ); 104 | fprintf('FullKr: The relative error in v^[3] is %g\n\n',e_p3/norm(py3)); 105 | end 106 | 107 | end 108 | 109 | if ( degree>2 ) 110 | 111 | tic; 112 | C4 = CT2Kron(n,4); 113 | S4 = Kron2CT(n,4); 114 | CTtime = toc; 115 | if ( verbose ) 116 | fprintf('CT to Kron mappings (4) require %g seconds\n',CTtime) 117 | end 118 | 119 | % if ( testTensor ) 120 | % Al{4} = ABKT; 121 | % r2 = R(:)/2; 122 | % bb = -( kron( (B*kk2+N).', eye(n^2) ) + ... 123 | % kron( kron( eye(n ), (B*kk2+N).'), eye(n) ) + ... 124 | % kron( eye(n^2), (B*kk2+N).' ) )*vv3(:) ... 125 | % - kron(kk2.',kk2.')*r2 ; 126 | % v4 = lyapunov_recursive(Al,reshape(bb,n,n,n,n)); 127 | % comp = comp+toc; 128 | % fprintf(' tensorized solution required %g seconds\n',comp); 129 | % 130 | % res = zeros(n*n*n,m); 131 | % for i=1:m 132 | % GG = ( kron( B(:,i).', eye(n^3) ) + ... 133 | % kron( eye(n ), kron(B(:,i).', eye(n^2) ) ) + ... 134 | % kron( eye(n^2), kron(B(:,i).', eye(n ) ) ) + ... 135 | % kron( eye(n^3), B(:,i).' ) ); 136 | % GG = C3*S3*GG; 137 | % res(:,i) = -GG*vv4(:); 138 | % end 139 | % k3 = R\res.'; 140 | % end 141 | 142 | k3 = k{3}; 143 | v4 = v{4}; 144 | 145 | ka3 = ka(:,idx2 +1:idx3 ); 146 | py4 = py( idx3-idx1+1:idx4-idx1); 147 | fprintf('NST: The norm of k^[3] is %g\n',norm(ka3)); 148 | fprintf('NST: The norm of v^[4] is %g\n',norm(py4)); 149 | 150 | e_k3 = norm( ka3 - k3*S3' ); 151 | fprintf('tensor: The relative error in k^[3] is %g\n',e_k3/norm(ka3)); 152 | e_p4 = norm( py4 - v4*S4' ); 153 | fprintf('tensor: The relative error in v^[4] is %g\n\n',e_p4/norm(py4)); 154 | 155 | 156 | if ( exist('testFull','var') && testFull ) 157 | kF3 = kF{3}; 158 | vF4 = vF{4}; 159 | 160 | % Convert to compact Taylor format for comparison with the NST toolbox 161 | % solution. 162 | e_k3 = norm( ka3 - kF3*S3' ); 163 | fprintf('FullKr: The relative error in k^[3] is %g\n',e_k3/norm(ka3)); 164 | 165 | e_p4 = norm( py4 - vF4*S4' ); 166 | fprintf('FullKr: The relative error in v^[4] is %g\n\n',e_p4/norm(py4)); 167 | end 168 | end 169 | 170 | if ( degree>3 ) 171 | 172 | tic; 173 | S5 = Kron2CT(n,5); 174 | CTtime = toc; 175 | if ( verbose ) 176 | fprintf('CT to Kron mappings (5) require %g seconds\n',CTtime) 177 | end 178 | 179 | % if ( testTensor ) 180 | % Al{5} = ABKT; 181 | % bb = -( kron( (B*kk2+N).', eye(n^3) ) + ... 182 | % kron( kron( eye(n ), (B*kk2+N).' ), eye(n^2) ) + ... 183 | % kron( kron( eye(n^2), (B*kk2+N).' ), eye(n) ) + ... 184 | % kron( eye(n^3), (B*kk2+N).' ) )*vv4(:) ... 185 | % -( kron( (B*kk3 ).', eye(n^2) ) + ... 186 | % kron( kron( eye(n ), (B*kk3 ).' ), eye(n ) ) + ... 187 | % kron( eye(n^2), (B*kk3 ).' ) )*vv3(:) ... 188 | % -( kron(kk2.',kk3.') + kron(kk3.',kk2.') )*r2 ; 189 | % vv5 = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n)); 190 | % comp = comp+toc; 191 | % fprintf(' tensorized solution required %g seconds\n',comp); 192 | % 193 | % res = zeros(n*n*n,m); 194 | % for i=1:m 195 | % GG = ( kron( B(:,i).',eye(n^4) ) + ... 196 | % kron( eye(n ),kron(B(:,i).',eye(n^3) ) ) + ... 197 | % kron( eye(n^2),kron(B(:,i).',eye(n^2) ) ) + ... 198 | % kron( eye(n^3),kron(B(:,i).',eye(n ) ) ) + ... 199 | % kron( eye(n^4), B(:,i).' ) ); 200 | % GG = C4*S4*GG; 201 | % res(:,i) = -GG*v5; 202 | % end 203 | % kk4 = R\res.'; 204 | % end 205 | 206 | k4 = k{4}; 207 | v5 = v{5}; 208 | 209 | ka4 = ka(:,idx3 +1:idx4 ); 210 | py5 = py( idx4-idx1+1:idx5-idx1); 211 | fprintf('NST: The norm of k^[4] is %g\n',norm(ka4)); 212 | fprintf('NST: The norm of v^[5] is %g\n',norm(py5)); 213 | 214 | e_k4 = norm( ka4 - k4*S4' ); 215 | fprintf('tensor: The relative error in k^[4] is %g\n',e_k4/norm(ka4)); 216 | 217 | e_p5 = norm( py5 - v5*S5' ); 218 | fprintf('tensor: The relative error in v^[5] is %g\n\n',e_p5/norm(py5)); 219 | 220 | if ( exist('testFull','var') && testFull ) 221 | kF4 = kF{4}; 222 | vF5 = vF{5}; 223 | 224 | % Convert to compact Taylor format for comparison with the NST toolbox 225 | % solution. 226 | e_k4 = norm( ka4 - kF4*S4' ); 227 | fprintf('FullKr: The relative error in k^[4] is %g\n',e_k4/norm(ka4)); 228 | 229 | e_p5 = norm( py5 - vF5*S5' ); 230 | fprintf('FullKr: The relative error in v^[5] is %g\n\n',e_p5/norm(py5)); 231 | end 232 | end 233 | 234 | if ( degree>4 ) 235 | tic; 236 | S6 = Kron2CT(n,6); 237 | CTtime = toc; 238 | if ( verbose ) 239 | fprintf('CT to Kron mappings (6) require %g seconds\n',CTtime) 240 | end 241 | 242 | k5 = k{5}; 243 | v6 = v{6}; 244 | 245 | ka5 = ka(:,idx4 +1:idx5 ); 246 | py6 = py( idx5-idx1+1:idx6-idx1); 247 | fprintf('NST: The norm of k^[5] is %g\n',norm(ka5)); 248 | fprintf('NST: The norm of v^[6] is %g\n',norm(py6)); 249 | 250 | e_k5 = norm( ka5 - k5*S5' ); 251 | fprintf('tensor: The relative error in k^[5] is %g\n',e_k5/norm(ka5)); 252 | 253 | e_p6 = norm( py6 - v6*S6' ); 254 | fprintf('tensor: The relative error in v^[6] is %g\n\n',e_p6/norm(py6)); 255 | end 256 | 257 | if ( degree>5 ) 258 | tic; 259 | S7 = Kron2CT(n,7); 260 | CTtime = toc; 261 | if ( verbose ) 262 | fprintf('CT to Kron mappings (7) require %g seconds\n',CTtime) 263 | end 264 | 265 | k6 = k{6}; 266 | v7 = v{7}; 267 | 268 | ka6 = ka(:,idx5 +1:idx6 ); 269 | py7 = py( idx6-idx1+1:idx7-idx1); 270 | fprintf('NST: The norm of k^[6] is %g\n',norm(ka6)); 271 | fprintf('NST: The norm of v^[7] is %g\n',norm(py7)); 272 | 273 | e_k6 = norm( ka6 - k6*S6' ); 274 | fprintf('tensor: The relative error in k^[6] is %g\n',e_k6/norm(ka6)); 275 | 276 | e_p7 = norm( py7 - v7*S7' ); 277 | fprintf('tensor: The relative error in v^[7] is %g\n\n',e_p7/norm(py7)); 278 | end 279 | 280 | if ( degree>6 ) 281 | 282 | k7 = k{7}; 283 | 284 | ka7 = ka(:,idx6 +1:idx7 ); 285 | fprintf('NST: The norm of k^[7] is %g\n',norm(ka7)); 286 | 287 | e_k7 = norm( ka7 - k7*S7' ); 288 | fprintf('tensor: The relative error in k^[7] is %g\n\n',e_k7/norm(ka7)); 289 | end 290 | 291 | 292 | % sometimes these errors are high, but the relative error is then low. 293 | % possibly due to factors like nearly singular R, nearly uncontrollable 294 | % (or extensions of this notion to the higher degree case?) 295 | % other times, we are computing a relative error for a quantity 296 | % that should be zero. 297 | -------------------------------------------------------------------------------- /examples/KuramotoSivashinsky1DControl/KuramotoSivashinskyFEMControl.m: -------------------------------------------------------------------------------- 1 | function [M,A,B,N,Q,zInit] = KuramotoSivashinskyFEMControl(n,m,epsilon) 2 | %KURAMOTO_SIVASHINSKY A control problem for the Kuramoto-Sivashinsky equations. 3 | % 4 | % A Kuramoto-Sivashinsky control problem is to find u that minimizes 5 | % 6 | % J(u) = \int_0^\infty \| z \|^2 + u'*R*u dt 7 | % 8 | % subject to the conservation form of the KS equations 9 | % 10 | % z_t = -\epsilon z_xx - \epsilon^2 z_xxxx - 2\epsilon z z_x + 11 | % \sum_{i=1}^m \chi_i(x) u_i(t) 12 | % 13 | % and 14 | % 15 | % z(0,t)=0=z(1,t), z_x(0,t)=0=z_x(1,t), z(x,0) = z_0(x) = sin(4*pi*x) 16 | % 17 | % These are discretized using piecewise Hermite cubic (H2) elements. 18 | % 19 | % Given a state dimension (n) and a control dimension (m, default=1), 20 | % we produce a discretized control problem of the form: 21 | % 22 | % M \dot{x} = A*x + N2*kron(x,x) + B*u 23 | % 24 | % J(u) = \int_0^\infty x'*Q*x + u'*R*u 25 | % 26 | % Note that a change of variables should be called by the calling routing 27 | % so that the mass matrix does not appear in the final discretized equation. 28 | % 29 | % Usage: 30 | % [M,A,B,N2,Q,z0] = KuramotoSivashinsky(n,m,epsilon) 31 | % 32 | % Default values: n=32, m=1, epsilon=0.005890757777002 33 | % (see Holmes, Lumley, and Berkooz) 34 | % 35 | %% 36 | 37 | if ( nargin<2 ) 38 | n = 32; 39 | m = 1; 40 | end 41 | 42 | if ( nargin<3 ) 43 | epsilon = 1/(13.0291)^2; % L = 13.0291 44 | end 45 | L = 1/sqrt(epsilon); 46 | 47 | [x,eConn] = oned_mesh([0; 1],[1 2],n); 48 | 49 | [n_nodes , ~ ] = size(x ); 50 | [n_elements, nel_dof ] = size(eConn); 51 | 52 | for n_nd=1:n_nodes-1 53 | ide(n_nd,1) = 2*n_nd-1; 54 | ide(n_nd,2) = 2*n_nd; 55 | end 56 | 57 | % set the periodic conditions 58 | ide(n_nodes,1) = 1; 59 | ide(n_nodes,2) = 2; 60 | 61 | n_equations = 2*(n_nodes-1); 62 | 63 | % Define an index function into N2 64 | tnm1 = 2*(n_nodes-1); 65 | idx2 = @(j,k) (j-1)*tnm1 + k; 66 | 67 | n_gauss = 5; % number of quadrature points. 68 | [r,wt] = oned_quadrature(n_gauss); 69 | n_equations = 2*(n_nodes-1); 70 | 71 | one = ones(n_gauss,1); 72 | eps_g = epsilon *one; 73 | eps2_g = epsilon^2*one; 74 | 75 | II = zeros(n_elements*2*nel_dof,1); 76 | JJ = zeros(n_elements*2*nel_dof,1); 77 | AA = zeros(n_elements*2*nel_dof,1); 78 | MM = zeros(n_elements*2*nel_dof,1); 79 | B = zeros(n_equations,m); 80 | z0 = zeros(n_equations,1); 81 | z0p = zeros(n_equations,1); 82 | 83 | IIn = zeros(4*n_elements*(2*nel_dof)^3,1); 84 | JJn = zeros(4*n_elements*(2*nel_dof)^3,1); 85 | NN = zeros(4*n_elements*(2*nel_dof)^3,1); 86 | 87 | n_triplets = 0; 88 | n_tripletsn = 0; 89 | 90 | b_loc = zeros(n_gauss,m); 91 | for n_el=1:n_elements 92 | % compute value of each test function and spatial derivatives 93 | % at the integration points (x_g - Gauss points, wt_g - Gauss weights) 94 | nodes_local = eConn(n_el,:); 95 | x_local = x(nodes_local,:); 96 | [x_g,wt_g, phi0,phi1, p0_x,p1_x, p0_xx,p1_xx] = ... 97 | oned_shapeherm(x_local,r,wt); 98 | 99 | M00_loc = oned_bilinear( one, phi0, phi0, wt_g ); 100 | M01_loc = oned_bilinear( one, phi0, phi1, wt_g ); 101 | M10_loc = M01_loc.'; 102 | M11_loc = oned_bilinear( one, phi1, phi1, wt_g ); 103 | 104 | A00_loc = oned_bilinear( eps_g, p0_x, p0_x, wt_g ) ... 105 | -oned_bilinear( eps2_g, p0_xx, p0_xx, wt_g ); 106 | A01_loc = oned_bilinear( eps_g, p0_x, p1_x, wt_g ) ... 107 | -oned_bilinear( eps2_g, p0_xx, p1_xx, wt_g ); 108 | A10_loc = A01_loc.'; 109 | A11_loc = oned_bilinear( eps_g, p1_x, p1_x, wt_g ) ... 110 | -oned_bilinear( eps2_g, p1_xx, p1_xx, wt_g ); 111 | 112 | for mm = 1:m 113 | b_loc(:,mm) = chi(x_g,mm,m); 114 | 115 | B0_loc = oned_f_int( b_loc, phi0, wt_g ); 116 | B1_loc = oned_f_int( b_loc, phi1, wt_g ); 117 | end 118 | 119 | [z_loc,zp_loc] = zZero(x_g,L); 120 | z0_loc = oned_f_int( z_loc, phi0, wt_g ); 121 | z1_loc = oned_f_int( z_loc, phi1, wt_g ); 122 | %--------------------------------------------------------------------------- 123 | % Assemble contributions into the system matrices 124 | %--------------------------------------------------------------------------- 125 | for n_t=1:nel_dof 126 | n_test0 = ide(nodes_local(n_t),1); 127 | n_test1 = ide(nodes_local(n_t),2); 128 | 129 | for n_u=1:nel_dof 130 | n_unk0 = ide(nodes_local(n_u),1); 131 | n_unk1 = ide(nodes_local(n_u),2); 132 | 133 | n_triplets = n_triplets + 1; 134 | II(n_triplets) = n_unk0; 135 | JJ(n_triplets) = n_test0; 136 | AA(n_triplets) = A00_loc(n_t,n_u); 137 | MM(n_triplets) = M00_loc(n_t,n_u); 138 | 139 | n_triplets = n_triplets + 1; 140 | II(n_triplets) = n_unk0; 141 | JJ(n_triplets) = n_test1; 142 | AA(n_triplets) = A01_loc(n_t,n_u); 143 | MM(n_triplets) = M01_loc(n_t,n_u); 144 | 145 | n_triplets = n_triplets + 1; 146 | II(n_triplets) = n_unk1; 147 | JJ(n_triplets) = n_test0; 148 | AA(n_triplets) = A10_loc(n_t,n_u); 149 | MM(n_triplets) = M10_loc(n_t,n_u); 150 | 151 | n_triplets = n_triplets + 1; 152 | II(n_triplets) = n_unk1; 153 | JJ(n_triplets) = n_test1; 154 | AA(n_triplets) = A11_loc(n_t,n_u); 155 | MM(n_triplets) = M11_loc(n_t,n_u); 156 | end 157 | 158 | for mm=1:m 159 | B(n_test0,mm) = B(n_test0,mm) + B0_loc(n_t,mm); 160 | B(n_test1,mm) = B(n_test1,mm) + B1_loc(n_t,mm); 161 | end 162 | 163 | z0(n_test0) = z0(n_test0) + z0_loc(n_t); 164 | z0(n_test1) = z0(n_test1) + z1_loc(n_t); 165 | 166 | % assemble the 2zz_x term with symmetries 167 | for nj=1:nel_dof 168 | j0 = ide(nodes_local(nj),1); 169 | j1 = ide(nodes_local(nj),2); 170 | for nk=nj:nel_dof 171 | k0 = ide(nodes_local(nk),1); 172 | k1 = ide(nodes_local(nk),2); 173 | 174 | tmp00 = sum(p0_x(:,n_t).*phi0(:,nj).*phi0(:,nk).*wt_g(:))/2; 175 | n_tripletsn = n_tripletsn+1; 176 | IIn(n_tripletsn) = n_test0; 177 | JJn(n_tripletsn) = idx2(j0,k0); 178 | NN(n_tripletsn) = tmp00; 179 | 180 | n_tripletsn = n_tripletsn+1; 181 | IIn(n_tripletsn) = n_test0; 182 | JJn(n_tripletsn) = idx2(k0,j0); 183 | NN(n_tripletsn) = tmp00; 184 | 185 | tmp01 = sum(p0_x(:,n_t).*phi0(:,nj).*phi1(:,nk).*wt_g(:))/2; 186 | n_tripletsn = n_tripletsn+1; 187 | IIn(n_tripletsn) = n_test0; 188 | JJn(n_tripletsn) = idx2(j0,k1); 189 | NN(n_tripletsn) = tmp01; 190 | 191 | n_tripletsn = n_tripletsn+1; 192 | IIn(n_tripletsn) = n_test0; 193 | JJn(n_tripletsn) = idx2(k1,j0); 194 | NN(n_tripletsn) = tmp01; 195 | 196 | tmp10 = sum(p0_x(:,n_t).*phi1(:,nj).*phi0(:,nk).*wt_g(:))/2; 197 | n_tripletsn = n_tripletsn+1; 198 | IIn(n_tripletsn) = n_test0; 199 | JJn(n_tripletsn) = idx2(j1,k0); 200 | NN(n_tripletsn) = tmp10; 201 | 202 | n_tripletsn = n_tripletsn+1; 203 | IIn(n_tripletsn) = n_test0; 204 | JJn(n_tripletsn) = idx2(k0,j1); 205 | NN(n_tripletsn) = tmp10; 206 | 207 | tmp11 = sum(p0_x(:,n_t).*phi1(:,nj).*phi1(:,nk).*wt_g(:))/2; 208 | n_tripletsn = n_tripletsn+1; 209 | IIn(n_tripletsn) = n_test0; 210 | JJn(n_tripletsn) = idx2(j1,k1); 211 | NN(n_tripletsn) = tmp11; 212 | 213 | n_tripletsn = n_tripletsn+1; 214 | IIn(n_tripletsn) = n_test0; 215 | JJn(n_tripletsn) = idx2(k1,j1); 216 | NN(n_tripletsn) = tmp11; 217 | 218 | % for p1_x 219 | tmp00 = sum(p1_x(:,n_t).*phi0(:,nj).*phi0(:,nk).*wt_g(:))/2; 220 | n_tripletsn = n_tripletsn+1; 221 | IIn(n_tripletsn) = n_test1; 222 | JJn(n_tripletsn) = idx2(j0,k0); 223 | NN(n_tripletsn) = tmp00; 224 | 225 | n_tripletsn = n_tripletsn+1; 226 | IIn(n_tripletsn) = n_test1; 227 | JJn(n_tripletsn) = idx2(k0,j0); 228 | NN(n_tripletsn) = tmp00; 229 | 230 | tmp01 = sum(p1_x(:,n_t).*phi0(:,nj).*phi1(:,nk).*wt_g(:))/2; 231 | n_tripletsn = n_tripletsn+1; 232 | IIn(n_tripletsn) = n_test1; 233 | JJn(n_tripletsn) = idx2(j0,k1); 234 | NN(n_tripletsn) = tmp01; 235 | 236 | n_tripletsn = n_tripletsn+1; 237 | IIn(n_tripletsn) = n_test1; 238 | JJn(n_tripletsn) = idx2(k1,j0); 239 | NN(n_tripletsn) = tmp01; 240 | 241 | tmp10 = sum(p1_x(:,n_t).*phi1(:,nj).*phi0(:,nk).*wt_g(:))/2; 242 | n_tripletsn = n_tripletsn+1; 243 | IIn(n_tripletsn) = n_test1; 244 | JJn(n_tripletsn) = idx2(j1,k0); 245 | NN(n_tripletsn) = tmp10; 246 | 247 | n_tripletsn = n_tripletsn+1; 248 | IIn(n_tripletsn) = n_test1; 249 | JJn(n_tripletsn) = idx2(k0,j1); 250 | NN(n_tripletsn) = tmp10; 251 | 252 | tmp11 = sum(p1_x(:,n_t).*phi1(:,nj).*phi1(:,nk).*wt_g(:))/2; 253 | n_tripletsn = n_tripletsn+1; 254 | IIn(n_tripletsn) = n_test1; 255 | JJn(n_tripletsn) = idx2(j1,k1); 256 | NN(n_tripletsn) = tmp11; 257 | 258 | n_tripletsn = n_tripletsn+1; 259 | IIn(n_tripletsn) = n_test1; 260 | JJn(n_tripletsn) = idx2(k1,j1); 261 | NN(n_tripletsn) = tmp11; 262 | 263 | end 264 | end 265 | end 266 | end 267 | 268 | II = II(1:n_triplets); 269 | JJ = JJ(1:n_triplets); 270 | A = sparse( II, JJ, AA(1:n_triplets), n_equations, n_equations ); 271 | M = sparse( II, JJ, MM(1:n_triplets), n_equations, n_equations ); 272 | 273 | N = sparse( IIn(1:n_tripletsn), JJn(1:n_tripletsn), NN(1:n_tripletsn), n_equations, n_equations^2 ); 274 | N = epsilon*N; 275 | 276 | Q = M; 277 | 278 | zInit = M\z0; 279 | end 280 | 281 | function [b_loc] = chi(x_local,mm,m) 282 | % The characteristic function over the interval ( (mm-1)/m, mm/m ). 283 | n = length(x_local); 284 | b_loc = zeros(n,1); 285 | 286 | for i=1:n 287 | if ( x_local(i)>(mm-1)/m && x_local(i)=6 ) 63 | classes = {'numeric'}; 64 | attributesA = {'size',[n,n]}; validateattributes(A,classes,attributesA ); 65 | attributesB = {'size',[n,m]}; validateattributes(B,classes,attributesB ); 66 | attributesQ = {'size',[n,n]}; validateattributes(Q,classes,attributesQ ); 67 | attributesR = {'size',[m,m]}; validateattributes(R,classes,attributesR ); 68 | attributesN3 = {'size',[n,n^3]};validateattributes(N,classes,attributesN3); 69 | else 70 | error('cqrOdd: expects at least 6 inputs'); 71 | end 72 | 73 | if ( nargin==6 ) 74 | degree = 3; 75 | end 76 | 77 | %============================================================================= 78 | % Define the linear solver 79 | %============================================================================= 80 | if ( ~exist('solver','var') ) 81 | if ( exist('./kronecker/tensor_recursive/lyapunov_recursive.m','file') ... 82 | && n>1 ) 83 | % lyapunov_recursive exists and is applicable 84 | solver = 'LyapunovRecursive'; 85 | elseif ( exist('./kronecker/tensor_recursive/laplace_recursive.m','file')... 86 | && n>1 ) 87 | % laplace_recursive is defined and is applicable 88 | solver = 'LaplaceRecursive'; 89 | else 90 | % either n=1 (which could also be treated separately) or testing N-Way 91 | % this is also the default solver. 92 | solver = 'BartelsStewart'; 93 | end 94 | end 95 | 96 | v = cell(1,degree+1); 97 | k = cell(1,degree); 98 | 99 | %============================================================================= 100 | % Compute the degree=1 feedback solution 101 | %============================================================================= 102 | [KK,PP] = lqr(full(A),full(B),full(Q),full(R)); 103 | 104 | K1 =-KK; 105 | v2 = PP(:); 106 | 107 | r2 = R(:); 108 | 109 | v{2} = v2.'; 110 | k{1} = K1; 111 | 112 | if ( degree>2 ) 113 | 114 | ABKT = (A+B*K1).'; 115 | Al{1} = ABKT; 116 | Al{2} = ABKT; 117 | Al{3} = ABKT; 118 | v{3} = sparse(1,n^3); 119 | k{2} = sparse(m,n^2); 120 | 121 | %=========================================================================== 122 | % Compute the degree=3 feedback solution 123 | %=========================================================================== 124 | % Efficiently solve the following (Kronecker) linear system 125 | % AA = ( kron( ABKT, eye(n^3) ) + ... 126 | % kron( eye(n ), kron( ABKT, eye(n^2) ) ) + ... 127 | % kron( eye(n^2), kron( ABKT, eye(n ) ) ) + ... 128 | % kron( eye(n^3), ABKT ) ); 129 | % bb =-( kron( N{3}, eye(n) ) + kron( eye(n),N{3} ) ).'*v2; 130 | % v4 = AA\bb; 131 | 132 | tic 133 | 134 | Al{4} = ABKT; 135 | bb = -LyapProduct(N.',v2,2); 136 | 137 | v4 = solveKroneckerSystem(Al,bb,n,4,solver); 138 | v4 = real(v4(:)); 139 | 140 | v4 = kronMonomialSymmetrize(v4,n,4); 141 | 142 | res = zeros(n*n*n,m); 143 | for i=1:m 144 | % Efficiently build the following products 145 | % GG = ( kron( B(:,i).', eye(n^3) ) + ... 146 | % kron( eye(n ), kron(B(:,i).', eye(n^2) ) ) + ... 147 | % kron( eye(n^2), kron(B(:,i).', eye(n ) ) ) + ... 148 | % kron( eye(n^3), B(:,i).' ) ); 149 | % GG = C*S*GG; 150 | % res(:,i) = -GG*v4; 151 | GGv4 = LyapProduct(B(:,i).',v4,4); 152 | res(:,i) = -GGv4; 153 | end 154 | 155 | v{4} = v4.'; 156 | k{3} = 0.5*(R\res.'); 157 | K3 = k{3}; 158 | 159 | comp3 = toc; 160 | end 161 | 162 | if ( degree>4 ) 163 | 164 | Al{5} = ABKT; 165 | 166 | v{5} = sparse(1,n^5); 167 | k{4} = sparse(m,n^4); 168 | %=========================================================================== 169 | % Compute the degree=5 feedback solution 170 | %=========================================================================== 171 | % Efficiently solve the following (Kronecker) linear system 172 | % AA = ( kron( ABKT, eye(n^5) ) + ... 173 | % kron( eye(n ), kron( ABKT, eye(n^4) ) ) + ... 174 | % kron( eye(n^2), kron( ABKT, eye(n^3) ) ) + ... 175 | % kron( eye(n^3), kron( ABKT, eye(n^2) ) ) + ... 176 | % kron( eye(n^4), kron( ABKT, eye(n ) ) ) + ... 177 | % kron( eye(n^5), ABKT ) ); 178 | % bb = 179 | % -( kron( (B*K3+N{3}).', eye(n^3) ) + ... 180 | % kron( kron( eye(n ), (B*K3+N{3}).' ), eye(n^2) ) + ... 181 | % kron( kron( eye(n^2), (B*K3+N{3}).' ), eye(n ) ) + ... 182 | % kron( eye(n^3), (B*K3+N{3}).' ) )*v4 ... 183 | % -( kron(K3.',K3.') )*r2 ; 184 | % v6 = AA\bb; 185 | 186 | tic 187 | 188 | Al{6} = ABKT; 189 | 190 | % form the Kronecker portion of the RHS 191 | % kron(K3.',K3.')*r2 192 | tmp = K3.'*R*K3; 193 | bb = -tmp(:); clear tmp 194 | 195 | % augment with the Kronecker sum products 196 | bb = bb - LyapProduct((B*K3+N).',v4,4); 197 | 198 | 199 | v6 = solveKroneckerSystem(Al,bb,n,6,solver); 200 | v6 = real(v6(:)); 201 | 202 | v6 = kronMonomialSymmetrize(v6,n,6); 203 | 204 | res = zeros(n*n*n*n*n,m); 205 | for i=1:m 206 | % Efficiently build the following products 207 | % GG = ( kron( B(:,i).',eye(n^4) ) + ... 208 | % kron( eye(n ),kron(B(:,i).',eye(n^3) ) ) + ... 209 | % kron( eye(n^2),kron(B(:,i).',eye(n^2) ) ) + ... 210 | % kron( eye(n^3),kron(B(:,i).',eye(n ) ) ) + ... 211 | % kron( eye(n^4), B(:,i).' ) ); 212 | % GG = C*S*GG; 213 | % res(:,i) = -GG*v5; 214 | GGv6 = LyapProduct(B(:,i).',v6,6); 215 | res(:,i) = -GGv6; 216 | 217 | end 218 | 219 | v{6} = v6.'; 220 | k{5} = 0.5*(R\res.'); 221 | K5 = k{5}; 222 | 223 | comp5 = toc; 224 | end 225 | 226 | if ( degree>6 ) 227 | 228 | Al{7} = ABKT; 229 | 230 | v{7} = sparse(1,n^7); 231 | k{6} = sparse(m,n^6); 232 | %=========================================================================== 233 | % Compute the degree=7 feedback solution 234 | %=========================================================================== 235 | % Efficiently solve the following (Kronecker) linear system 236 | % AA = ( kron( ABKT, eye(n^7) ) + ... 237 | % kron( eye(n ), kron( ABKT, eye(n^6) ) ) + ... 238 | % kron( eye(n^2), kron( ABKT, eye(n^5) ) ) + ... 239 | % kron( eye(n^3), kron( ABKT, eye(n^4) ) ) + ... 240 | % kron( eye(n^4), kron( ABKT, eye(n^3) ) ) + ... 241 | % kron( eye(n^5), kron( ABKT, eye(n^2) ) ) + ... 242 | % kron( eye(n^6), kron( ABKT, eye(n ) ) ) + ... 243 | % kron( eye(n^7), ABKT ) ); 244 | % bb = 245 | % -( kron(K3.',K5.') + ... 246 | % kron(K5.',K3.') )*r2 ; 247 | % v8 = AA\bb; 248 | 249 | tic 250 | 251 | Al{8} = ABKT; 252 | 253 | % form the Kronecker portion of the RHS 254 | % -( kron(K3.',K5.') + kron(K5.',K3.') )*r2 255 | tmp = K3.'*R*K5; 256 | bb =-tmp(:); 257 | tmp = tmp.'; 258 | bb = bb - tmp(:); clear tmp 259 | 260 | % augment with the Kronecker sum products 261 | bb = bb -LyapProduct((B*K3+N).',v6,6) ... 262 | -LyapProduct((B*K5 ).',v4,4); 263 | 264 | v8 = solveKroneckerSystem(Al,bb,n,8,solver); 265 | v8 = real(v8(:)); 266 | 267 | v8 = kronMonomialSymmetrize(v8,n,8); 268 | 269 | res = zeros(n*n*n*n*n*n*n,m); 270 | for i=1:m 271 | % Efficiently build the following products 272 | % GG = ( kron( B(:,i).',eye(n^4) ) + ... 273 | % kron( eye(n ),kron(B(:,i).',eye(n^3) ) ) + ... 274 | % kron( eye(n^2),kron(B(:,i).',eye(n^2) ) ) + ... 275 | % kron( eye(n^3),kron(B(:,i).',eye(n ) ) ) + ... 276 | % kron( eye(n^4), B(:,i).' ) ); 277 | % GG = C*S*GG; 278 | % res(:,i) = -GG*v5; 279 | res(:,i) = -LyapProduct(B(:,i).',v8,8); 280 | 281 | end 282 | 283 | v{8} = v8.'; 284 | k{7} = 0.5*(R\res.'); 285 | % K7 = k{7}; 286 | 287 | comp7 = toc; 288 | end 289 | 290 | if ( degree>7 ) 291 | warning('cqr: Only controls of degree <=7 have been implemented so far') 292 | end 293 | 294 | if ( verbose ) 295 | if ( degree>2 ) 296 | fprintf('cqr: CPU time for degree 3 controls: %g\n',comp3); 297 | end 298 | 299 | if ( degree>4 ) 300 | fprintf('cqr: CPU time for degree 5 controls: %g\n',comp5); 301 | end 302 | 303 | if ( degree>6 ) 304 | fprintf('cqr: CPU time for degree 7 controls: %g\n',comp7); 305 | end 306 | end 307 | end 308 | 309 | 310 | function [v] = solveKroneckerSystem(Al,bb,n,degree,solver) 311 | 312 | if ( strcmp(solver,'LyapunovRecursive') ) 313 | switch degree 314 | case 2 315 | v = lyapunov_recursive(Al,reshape(bb,n,n,n)); 316 | case 3 317 | v = lyapunov_recursive(Al,reshape(bb,n,n,n,n)); 318 | case 4 319 | v = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n)); 320 | case 5 321 | v = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n,n)); 322 | case 6 323 | v = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n,n,n)); 324 | case 7 325 | v = lyapunov_recursive(Al,reshape(bb,n,n,n,n,n,n,n,n)); 326 | otherwise 327 | warning('cqr: degree not supported') 328 | end 329 | 330 | elseif ( strcmp(solver,'LaplaceRecursive') ) 331 | switch degree 332 | case 2 333 | v = laplace_recursive(Al,reshape(bb,n,n,n)); 334 | case 3 335 | v = laplace_recursive(Al,reshape(bb,n,n,n,n)); 336 | case 4 337 | v = laplace_recursive(Al,reshape(bb,n,n,n,n,n)); 338 | case 5 339 | v = laplace_recursive(Al,reshape(bb,n,n,n,n,n,n)); 340 | case 6 341 | v = laplace_recursive(Al,reshape(bb,n,n,n,n,n,n,n)); 342 | case 7 343 | v = laplace_recursive(Al,reshape(bb,n,n,n,n,n,n,n,n)); 344 | otherwise 345 | warning('cqr: degree not supported') 346 | end 347 | 348 | else 349 | v = KroneckerSumSolver(Al,bb,degree); 350 | 351 | end 352 | 353 | end % function solveKroneckerSystem 354 | -------------------------------------------------------------------------------- /examples/example07.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------------------------------------------- 2 | %EXAMPLE07 Compares feedback strategies for a simple first-order problem taken 3 | % from Sakamoto and van der Schaft, 2007. 4 | % 5 | % \dot{x} = x - x^3 + u, J(u) = \int_0^\infty 0.5 x(t)^2 + 0.5 u(t)^2 dt. 6 | %------------------------------------------------------------------------------- 7 | %% 8 | n = 1; 9 | m = 1; 10 | degree = 9; 11 | fprintf('example07: the maximum degree is %d\n',degree); 12 | 13 | A = 1.0; 14 | N2 = 0.0; 15 | N3 =-1.0; 16 | B = 1.0; 17 | Q = 0.5; 18 | R = 0.5; 19 | 20 | T = 5; % this is T=\infty... 21 | x0 = 0.25; % the original value, not significant improvement in performance 22 | x0 = 1.25; % about a 17% improvement in closed-loop performance using 23 | %x0 = 1.00; 24 | 25 | % [k,v] = cqr(A,B,Q,R,{[],N2,N3},degree,true); 26 | [k,v] = pqr(A,B,Q,R,{[],N2,N3},degree,true); 27 | 28 | N = 401; 29 | x = linspace(0,1.8,N); 30 | 31 | u1 = k{1}*x; 32 | u2 = u1 + k{2}*x.*x; 33 | u3 = u2 + k{3}*x.^3; 34 | u4 = u3 + k{4}*x.^4; 35 | u5 = u4 + k{5}*x.^5; 36 | u6 = u5 + k{6}*x.^6; 37 | u7 = u6 + k{7}*x.^7; 38 | u8 = u7 + k{8}*x.^8; 39 | u9 = u8 + k{9}*x.^9; 40 | 41 | Ka = x.^3-x-sqrt((x-x.^3).^2+x.^2); 42 | plot(x,u1,x,u3,x,u5,x,u7,x,u9,x,Ka,'LineWidth',2) 43 | legend('u_1','u_3','u_5','u_7','u_9','analytic','Location','NorthWest') 44 | xlabel('z') 45 | ylabel('Feedback K(z)') 46 | 47 | v2 = v{ 2}*x.*x; 48 | v3 = v2 + v{ 3}*x.^3; 49 | v4 = v3 + v{ 4}*x.^4; 50 | v5 = v4 + v{ 5}*x.^5; 51 | v6 = v5 + v{ 6}*x.^6; 52 | v7 = v6 + v{ 7}*x.^7; 53 | v8 = v7 + v{ 8}*x.^8; 54 | v9 = v8 + v{ 9}*x.^9; 55 | v10 = v9 + v{10}*x.^10; 56 | 57 | rhs = @(t,x) t-t^3 + sqrt((t-t^3)^2+t^2); 58 | [T,V] = ode23(rhs,[0 1.8],0); 59 | 60 | figure 61 | plot(x,v2,x,v4,x,v6,x,v8,x,v10,T,V,'LineWidth',2) 62 | legend('degree 2','degree 4','degree 6','degree 8','degree 10','analytic','Location','NorthWest') 63 | xlabel('z_0') 64 | ylabel('Value Function Approximation') 65 | 66 | options = odeset('AbsTol',1e-7); 67 | 68 | runOpen = false; 69 | if ( runOpen ) 70 | %--------------------------------------------------------------------------- 71 | % Open loop simulation 72 | %--------------------------------------------------------------------------- 73 | tic 74 | rhs_open = @(t,x) [ A*x(1:end-1) + N2*kron(x(1:end-1),x(1:end-1)) + ... 75 | N3*kron(x(1:end-1),kron(x(1:end-1),x(1:end-1))) ; ... 76 | x(1:end-1).'*Q*x(1:end-1) ]; 77 | 78 | [t,x] = ode15s( rhs_open, [0 T], [x0;0], options ); 79 | figure; hold on 80 | plot(t,x(:,1)) 81 | xlabel('time'); ylabel('z') 82 | title('Open Loop Simulation') 83 | 84 | fprintf('Open Loop Cost (0,T) is %g\n\n',x(end,end)); 85 | toc 86 | end 87 | 88 | runClosed = false; T = 10; 89 | if ( runClosed ) 90 | %----------------------------------------------------------------------------- 91 | % Linear feedback 92 | %----------------------------------------------------------------------------- 93 | v2 = v{2}; 94 | APBK = A + B*k{1}; 95 | computeU1 = @(x) k{1}*x; 96 | rhs_k1 = @(t,x) [ APBK*x(1:end-1) + N2*kron(x(1:end-1),x(1:end-1)) + ... 97 | N3*kron(x(1:end-1),kron(x(1:end-1),x(1:end-1))) ; ... 98 | x(1:end-1).'*Q*x(1:end-1) + ... 99 | computeU1(x(1:end-1)).'*R*computeU1(x(1:end-1)) ]; 100 | 101 | [t1,x1] = ode23s( rhs_k1, [0 T], [x0;0], options ); 102 | figure(10); hold on 103 | plot(t1,x1(:,1)) 104 | xlabel('time'); ylabel('z') 105 | title('Closed-Loop Simulation with k^{[1]}') 106 | 107 | c2 = v2*kron(x0,x0); 108 | fprintf('Approx regulator cost to v^[2]: %g\n',c2) 109 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x1(end,end)); 110 | 111 | if ( degree>1 ) 112 | %--------------------------------------------------------------------------- 113 | % Quadratic feedback 114 | %--------------------------------------------------------------------------- 115 | v3 = v{3}; 116 | NPBK2 = N2 + B*k{2}; 117 | computeU2 = @(x) k{1}*x + k{2}*kron(x,x); 118 | rhs_k2 = @(t,x) [ APBK*x(1:end-1) + NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 119 | N3*kron(x(1:end-1),kron(x(1:end-1),x(1:end-1))) ; ... 120 | x(1:end-1).'*Q*x(1:end-1) + ... 121 | computeU2(x(1:end-1)).'*R*computeU2(x(1:end-1)) ]; 122 | 123 | [t2,x2] = ode23s( rhs_k2, [0 T], [x0;0], options ); 124 | 125 | figure(20); hold on 126 | plot(t2,x2(:,1:end-1)) 127 | xlabel('time'); ylabel('z') 128 | title('Closed-Loop Simulation with k^{[2]}') 129 | 130 | c3 = c2 + v3*kron(x0,kron(x0,x0)); 131 | fprintf('Approx regulator cost to v^[3]: %g\n',c3) 132 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x2(end,end)); 133 | 134 | end 135 | 136 | if ( degree>2 ) 137 | %--------------------------------------------------------------------------- 138 | % Cubic feedback 139 | %--------------------------------------------------------------------------- 140 | v4 = v{4}; 141 | NPBK3 = N3 + B*k{3}; 142 | computeU3 = @(x) k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 143 | 144 | rhs_k3 = @(t,x) [ APBK*x(1:end-1) + ... 145 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 146 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)); ... 147 | x(1:end-1).'*Q*x(1:end-1) + ... 148 | computeU3(x(1:end-1)).'*R*computeU3(x(1:end-1)) ]; 149 | 150 | [t3,x3] = ode23s( rhs_k3, [0 T], [x0;0], options ); 151 | 152 | figure(30); hold on 153 | plot(t3,x3(:,1:end-1)) 154 | xlabel('time'); ylabel('z') 155 | title('Closed-Loop Simulation with k^{[3]}') 156 | 157 | c4 = c3 + v4*kron(kron(kron(x0,x0),x0),x0); 158 | fprintf('Approx regulator cost to v^[4]: %g\n',c4) 159 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x3(end,end)); 160 | end 161 | 162 | 163 | if ( degree>3 ) 164 | %--------------------------------------------------------------------------- 165 | % Quartic feedback 166 | %--------------------------------------------------------------------------- 167 | v5 = v{5}; 168 | computeU4 = @(x) k{1}*x + ... 169 | k{2}*kron(x,x) + ... 170 | k{3}*kron(kron(x,x),x) + ... 171 | k{4}*kron(kron(kron(x,x),x),x); 172 | 173 | computeU4_4 = @(x) k{4}*kron(kron(kron(x,x),x),x); 174 | 175 | rhs_k4 = @(t,x) [ APBK*x(1:end-1) + ... 176 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 177 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 178 | B*computeU4_4(x(1:end-1)) ; ... 179 | x(1:end-1).'*Q*x(1:end-1) + ... 180 | computeU4(x(1:end-1)).'*R*computeU4(x(1:end-1)) ]; 181 | 182 | [t4,x4] = ode23s( rhs_k4, [0 T], [x0;0], options ); 183 | 184 | figure(40); hold on 185 | plot(t4,x4(:,1:end-1)) 186 | xlabel('time'); ylabel('z') 187 | title('Closed-Loop Simulation with k^{[4]}') 188 | 189 | c5 = c4 + v5*kron(kron(kron(kron(x0,x0),x0),x0),x0); 190 | fprintf('Approx regulator cost to v^[5]: %g\n',c5) 191 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x4(end,end)); 192 | 193 | end 194 | 195 | if ( degree>4 ) 196 | %----------------------------------------------------------------------------- 197 | % Quintic feedback 198 | %----------------------------------------------------------------------------- 199 | v6 = v{6}; 200 | computeU5 = @(x) k{1}*x + ... 201 | k{2}*kron(x,x) + ... 202 | k{3}*kron(kron(x,x),x) + ... 203 | k{4}*kron(kron(kron(x,x),x),x) + ... 204 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 205 | computeU4_5 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 206 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 207 | rhs_k5 = @(t,x) [ APBK*x(1:end-1) + ... 208 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 209 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 210 | B*computeU4_5(x(1:end-1)); ... 211 | x(1:end-1).'*Q*x(1:end-1) + ... 212 | computeU5(x(1:end-1)).'*R*computeU5(x(1:end-1)) ]; 213 | 214 | [t5,x5] = ode23s( rhs_k5, [0 T], [x0;0], options ); 215 | 216 | figure(50); hold on 217 | plot(t5,x5(:,1:end-1)) 218 | xlabel('time'); ylabel('z') 219 | title('Closed-Loop Simulation with k^{[5]}') 220 | 221 | c6 = c5 + v6*kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0); 222 | fprintf('Approx regulator cost to v^[6]: %g\n',c6) 223 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x5(end,end)); 224 | 225 | end 226 | 227 | if ( degree>6 ) 228 | %----------------------------------------------------------------------------- 229 | % Septic feedback 230 | %----------------------------------------------------------------------------- 231 | v7 = v{7}; 232 | v8 = v{8}; 233 | computeU7 = @(x) k{1}*x + ... 234 | k{2}*kron(x,x) + ... 235 | k{3}*kron(kron(x,x),x) + ... 236 | k{4}*kron(kron(kron(x,x),x),x) + ... 237 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 238 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x) + ... 239 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 240 | computeU4_7 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 241 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 242 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x) + ... 243 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 244 | rhs_k7 = @(t,x) [ APBK*x(1:end-1) + ... 245 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 246 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 247 | B*computeU4_7(x(1:end-1)); ... 248 | x(1:end-1).'*Q*x(1:end-1) + ... 249 | computeU7(x(1:end-1)).'*R*computeU7(x(1:end-1)) ]; 250 | 251 | [t7,x7] = ode23s( rhs_k7, [0 T], [x0;0], options ); 252 | 253 | figure(70); hold on 254 | plot(t7,x7(:,1:end-1)) 255 | xlabel('time'); ylabel('z') 256 | title('Closed-Loop Simulation with k^{[7]}') 257 | 258 | c8 = c6 + v7*kron(kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0),x0) + ... 259 | v8*kron(kron(kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0),x0),x0); 260 | fprintf('Approx regulator cost to v^[8]: %g\n',c8) 261 | fprintf('Actual closed-loop cost (0,T) is: %g\n\n',x7(end,end)); 262 | 263 | end 264 | 265 | end % runClosed 266 | % 267 | % figure 268 | % x0 = linspace(-.1,.1,201); 269 | % c2 = v{2}*x0.^2; 270 | % c3 = c2 + v{3}*x0.^3; 271 | % c4 = c3 + v{4}*x0.^4; 272 | % c5 = c4 + v{5}*x0.^5; 273 | % c6 = c5 + v{6}*x0.^6; 274 | % plot(x0,c2,x0,c3,x0,c4,x0,c5,x0,c6) 275 | % legend('v2','v3','v4','v5','v6') 276 | % 277 | % % TO DO: add_derivatives_along_solutions 278 | % 279 | % % 280 | % % [t3,x3] = ode23s( rhs_k3, [0 T], x0 ); 281 | % % figure(13); hold on 282 | % % plot3(x3(1,1),x3(1,2),x3(1,3),'*') 283 | % % plot3(x3(:,1),x3(:,2),x3(:,3),'k') 284 | % % plot3(0,0,0,'o') 285 | % % view([1 1 1]) 286 | % % 287 | % % c4 = c3 + v4*kron(x0,kron(x0,kron(x0,x0))); 288 | % % fprintf('approx. regulator cost to k3: %g\n',c4) 289 | % % 290 | % % figure(31) 291 | % % plot(t1,x1(:,1),'r',t2,x2(:,1),'b',t3,x3(:,1),'k') 292 | % % legend('degree 1','degree 2','degree 3') 293 | % % figure(32) 294 | % % plot(t1,x1(:,2),'r',t2,x2(:,2),'b',t3,x3(:,2),'k') 295 | % % legend('degree 1','degree 2','degree 3') 296 | % % figure(33) 297 | % % plot(t1,x1(:,3),'r',t2,x2(:,3),'b',t3,x3(:,3),'k') 298 | % % legend('degree 1','degree 2','degree 3') 299 | % % 300 | % % figure(41) 301 | % % u1 = k{1}*x1.'; 302 | % % 303 | % % u2 = k{1}*x2.'; 304 | % % for i=1:length(u2) 305 | % % u2(i) = u2(i) + k{2}*kron(x2(i,:).',x2(i,:).'); 306 | % % end 307 | % % 308 | % % u3 = k{1}*x3.'; 309 | % % for i=1:length(u3) 310 | % % u3(i) = u3(i) + k{2}*kron(x3(i,:).',x3(i,:).') ... 311 | % % + k{3}*kron(x3(i,:).',kron(x3(i,:).',x3(i,:).')); 312 | % % end 313 | % % 314 | % % plot(t1,u1,'r',t2,u2,'b',t3,u3,'k') 315 | % % legend('degree 1','degree 2','degree 3') 316 | % % 317 | % % %end 318 | % % 319 | -------------------------------------------------------------------------------- /examples/example08.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------------------------------------------- 2 | %EXAMPLE08 Compares feedback strategies for a coupled system of van der Pol 3 | % oscillators. 4 | % 5 | % \ddot{x}_i = a_i (1-x_i^2) \dot{x}_i - x_i + u_i, 6 | % J(u) = \int_0^\infty x(t)^2 + u(t)^2 dt. 7 | %------------------------------------------------------------------------------- 8 | %% 9 | if ( ~exist('g','var') ) 10 | fprintf('example08: using default values\n') 11 | g = 4; % number of van der Pol oscillators 12 | Cidx = [1 2]; 13 | degree = 5; 14 | end 15 | 16 | n = 2*g; 17 | m = length(Cidx); 18 | fprintf('example08: the maximum degree is %d\n',degree); 19 | 20 | a = ones(g,1); % viscous damping parameter 21 | b = ones(g,1); % coupling parameter 22 | 23 | A = zeros(n,n); 24 | 25 | for i=1:g 26 | i1 = 2*i-1; 27 | i2 = 2*i; 28 | A(i1,i2) = 1; 29 | A(i2,i2) = a(i); 30 | A(i2,i1) = -(1+2*b(1)); 31 | 32 | if ( i==1 ) 33 | im1 = 2*g-1; 34 | im2 = 2*g; 35 | else 36 | im1 = 2*(i-1)-1; 37 | im2 = 2*(i-1); 38 | end 39 | 40 | if ( i==g ) 41 | ip1 = 1; 42 | ip2 = 2; 43 | else 44 | ip1 = 2*(i+1)-1; 45 | ip2 = 2*(i+1); 46 | end 47 | 48 | A(i2,ip1) = A(i2,ip1) + b(i); 49 | A(i2,im1) = A(i2,im1) + b(i); 50 | end 51 | 52 | N{2} = zeros(n,n^2); 53 | N{3} = zeros(n,n^3); 54 | 55 | % set the N3 terms here... 56 | idx3 = @(i1,i2,i3) i1 + (i2-1)*n + (i3-1)*n^2; 57 | 58 | for i=1:g 59 | i1 = 2*i-1; 60 | i2 = 2*i; 61 | N{3}(i2,idx3(i1,i1,i2)) = N{3}(i2,idx3(i1,i1,i2)) - a(i)/3; 62 | N{3}(i2,idx3(i1,i2,i1)) = N{3}(i2,idx3(i1,i2,i1)) - a(i)/3; 63 | N{3}(i2,idx3(i2,i1,i1)) = N{3}(i2,idx3(i2,i1,i1)) - a(i)/3; 64 | end 65 | 66 | B = zeros(n,m); 67 | 68 | for i=1:m 69 | cIndex = Cidx(i); 70 | B(2*cIndex,i) = 1; 71 | end 72 | 73 | Q = eye(n); 74 | R = eye(m); 75 | 76 | T = 50; % this is T=\infty... 77 | x0 = [0.3*ones(g,1), zeros(g,1)].'; x0 = x0(:); 78 | 79 | [k,v] = cqr(A,B,Q,R,N,degree,'LyapunovRecursive'); 80 | % [k,v] = cqr(A,B,Q,R,N,degree,'BartelsStewart'); 81 | 82 | 83 | 84 | 85 | options = odeset('AbsTol',1e-7); 86 | 87 | runOpen = true; 88 | if ( runOpen ) 89 | %--------------------------------------------------------------------------- 90 | % Open loop simulation 91 | %--------------------------------------------------------------------------- 92 | tic 93 | rhs_open = @(t,x) [ A*x(1:end-1) + N{3}*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1));... 94 | x(1:end-1).'*Q*x(1:end-1) ]; 95 | 96 | [t,x] = ode15s( rhs_open, [0 T], [x0;0], options ); 97 | figure(1); hold on 98 | plot(t,x(:,1:2:end-1)) 99 | xlabel('time'); ylabel('x') 100 | title('Open Loop Simulation') 101 | 102 | fprintf('Open Loop Cost (0,T) is %15.10f\n\n',x(end,end)); 103 | toc 104 | end 105 | 106 | runClosed = true; 107 | if ( runClosed ) 108 | %----------------------------------------------------------------------------- 109 | % Linear feedback 110 | %----------------------------------------------------------------------------- 111 | APBK = A + B*k{1}; 112 | computeU1 = @(x) k{1}*x; 113 | rhs_k1 = @(t,x) [ APBK*x(1:end-1) + N{3}*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1));... 114 | x(1:end-1).'*Q*x(1:end-1) + ... 115 | computeU1(x(1:end-1)).'*R*computeU1(x(1:end-1)) ]; 116 | 117 | [t1,x1] = ode23s( rhs_k1, [0 T], [x0;0], options ); 118 | figure(10); hold on 119 | plot(t1,x1(:,1:2:end-1)) 120 | xlabel('time'); ylabel('x') 121 | title('Closed-Loop Simulation with k^{[1]}') 122 | 123 | 124 | c2 = v{2}*kron(x0,x0); 125 | fprintf('Approx regulator cost to v^[2]: %15.10f\n',c2) 126 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x1(end,end)); 127 | 128 | if ( degree>1 ) 129 | %--------------------------------------------------------------------------- 130 | % Quadratic feedback 131 | %--------------------------------------------------------------------------- 132 | NPBK2 = N{2} + B*k{2}; 133 | computeU2 = @(x) k{1}*x + k{2}*kron(x,x); 134 | rhs_k2 = @(t,x) [ APBK*x(1:end-1) + NPBK2*kron(x(1:end-1),x(1:end-1))+ ... 135 | N{3}*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)); ... 136 | x(1:end-1).'*Q*x(1:end-1) + ... 137 | computeU2(x(1:end-1)).'*R*computeU2(x(1:end-1)) ]; 138 | 139 | [t2,x2] = ode23s( rhs_k2, [0 T], [x0;0], options ); 140 | 141 | figure(20); hold on 142 | plot(t2,x2(:,1:2:end-1)) 143 | xlabel('time'); ylabel('x') 144 | title('Closed-Loop Simulation with k^{[2]}') 145 | 146 | c3 = c2 + v{3}*kron(x0,kron(x0,x0)); 147 | fprintf('Approx regulator cost to v^[3]: %15.10f\n',c3) 148 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x2(end,end)); 149 | 150 | end 151 | 152 | if ( degree>2 ) 153 | %--------------------------------------------------------------------------- 154 | % Cubic feedback 155 | %--------------------------------------------------------------------------- 156 | NPBK3 = N{3} + B*k{3}; 157 | computeU3 = @(x) k{1}*x + k{2}*kron(x,x) + k{3}*kron(kron(x,x),x); 158 | 159 | rhs_k3 = @(t,x) [ APBK*x(1:end-1) + ... 160 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 161 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)); ... 162 | x(1:end-1).'*Q*x(1:end-1) + ... 163 | computeU3(x(1:end-1)).'*R*computeU3(x(1:end-1)) ]; 164 | 165 | [t3,x3] = ode23s( rhs_k3, [0 T], [x0;0], options ); 166 | 167 | figure(30); hold on 168 | plot(t3,x3(:,1:2:end-1)) 169 | xlabel('time'); ylabel('x') 170 | title('Closed-Loop Simulation with k^{[3]}') 171 | 172 | c4 = c3 + v{4}*kron(kron(kron(x0,x0),x0),x0); 173 | fprintf('Approx regulator cost to v^[4]: %15.10f\n',c4) 174 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x3(end,end)); 175 | end 176 | 177 | 178 | if ( degree>3 ) 179 | %--------------------------------------------------------------------------- 180 | % Quartic feedback 181 | %--------------------------------------------------------------------------- 182 | computeU4 = @(x) k{1}*x + ... 183 | k{2}*kron(x,x) + ... 184 | k{3}*kron(kron(x,x),x) + ... 185 | k{4}*kron(kron(kron(x,x),x),x); 186 | 187 | computeU4_4 = @(x) k{4}*kron(kron(kron(x,x),x),x); 188 | 189 | rhs_k4 = @(t,x) [ APBK*x(1:end-1) + ... 190 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 191 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 192 | B*computeU4_4(x(1:end-1)) ; ... 193 | x(1:end-1).'*Q*x(1:end-1) + ... 194 | computeU4(x(1:end-1)).'*R*computeU4(x(1:end-1)) ]; 195 | 196 | [t4,x4] = ode23s( rhs_k4, [0 T], [x0;0], options ); 197 | 198 | figure(40); hold on 199 | plot(t4,x4(:,1:2:end-1)) 200 | xlabel('time'); ylabel('x') 201 | title('Closed-Loop Simulation with k^{[4]}') 202 | 203 | c5 = c4 + v{5}*kron(kron(kron(kron(x0,x0),x0),x0),x0); 204 | fprintf('Approx regulator cost to v^[5]: %15.10f\n',c5) 205 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x4(end,end)); 206 | 207 | end 208 | 209 | if ( degree>4 ) 210 | %----------------------------------------------------------------------------- 211 | % Quintic feedback 212 | %----------------------------------------------------------------------------- 213 | computeU5 = @(x) k{1}*x + ... 214 | k{2}*kron(x,x) + ... 215 | k{3}*kron(kron(x,x),x) + ... 216 | k{4}*kron(kron(kron(x,x),x),x) + ... 217 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 218 | computeU4_5 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 219 | k{5}*kron(kron(kron(kron(x,x),x),x),x); 220 | rhs_k5 = @(t,x) [ APBK*x(1:end-1) + ... 221 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 222 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 223 | B*computeU4_5(x(1:end-1)); ... 224 | x(1:end-1).'*Q*x(1:end-1) + ... 225 | computeU5(x(1:end-1)).'*R*computeU5(x(1:end-1)) ]; 226 | 227 | [t5,x5] = ode23s( rhs_k5, [0 T], [x0;0], options ); 228 | 229 | figure(50); hold on 230 | plot(t5,x5(:,1:2:end-1)) 231 | xlabel('time'); ylabel('x') 232 | title('Closed-Loop Simulation with k^{[5]}') 233 | 234 | c6 = c5 + v{6}*kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0); 235 | fprintf('Approx regulator cost to v^[6]: %15.10f\n',c6) 236 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x5(end,end)); 237 | 238 | end 239 | 240 | if ( degree>5 ) 241 | %----------------------------------------------------------------------------- 242 | % Hexic feedback 243 | %----------------------------------------------------------------------------- 244 | computeU6 = @(x) k{1}*x + ... 245 | k{2}*kron(x,x) + ... 246 | k{3}*kron(kron(x,x),x) + ... 247 | k{4}*kron(kron(kron(x,x),x),x) + ... 248 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 249 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x); 250 | computeU4_6 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 251 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 252 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x); 253 | rhs_k6 = @(t,x) [ APBK*x(1:end-1) + ... 254 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 255 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 256 | B*computeU4_6(x(1:end-1)); ... 257 | x(1:end-1).'*Q*x(1:end-1) + ... 258 | computeU6(x(1:end-1)).'*R*computeU6(x(1:end-1)) ]; 259 | 260 | [t6,x6] = ode23s( rhs_k6, [0 T], [x0;0], options ); 261 | 262 | figure(60); hold on 263 | plot(t6,x6(:,1:2:end-1)) 264 | xlabel('time'); ylabel('x') 265 | title('Closed-Loop Simulation with k^{[6]}') 266 | 267 | c7 = c6 + v{7}*kron(kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0),x0); 268 | fprintf('Approx regulator cost to v^[7]: %15.10f\n',c7) 269 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x6(end,end)); 270 | 271 | end 272 | 273 | if ( degree>6 ) 274 | %----------------------------------------------------------------------------- 275 | % Septic feedback 276 | %----------------------------------------------------------------------------- 277 | computeU7 = @(x) k{1}*x + ... 278 | k{2}*kron(x,x) + ... 279 | k{3}*kron(kron(x,x),x) + ... 280 | k{4}*kron(kron(kron(x,x),x),x) + ... 281 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 282 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x)+ ... 283 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 284 | computeU4_7 = @(x) k{4}*kron(kron(kron(x,x),x),x) + ... 285 | k{5}*kron(kron(kron(kron(x,x),x),x),x) + ... 286 | k{6}*kron(kron(kron(kron(kron(x,x),x),x),x),x)+ ... 287 | k{7}*kron(kron(kron(kron(kron(kron(x,x),x),x),x),x),x); 288 | rhs_k7 = @(t,x) [ APBK*x(1:end-1) + ... 289 | NPBK2*kron(x(1:end-1),x(1:end-1)) + ... 290 | NPBK3*kron(kron(x(1:end-1),x(1:end-1)),x(1:end-1)) + ... 291 | B*computeU4_7(x(1:end-1)); ... 292 | x(1:end-1).'*Q*x(1:end-1) + ... 293 | computeU7(x(1:end-1)).'*R*computeU7(x(1:end-1)) ]; 294 | 295 | [t7,x7] = ode23s( rhs_k7, [0 T], [x0;0], options ); 296 | 297 | figure(70); hold on 298 | plot(t7,x7(:,1:2:end-1)) 299 | xlabel('time'); ylabel('x') 300 | title('Closed-Loop Simulation with k^{[7]}') 301 | 302 | c8 = c7 + v{8}*kron(kron(kron(kron(kron(kron(kron(x0,x0),x0),x0),x0),x0),x0),x0); 303 | fprintf('Approx regulator cost to v^[8]: %15.10f\n',c8) 304 | fprintf('Actual closed-loop cost (0,T) is: %15.10f\n\n',x7(end,end)); 305 | 306 | end 307 | 308 | end % runClosed 309 | --------------------------------------------------------------------------------