1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 | % Example explicit MPC for a 2D system From "Optimal control of constrained % piecewise affine discrete-time systems" (Mayne and Rakovic, 2002). % Choose system. A = [1,0.1; 0,1]; B = [0; 0.0787]; Q = [1, 0; 0, 0]; R = 0.1; ulb = -1; uub = 1; N = 10; % Find LQR terminal set. [K, P] = dlqr(A, B, Q, R); K = -K; A_u = [-K; K]; b_u = [-ulb; uub]; [A_lqr, b_lqr] = calcOinf(A + B*K, A_u, b_u); % Get mpQP, solve, and plot. mpqp = ss2mpqp('A', A, 'B', B, 'Q', Q, 'R', R, 'P', P, 'N', N, ... 'ulb', ulb, 'uub', uub, 'Af', A_lqr, 'bf', b_lqr); regions = solvempqp(mpqp, 'plot', true()); |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 | function regions = solvempqp(prob, varargin) % regions = solvempqp(prob, 'key', value, ...) % % Solves the mpQP given by % % min 0.5*z'*Q*z + p'*C'*z % z % s.t. A*z <= b + S*z % % for all p satisfying % % E*p <= e % % with prob a struct containing fields Q, C, A, b, and S. Note that Q must be % strictly positive definite. E and e can also be provided (they default to % empty matrices). % % Available options are as follows: % % - 'display' : Whether to display status of iterations (default true) % - 'printevery' : Number of iterations for each print (default 100) % - 'logfile' : Filename to write detailed log (default '', which means no log) % - 'vertices' : Whether to find vertices for each region (default true for % Np = 2, false for Np > 2) % - 'plot' : Whether to make a plot of all the regions (default false; ignored % if 'vertices' is not true); only possible if length(p) == 2 % - 'maxiter' : Maximum integer number of iterations (default inf) % - 'maxtime' : Maximum time to run (in seconds; default inf). % - 'ascell' : Whether to return cell array (default true) % % Returned value is cell array of structs containing the halfspace and % vertex representations of the polytopes. If 'ascell' is false, instead returns % a struct whose fields are hexidecimal ID numbers and values contain the other % information. % % Essential mpQP algorithm is from "A multiparametric quadratic programming % algorithm with polyhedral computations based on nonnegative least squares" % (Bemporad, 2015), although the polyhedral computations are performed using % qhull and linprog rather than nonnegative least squares. global SUPPRESS_OUTPUT if nargin() < 1 error('Argument prob is required!'); end args = struct(varargin{:}); defaults = {'display', true(); 'logfile', ''; 'vertices', NaN(); ... 'plot', false(); 'maxiter', inf(); 'ascell', true(); ... 'maxtime', inf(); 'printevery', 100}; options = struct(); for i = 1:size(defaults, 1) f = defaults{i, 1}; options.(f) = structGet(args, f, defaults{i, 2}); end if ~isempty(options.logfile) logfile = fopen(options.logfile, 'w'); log_(logfile); % Enables logging. end log_('Log started %s\n', datestr(now())); % Disable output if a global SUPPRESS_OUTPUT is set (overrides user preference). if ~isempty(SUPPRESS_OUTPUT) && SUPPRESS_OUTPUT options.display = false(); end % Convert to nomenclature from Bemporad (2015). pmpqp = struct(); % Primal mpQP. pmpqp.G = prob.A; pmpqp.W = prob.b; pmpqp.S = prob.S; pmpqp.Q = prob.Q; pmpqp.Q = 0.5*(pmpqp.Q + pmpqp.Q'); % Matlab is picky about symmetry. pmpqp.F = prob.C; pmpqp.c = zeros(size(pmpqp.F, 1), 1); pmpqp.Qhalf = chol(pmpqp.Q); pmpqp.E = structGet(prob, 'E', zeros(0, size(prob.S, 2))); if isempty(pmpqp.E) pmpqp.e = zeros(0, 1); else if ~isfield(prob, 'e') error('prob.e must be provided if prob.E is given!'); end pmpqp.e = prob.e; end if isnan(options.vertices) options.vertices = (size(prob.S, 2) == 2); end % Convert to dual problem (with fewer parameters). dmpqp = struct(); % Dual mpQP. dmpqp.H = pmpqp.G*(pmpqp.Q\pmpqp.G'); dmpqp.D = pmpqp.G*(pmpqp.Q\pmpqp.F) + pmpqp.S; dmpqp.d = pmpqp.G*(pmpqp.Q\pmpqp.c) + pmpqp.W; % Choose qhull options. qhullopts = {'Qt', 'QbB', 'Pp'}; if size(prob.S, 2) >= 5 qhullopts = [qhullopts, {'Qx'}]; end % To start, we know no constraints are active at the origin. Ncon = size(dmpqp.D, 1); opensets = struct('N1', struct('active', false(Ncon, 1))); % All cons inactive. opensets.N1.id = getcrid(opensets.N1.active); nopen = 1; nfound = 0; regions = struct(); probedregions = struct(opensets.N1.id, []); niter = 0; starttime = cputime(); options.maxtime = options.maxtime + starttime; while nopen > 0 && niter < options.maxiter && cputime() < options.maxtime niter = niter + 1; if options.display && mod(niter, options.printevery) == 0 fprintf('Iteration %d (%d found, %d left to search).\n', ... niter, nfound, nopen); end log_('Iteration %d\n', niter); activestr = sprintf('N%d', nopen); nopen = nopen - 1; currentset = opensets.(activestr); opensets.(activestr) = []; crid = currentset.id; log_(' Getting %s\n', crid); [cr, okay] = getcriticalregion(pmpqp, dmpqp, currentset); if okay log_(' Found representation\n'); [nonredundant, cr.A, cr.b] = removeredundantcon(cr.A, cr.b, cr.x, ... [], qhullopts); cr.cind = cr.cind(nonredundant); cr.ctype = cr.ctype(nonredundant); if options.vertices cr.V = halfspace2vertex(cr.A, cr.b, cr.x); end regions.(crid) = cr; nfound = nfound + 1; % Try to find adjacent critical regions. log_(' Searching for adjacent regions\n'); for i = 1:length(nonredundant) n = cr.cind(i); newactive = currentset.active; tryqpsearch = true(); switch cr.ctype(i) case 'w' newactive(n) = true(); case 'y' newactive(n) = false(); tryqpsearch = false(); % QP doesn't help in this case. otherwise continue end keepgoing = true(); while keepgoing % Check whether the new active combination has full rank. % If so, add it. Otherwise, try once to fix it. newcrid = getcrid(newactive); if ~isfield(probedregions, newcrid) probedregions.(newcrid) = []; [newfullrank, newHiihalf] = checkcrfullrank(dmpqp, newactive); if newfullrank newcr = struct('active', newactive, 'id', newcrid, ... 'Hiihalf', newHiihalf); [opensets, nopen] = addcr(opensets, nopen, newcr); keepgoing = false(); else log_(' %s is not full-rank\n', newcrid); if tryqpsearch tryqpsearch = false(); log_(' Trying QP neighbor search\n'); cr.facet = i; [newactive, keepgoing] = findadjacentcr(cr, pmpqp); if ~keepgoing log_(' No neighbor found\n'); end else keepgoing = false(); end end else keepgoing = false(); end end end else log_(' %s is infeasible\n', crid); end log_(' %d regions open\n', nopen); end log_('Finished with %d open regions\n', nopen); log_(); % Closes the log. if options.display fprintf(['Found %d regions (%d left to search) after %d iterations (in ' ... '%g s).\n'], nfound, nopen, niter, cputime() - starttime); end % Make a plot. if options.plot && size(pmpqp.S, 2) == 2 fprintf('Making plot (may take some time).\n'); figure(); hold('on'); fields = fieldnames(regions); colors = viridiscolors(length(fields)); [~, cperm] = sort(sin(1:length(fields))); colors = colors(cperm,:); % (Pseudo-)randomize colors. for i = 1:length(fields) f = fields{i}; fill(regions.(f).V(:,1), regions.(f).V(:,2), colors(i,:)); end xlabel('p_1'); ylabel('p_2', 'rotation', 0); end % Reshape into cell array. if options.ascell regionsstruct = regions; fields = fieldnames(regionsstruct); regions = cell(length(fields), 1); for i = 1:length(fields) f = fields{i}; regions{i} = regionsstruct.(f); regions{i}.id = f; end end end%function function [fullrank, Hiihalf] = checkcrfullrank(dmpqp, activecon) % [fullrank, Hiihalf] = checkcrfullrank(dmpqp, activecon) % % Checks whether the critical region defined by the set of active % constraints is full-rank. If so, fullrank is true(), and the Cholesky % decomposition of the reduced Hessian is also returned. If not, fullrank is % false, and the second output is meaningless. narginchk(2, 2); i = activecon; Hii = dmpqp.H(i,i); if isempty(Hii) Hiihalf = Hii; fullrank = true(); else [Hiihalf, flag] = chol(Hii); fullrank = (flag == 0) && all(diag(Hiihalf) > 1e-6); end end%function function [cr, okay] = getcriticalregion(pmpqp, dmpqp, crset) % [cr, okay] = getcriticalregion(pmpqp, dmpqp, crset) % % Returns corresponding critical region based on given set of active % constraints. cr is a struct with fields A, b, x (with x giving an interior % point of the critical region), cind (giving indices of w or y for each % row of the constraint matrix), and ctype (giving 'w' or 'y'). % % If activecon does not defin an optimal set of constraints, then cr is % empty, and okay is false. narginchk(3, 3); % Check for cholesky decomposition in crset. activecon = crset.active; if isfield(crset, 'Hiihalf') Hiihalf = crset.Hiihalf; okay = true(); else [okay, Hiihalf] = checkcrfullrank(dmpqp, activecon); end % Get inequalities that define critical region (often redundant). cr = struct(); if okay i = activecon; j = ~activecon; Y = -(Hiihalf\(Hiihalf'\[dmpqp.D(i,:), dmpqp.d(i)])); W = dmpqp.H(j,i)*Y + [dmpqp.D(j,:), dmpqp.d(j)]; P = [W; Y]; % Feasible region is P*[x; 1] >= 0. A = -P(:,1:end-1); b = P(:,end); % Screen out near-zero rows of A. In this case, we just set them to % actual zero so the LP solver can handle it. Anorms = max(abs(A), [], 2); badrows = (Anorms < 1e-10); if any(b(badrows) < 0) okay = false(); end A(badrows,:) = 0; b(badrows) = 1; % Add the inequalities that define the region of interest for x. A = [A; pmpqp.E]; b = [b; pmpqp.e]; [A, b] = scalerows(A, b); Ne = length(pmpqp.e); % Attempt to find an interior point. if okay [x, okay] = findinteriorpoint(A, b); if okay % Critical region. cr.A = A; cr.b = b; cr.cind = [find(j); find(i); -1*(1:Ne)']; cr.ctype = [repmat('w', size(W, 1), 1); ... repmat('y', size(Y, 1), 1); ... repmat('e', Ne, 1)]; cr.x = x; % Optimal solution. law = -pmpqp.Qhalf\(pmpqp.Qhalf'\(pmpqp.G(i,:)'*Y ... + [pmpqp.F, pmpqp.c])); cr.Z = law(:,1:end-1); cr.z0 = law(:,end); end end end end%function function [z, feas, slack, active] = solveprimalqp(x, pmpqp) % Solves the primal qp at the given value of x using quadprog (Matlab) % or qp (Octave). narginchk(2, 2); Q = pmpqp.Q; f = pmpqp.F*x; A = pmpqp.G; b = pmpqp.S*x + pmpqp.W; Aeq = []; beq = []; if isOctave() zguess = zeros(size(Q, 1), 1); [z, ~, flag] = qp(zguess, Q, f, Aeq, beq, [], [], [], A, b); feas = (flag.info == 0); else prob = struct('H', Q, 'f', f, 'Aineq', A, 'bineq', b, ... 'Aeq', Aeq, 'beq', beq, 'options', ... optimoptions('quadprog', 'display', 'off'), ... 'solver', 'quadprog'); [z, ~, flag] = quadprog(prob); feas = (flag == 1); if isempty(z) z = NaN(length(f), 1); end end slack = pmpqp.G*z - pmpqp.S*x - pmpqp.W; active = (abs(slack) < 1e-5); end%function function [opensets, nopen] = addcr(opensets, nopen, newcr) % Adds the new set to the list of open sets. nopen = nopen + 1; openstr = sprintf('N%d', nopen); opensets.(openstr) = newcr; log_(' Adding %s\n', newcr.id); end%function function [newactive, feas] = findadjacentcr(cr, pmpqp) % Finds an adjacent CR by stepping in the direction of the shared facet and % solving the QP. narginchk(2, 2); % Get point on the facet. ineq = true(size(cr.A, 1), 1); ineq(cr.facet) = false(); [x0, okay] = findinteriorpoint(cr.A(ineq,:), cr.b(ineq), ... cr.A(cr.facet,:), cr.b(cr.facet)); if okay normal = cr.A(cr.facet,:)'; x0p = x0 + 1e-4*normal/norm(normal); [~, feas, ~, newactive] = solveprimalqp(x0p, pmpqp); else feas = false(); end if ~okay || ~feas newactive = []; end end%function function [A, b, scale] = scalerows(A, b, zerotol) % [A, b, scale] = scalerows(A, b, [zerotol]) % % Scales the rows of A and b so that each row of [A, b] has unit norm. % If zerotol is given, any entries of A with absolute value less than % zerotol (after scaling) are set to zero. The default for zerotol is 1e-12. % % Any trivial rows (with A(i,:) = 0 and b(i) = 0) are set to b = 1. narginchk(2, 3); if nargin() < 3 zerotol = 1e-12; end % Perform scaling. Z = [A, b]; scale = sqrt(sum(Z.^2, 2)); trivrows = (scale == 0); scale(trivrows) = 1; % Don't do anything to these guys. A = bsxfun(@rdivide, A, scale); b = b./scale; b(trivrows) = 1; % Screen out any elements that are very small. A(abs(A) < 1e-12) = 0; end%function function hex = bin2hex(bits) % Converts logical vector bits to hexidecimal string. hex = '0123456789ABCDEF'; bits = bits(:); Npad = 4 - mod(length(bits), 4); if Npad == 4 Npad = 0; end bits = [zeros(Npad, 1); double(bits)]; Nhex = length(bits)/4; digits = [8, 4, 2, 1]*reshape(bits, 4, Nhex); hex = hex(digits + 1); end%function function crid = getcrid(active) % Returns the string ID of the given set of active constraints. crid = ['CR_', bin2hex(active)]; end%function function log_(file_or_str, varargin) % Writes to log file using sprintf syntax. Pass integer file ID to start % logging. Call with no arguments to close. persistent logfile if nargin() == 1 && ~ischar(file_or_str) && isscalar(file_or_str) % Open log file. logfile = file_or_str; if logfile == -1 error('Invalid log file!'); end elseif nargin() == 0 if ~isempty(logfile) fclose(logfile); logfile = []; end elseif ~isempty(logfile) fprintf(logfile, file_or_str, varargin{:}); if isOctave() fflush(logfile); end end end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 | function [mpqp, T] = ss2mpqp(varargin) % mpqp = ss2mpqp(sys) % % Converts the linear state-space system described by sys. % % sys must contain the following fields: % % - A, B: matrices that describe model x^+ = A*x + B*u % - Q, R: matrices for stage cost x'*Q*x + u'*Q*u % - S: cross-term matrix for 2*x'*S'u (defaults to 0). % - P: matrix for terminal penalty x'*P*x (defaults to Q). % - N: integer choosing the horizon % % sys can optionally contain the following fields: % % - xlb, xub, ulb, uub: vectors of bounds for x or u % - C, D, ylb, yub: output matrices and bounds % - Af, bf: matrices that define Xf as polyhedron Af*xf <= bf % - keepstates: True or False whether to eliminate the state matrices. If True, % variables are interleaved as [u(0); x(1); u(1); x(2); ...]. % - dualstates: True or False whether to dualize the state equality constraints. % If True, variables are interleaved as [u(0); lam(1); x(1); ...] % Note that this implicitly sets keepstates=True. % % Values can also be passed as (key, value) pairs instead of as a struct. % % Note that all of these bounds must be chosen so that the feasible set is % full-dimensional. In particular, lb < ub, and (Af, bf) cannot contain % implicit equality constraints. % % Returned value is a struct with fields to describe the mpQP % % min 0.5*z'*Q*x + p'*C*z % z % s.t. A*z <= b + S*p % % with p = x(0) and z = [u(0); u(1); ...]. if nargin() == 1 sys = varargin{1}; else sys = struct(varargin{:}); end if ~isstruct(sys) error('Input sys must be a struct!'); elseif any(~isfield(sys, {'A', 'B', 'Q', 'R', 'N'})) error('Input sys is missing required fields!'); end % Get main matrices and sizes. A = sys.A; B = sys.B; Q = sys.Q; R = sys.R; [Nx, Nu] = size(B); Nt = sys.N; % Get optional values. S = structGet(sys, 'S', zeros(Nx, Nu)); P = structGet(sys, 'P', Q); xlb = structGet(sys, 'xlb', -inf(Nx, 1)); xub = structGet(sys, 'xub', inf(Nx, 1)); ulb = structGet(sys, 'ulb', -inf(Nu, 1)); uub = structGet(sys, 'uub', inf(Nu, 1)); C = structGet(sys, 'C', zeros(0, Nx)); Ny = size(C, 1); D = structGet(sys, 'D', zeros(Ny, Nu)); ylb = structGet(sys, 'ylb', -inf(Ny, 1)); yub = structGet(sys, 'yub', inf(Ny, 1)); Af = structGet(sys, 'Af', zeros(0, Nx)); if isempty(Af) bf = zeros(0, 1); else if ~isfield(sys, 'bf') error('Must provide bf if Af is given!'); else bf = sys.bf; end end dualstates = structGet(sys, 'dualstates', false()); keepstates = dualstates || structGet(sys, 'keepstates', false()); % Compute state transition matrices that give % % w = T*z % % with z = [x(0); u(0); u(1); ...; u(N - 1)], and % w = [x(0); u(0); x(1); u(1); ...; x(N - 1); u(N - 1); x(N)]. Nxu = Nx + Nu; Nw = Nt*Nxu + Nx; % Rows corresponding to x(k). ix = bsxfun(@plus, (1:Nx)', Nxu:Nxu:(Nxu*Nt)); % Columns for x(0) and u(k). jx0 = (1:Nx)'; ju = bsxfun(@plus, (1:Nu)', Nx:Nxu:(Nxu*Nt)); % Get state transition matrix. T = eye(Nw); if keepstates Nz = Nw; keep = true(Nw, 1); else Cont = zeros(Nx, Nw); Cont(:,jx0) = eye(Nx); for k = 1:Nt % Update controllability matrix. Cont = A*Cont; Cont(:,ju(:,k)) = B; % Add entries for x(k). T(ix(:,k),:) = Cont; end Nz = Nt*Nu + Nx; keep = [jx0(:); ju(:)]; end T = T(:,keep); % Get model constraints. if keepstates Aeq = kron(eye(Nt + 1), [A, B]) + kron(diag(ones(Nt, 1), 1), ... [-eye(Nx), zeros(size(B))]); Aeq = Aeq(1:(end - Nx),1:(end - Nu)); beq = zeros(Nt*Nx, 1); else Aeq = zeros(0, Nw); beq = zeros(0, 1); end % Get output constraints. A_y = [kron(eye(Nt), [C, D]), zeros(Ny*Nt, Nx)]; A_y = [A_y; -A_y]; b_y = [repmat(yub, Nt, 1); repmat(-ylb, Nt, 1)]; % Get polyhedron for bound constraints. lb = [-inf(Nx, 1); repmat([ulb; xlb], Nt, 1)]; ub = [inf(Nx, 1); repmat([uub; xub], Nt, 1)]; [A_lim, b_lim] = hyperrectangle(lb, ub); % Get polyhedron for terminal constraints. A_Xf = [zeros(size(Af, 1), Nxu*Nt), Af]; b_Xf = bf; % Formulate objective function. H = blkdiag(kron(eye(Nt), [Q, S; S', R]), P); % Reduce all of the matrices using the state transition matrix. Alt = [A_lim; A_y; A_Xf]*T; blt = [b_lim; b_y; b_Xf]; Aeq = Aeq*T; H = T'*H*T; % Dualize if requested. if dualstates [H, Alt] = dualize(H, Alt, Aeq, [Nx, Nx + Nu]); Nz = Nz + Nx*Nt; Aeq = zeros(0, Nz); end % Remove trivial rows. keep = (blt ~= inf()); Alt = Alt(keep,:); blt = blt(keep); % Formulate QP matrices. mpqp = struct(); x0 = [true(Nx, 1); false(Nz - Nx, 1)]; mpqp.A = Alt(:,~x0); mpqp.b = blt; mpqp.S = -Alt(:,x0); mpqp.Aeq = Aeq(:,~x0); mpqp.beq = beq; mpqp.Seq = -Aeq(:,x0); mpqp.Q = H(~x0,~x0); mpqp.C = H(~x0,x0); end%function function [H_, Alt_] = dualize(H, Alt, Aeq, blocksize) % Dualizes the equality constraints for the given QP. % % blocksize should give the size of each variable block in Aeq. Dual % multipliers are then interleaved between the blocks. Nvar = size(H, 1); Nblocks = size(Aeq, 1)/blocksize(1); if round(Nblocks) ~= Nblocks error('blocksize(1) must be a multiple of size(Aeq, 1)!'); end % Get logical masks for variables and dual multipliers. var = [repmat([true(blocksize(2), 1); ... false(blocksize(1), 1)], Nblocks, 1); ... true(Nvar - blocksize(2)*Nblocks, 1)]; mul = ~var; Ntot = length(var); H_ = zeros(Ntot, Ntot); H_(var,var) = H; H_(mul,var) = Aeq; H_ = 0.5*(H_' + H_); % Make symmetric. Alt_ = zeros(size(Alt, 1), Ntot); Alt_(:,var) = Alt; end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 | function [AOinf, bOinf, tstar] = calcOinf(F, A, b, tmax) % [AOinf, bOinf, tstar] = calcOinf(F, A, b, [tmax]) % % Calculates the maximum admissible set for x^+ = F*x subject to A*x <= b. Note % that if F is unstable, this procedure will not work. % % tmax is the value of t to stop at, as an upper bound is not known a-priori. % The default value is 100. If this bound is reached without termination, then % tstar is set to inf. % Arguments and sizes. narginchk(3, 4); if nargin() < 4 tmax = 100; end Nx = size(F, 1); if size(F, 2) ~= Nx error('F must be square!'); end Nc = size(A, 1); if size(A, 2) ~= Nx error('A must have the same number of columns as F!'); elseif ~isvector(b) || length(b) ~= Nc error('b must be a vector with an entry for each row of A!'); end b = b(:); % Define linear programming function. if isOctave() solvelp = @solvelp_octave; else solvelp = @solvelp_matlab; end % Start the algorithm. Ft = eye(Nx); AOinf = zeros(0, Nx); bOinf = zeros(0, 1); tstar = inf(); for t = 0:tmax % Add constraints for new time point. AOinf = [AOinf; A*Ft]; bOinf = [bOinf; b]; % Recalculate objective. Ft = F*Ft; fobj = A*Ft; % Maximize each component, stopping early if any bounds are violated. okay = true(); for i = 1:Nc [obj, feas] = solvelp(fobj(i,:), AOinf, bOinf); if ~feas || obj > b(i) okay = false(); % N isn't high enough Need to keep going. continue end end % If everything was feasible, then we're done. if okay tstar = t; break end end end%function function [obj, feas] = solvelp_octave(f, A, b) % Octave version to solve LP. Nx = size(A, 2); lb = -inf(Nx, 1); ub = inf(Nx, 1); ctype = repmat('U', size(A, 1), 1); vtype = repmat('C', Nx, 1); sense = -1; % Maximization. [~, obj, err, extra] = glpk(f, A, b, lb, ub, ctype, vtype, sense, ... struct('msglev', 0)); status = extra.status; feas = (err == 0) && (status == 5); end%function function [obj, feas] = solvelp_matlab(f, A, b) % Matlab version to solve LP. prob = struct('f', -f, 'Aineq', A, 'bineq', b, 'options', ... optimoptions('linprog', 'Algorithm', 'dual-simplex', ... 'display', 'off'), 'solver', 'linprog'); [~, obj, exitflag] = linprog(prob); obj = -obj; % Fix sign of objective value. feas = (exitflag == 1); end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 | function [A, b] = hyperrectangle(lb, ub) % [A, b] = hyperrectangle(lb, ub) % % Returns halfspace representation of hyperrectangle with bounds lb and ub. % Any infinite or NaN bounds are ignored. narginchk(2, 2); if ~isvector(lb) || ~isvector(ub) || length(lb) ~= length(ub) error('Inputs must be vectors of the same size!'); end A = kron(eye(length(lb)), [1; -1]); b = reshape([ub(:)'; -lb(:)'], 2*length(lb), 1); goodrows = ~isinf(b) & ~isnan(b); A = A(goodrows,:); b = b(goodrows); end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 | function [nr, Anr, bnr, h, x0] = removeredundantcon(A, b, x0, tol, qhullopts) % [nr, Anr, bnr, h, x0] = removeredundantcon(A, b, [x0], [tol], qhullopts) % % Finds the non-redundant constraints for the polyhedron Ax <= b. nr is a % column vector with the non-redundant rows. If requested, Anr and bnr are the % non-redundant parts of A and b. % % If x0 is supplied, it must be in the strict interior of A. Otherwise an error % is thrown. Specifying a valid x0 will speed up the function. % % tol is used to decide how much on the interior we need to be. If not supplied, % the default value is 1e-8*max(1, norm(b)/N). Note that this may be too large % if b is poorly scaled. % % qhullopts is a string of options to pass to qhull. Defaults are described in % the documentation for convhulln (see `help convhulln`). % % h is the output of convhulln, which actually describes the facets, and x0 % is a point on the interior of the polyhedron. Note that if the input % polyhedron is unbounded, h may have zeros in some entries corresponding to the % row of all zeros that needs to be added for the method to work. % % Note that this requires finding convex hull in N + 1 dimensions, where N % is the number of columns of A. Thus, this will get very slow if A has a lot % of columns. narginchk(2, 5); % Force b to column vector and check sizes. b = b(:); if isrow(b) b = b'; end if size(A, 1) ~= length(b) error('A and b must have the same number of rows!'); end if nargin() < 3 x0 = []; end if nargin() < 4 || isempty(tol) tol = 1e-8*max(1, norm(b)/length(b)); elseif tol <= 0 error('tol must be strictly positive!') end if nargin() < 5 if size(A, 2) <= 4 qhullopts = {'Qt'}; else qhullopts = {'Qt', 'Qx'}; end end % Save copies before things get messed up. Anr = A; bnr = b; % First, get rid of any rows of A that are all zero. Anorms = max(abs(A), [], 2); badrows = (Anorms == 0); if any(b(badrows) < 0) error('A has infeasible trivial rows.') end A(badrows, :) = []; b(badrows) = []; goodrows = [0; find(~badrows)]; % Add zero for all zero row that gets added. % Need to find a point in the interior of the polyhedron. if isempty(x0) if all(b > 0) % If b is strictly positive, we know the origin works. x0 = zeros(size(A, 2), 1); else error('Must supply an interior point!'); end else x0 = x0(:); if isrow(x0) x0 = x0'; end if length(x0) ~= size(A,2) error('x0 must have as many entries as A has columns.') end if any(A*x0 >= b - tol) error('x0 is not in the strict interior of Ax <= b!') end end % Now, project the rows of P and find the convex hull. btilde = b - A*x0; if any(btilde <= 0) warning('Shifted b is not strictly positive. convhull will likely fail.') end Atilde = [zeros(1, size(A, 2)); bsxfun(@rdivide, A, btilde)]; h = convhulln(Atilde, qhullopts); u = unique(h(:)); nr = goodrows(u); if nr(1) == 0 nr(1) = []; end h = goodrows(h); % Finally, grab the appropriate rows for Anr and bnr. Anr = Anr(nr, :); bnr = bnr(nr); end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 | function [x0, okay, feas, margin] = findinteriorpoint(A, b, Aeq, beq, tol, maxinside) % [x0, okay, feas, margin] = findinteriorpoint(A, b, [Aeq], [beq], [tol]) % % Find a strictly feasible point x0 so that A*x0 <= b - tol. If no such point % can be found, okay is set to False. % % If there is at least a feasible point (but not necessarily on the interior), % then feas is true, and x0 gives that point. If both okay and feas are false, % then x0 is meaningless. % % margin gives the value e such that A*x0 <= b - e. % % The origin is always checked first. If it does not work, an LP is solved % to find a valid point. % % tol is used to decide how much on the interior we need to be. If not % supplied, the default value is 1e-8*max(1, norm(b)/N). Note that this may % be too large if b is poorly scaled. if nargin() < 2 error('A and b are mandatory.') elseif nargin() < 5 tol = 1e-8*max(1, norm(b)/length(b)); end if nargin() < 6 maxinside = 1; else maxinside = max(tol, maxinside); end [m, n] = size(A); if nargin() < 4 || isempty(Aeq) Aeq = zeros(0, n); beq = []; end meq = size(Aeq, 1); okay = false(); % Check whether the origin is on the inside. if all(abs(beq) < tol) && all(b > tol) x0 = zeros(n, 1); okay = true(); feas = true(); margin = min(b); end % Try to use fminsearch if there are no equality constraints. Doesn't work % well if the number of dimensions is very high, so we cap it at 10. if ~okay && meq == 0 && m <= 10 options = optimset('display', 'off'); [x0, maxr] = fminsearch(@(x) max(max(A*x - b), -1e5*tol), A\b, options); okay = (maxr < -tol); feas = okay; margin = -maxr; end % Solve LP otherwise. if ~okay c = [zeros(n, 1); -1]; AA = [A, ones(m, 1)]; AAeq = [Aeq, zeros(meq,1)]; lb = [-inf(n, 1); 0]; ub = [inf(n, 1); maxinside]; if isOctave() ctype = [repmat('U', m, 1); repmat('S', meq, 1)]; [xtilde, ~, err, extra] = glpk(c, [AA; AAeq], [b; beq], lb, ub, ... ctype, repmat('C', n + 1, 1), 1, struct('msglev', 0)); okay = (err == 0 && extra.status == 5); else options = optimoptions('linprog', 'display', 'off', 'algorithm', 'dual-simplex'); [xtilde, ~, exitflag] = linprog(c, AA, b, AAeq, beq, lb, ub, [], options); okay = (exitflag == 1); end if isempty(xtilde) margin = -inf(); else margin = xtilde(end); % Amount constraints could be tightened. end okay = okay && (margin >= tol); if isempty(xtilde) x0 = zeros(n, 1); okay = false(); else x0 = xtilde(1:end-1); end % Check feasibility of x0. feas = all(A*x0 - b < tol); if feas && ~isempty(Aeq) feas = all(abs(Aeq*x0 - beq) < tol); end okay = okay && feas; end end%function |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 | function [V, nr] = halfspace2vertex(A, b, x0) % [V, nr] = halfspace2vertex(A, b, [x0]) % % Finds extreme points of polyhedron A*x <= b. Note that the polyhedron must % have a point strictly on its interior. % % If provided, x0 must be a point on the interior of the polyhedron. If it is % not given, one is found by solving a linear program. % % V is returned as an N by 2 matrix with each row giving an extreme point. % % Second output nr is a list of the non-redundant constraints of the polytope. % Check inputs. narginchk(2, 3); Nc = size(A, 1); Nx = size(A, 2); if ~isvector(b) || length(b) ~= Nc error('b is the incorrect size!'); end b = b(:); % Make sure b is a column vector. % Sort out interior point. if nargin() < 3 if all(b > 0) % The origin is on the interior. Can rescale rows so that b = 1. x0 = zeros(Nx, 1); A = bsxfun(@rdivide, A, b); b = ones(size(b)); else x0 = findinteriorpoint(A, b); end elseif ~isvector(x0) || length(x0) ~= Nx error('Invalid size for x0!'); end x0 = x0(:); % Make sure x0 is a column vector. % Get non-redundant constraints from A and b. [nr, ~, ~, k] = removeredundantcon(A, b, x0); % Now loop through facets to find vertices. V = zeros(size(k, 1), Nx); keep = true(size(k, 1), 1); for ix = 1:size(k, 1) F = A(k(ix,:),:); g = b(k(ix,:)); [keep(ix), V(ix,:)] = fullranksolve(F, g); end V = V(keep,:); [~, u] = unique(round(V*1e6), 'rows'); V = V(u,:); % If in 2D, sort the vertices. if Nx == 2 V = polarsort(V); end end%function function [fullrank, x] = fullranksolve(A, b); % Checks whether the system is full rank and if so, solves it. If it is not % full rank, a vector of NaNs are returned. Nx = size(A, 1); [U, S, V] = svd(A); s = diag(S); tol = Nx*eps(s(1)); % Rank tolerance. fullrank = all(s >= tol); if fullrank x = V*diag(1./s)*U'*b; else x = NaN(Nx, 1); end end%function function [p, s] = polarsort(p) % [p, s] = polarsort(p) % % Sorts the [n by 2] matrix p so that the points are counter-clockwise starting % at the theta = 0 axis. For ties in theta, sorts on radius. x = p(:,1); y = p(:,2); x = (x - mean(x))/std(x); % Normalize so that the origin is at the center. y = (y - mean(y))/std(y); [th, r] = cart2pol(x, y); [~, s] = sortrows([th, r]); % Sort on theta then r. p = p(s,:); end%function |