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 | % Exercise "Terminal constraint and region of attraction" part (c) % Define model, cost function, and bounds. A = [2, 1; 0, 2]; B = [1, 0; 0, 1]; N = 3; alpha = 1e-5; Q = alpha*eye(2); R = eye(2); K = zeros(size(B')); % Don't actually use this, but it's needed for findXn(). % Bounds. xlb = [-inf(); -inf()]; xub = [5; inf()]; ulb = [-1; -1]; uub = [1; 1]; % Find region of stability. [Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, 'termeq'); % Compute lines. p = V{end}; % Vertices of Xn. xlims = [-3, 3]; ylims = [-3, 3]; Xnlines = vertices2lines(p, xlims, ylims); % Save data files. p = p'; % Vertices of Xn. 'Xnterminal_c.dat' l = Xnlines; % Lines for all sides. 'Xnterminal_lines_c.dat' save('Xnterminal_c.dat', 'p', 'l'); |
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 | function [Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, terminal, doplot) % [Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, terminal, [doplot]) % % Calculates the various sets for the given system and parameters. % % Most inputs are self-explanatory. terminal should be either the string % 'termeq' to use a terminal equality constraint, or 'lqr' to use the % maximum LQR-invariant set. % % Output Xn is a cell array defining each of the \bbX_n sets. Xn{1} is the % terminal set, Xn{2} is \bbX_1, etc. V is a cell array with each entry giving % the extreme vertices of the corresponding Xn (as a 2 by N matrix). Z is a % structure defining the feasible space. % % Also makes a plot of all the sets unless doplot is False. narginchk(9, 10); if nargin() < 10 doplot = true(); end % Define constraints for Z. Nx = size(A, 1); [Az, bz] = hyperrectangle([xlb; ulb], [xub; uub]); Z = struct('G', Az(:,1:Nx), 'H', Az(:,(Nx + 1):end), 'psi', bz); % Decide terminal constraint. switch terminal case 'termeq' % Equality constraint at the origin. Xf = [0; 0]; case 'lqr' % Build feasible region considering x \in X and Kx \in U. [A_U, b_U] = hyperrectangle(ulb, uub); A_lqr = A_U*K; b_lqr = b_U; [A_X, b_X] = hyperrectangle(xlb, xub); Acon = [A_lqr; A_X]; bcon = [b_lqr; b_X]; % Use LQR-invariant set. Xf = struct(); ApBK = A + B*K; % LQR evolution matrix. [Xf.A, Xf.b] = calcOinf(ApBK, Acon, bcon); [~, Xf.A, Xf.b] = removeredundantcon(Xf.A, Xf.b); otherwise error('Unknown value for terminal: %s', terminal); end % Now do feasible sets computation. if doplot figure(); hold('on'); colors = jet(N + 1); end Xn = cell(N + 1, 1); Xn{1} = Xf; names = cell(N + 1, 1); names{1} = 'Xf'; V = cell(N + 1, 1); for n = 1:(N + 1) % Plot current set. if n > 1 names{n} = sprintf('X%d', n - 1); end if doplot plotargs = {'-o', 'color', colors(n,:)}; else plotargs = {false()}; end V{n} = plotpoly(Xn{n}, plotargs{:}); if n == (N + 1) break end % Compute next set. Also need to prune constraints. nextXn = computeX1(Z, A, B, Xn{n}); [~, nextXn.A, nextXn.b] = removeredundantcon(nextXn.A, nextXn.b); Xn{n + 1} = nextXn; end if doplot legend(names{:}); end 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 | 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 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 | function X1 = computeX1(Z, A, B, Xf) % X1 = computeX1(Z, [A], [B], [Xf]) % % Computes the feasible set X_1 for the system x^+ = Ax + Bu subject to % constraints Gx + Hu <= psi and x^+ \in Xf. % % Z must be a struct with fields G, H, and psi. % % A and B are only necessary if the terminal constraint is given. % % Xf can be either a struct with fields A and b to define a polytope, or a % single vector to define a point constraint. If not provided, it is assumed % that Xf is the entire space. % % X1 is returned as a struct with fields A and b defining a set of inequality % constraints. % Check arguments. narginchk(1, 4); if ~isstruct(Z) || ~all(isfield(Z, {'G', 'H', 'psi'})) error('Invalid input for Z!'); end Nx = size(Z.G, 2); Nu = size(Z.H, 2); if nargin() >= 3 sys = struct('A', A, 'B', B); % Save these. end % Preallocate constraint matrices. A = [Z.G, Z.H]; b = Z.psi; Aeq = zeros(0, Nx + Nu); beq = zeros(0, 1); % Do some more stuff if there is a terminal constraint. if nargin() >= 4 && ~isempty(Xf) if ~isequal([size(sys.A), size(sys.B)], [Nx, Nx, Nx, Nu]) error('Incorrect size for A or B!'); end if isstruct(Xf) % Polyhedron. if ~all(isfield(Xf, {'A', 'b'})) error('Struct Xf must have fields A and b!'); end A = [A; Xf.A*[sys.A, sys.B]]; b = [b; Xf.b]; elseif isvector(Xf) && length(Xf) == Nx % Terminal equality constraint. Aeq = [sys.A, sys.B]; beq = Xf; else error('Invalid input for Xf!'); end end % Now do the projection step. for i = 1:Nu [A, b, Aeq, beq] = fouriermotzkin(A, b, Aeq, beq); end X1 = struct('A', A, 'b', b); 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 | function p = plotpoly(p, varargin) % p = plotpoly(p, ...) % p = plotpoly({A, b}, ...) % p = plotpoly(struct('A', A, 'b', b), ...) % % Plots the polyhedron with vertices given in the 2 by N matrix p or given by % the extreme points of A*x <= b. Any additional arguments are passed to plot. % % Returns the extreme points matrix p. If you only want this matrix, pass % false as the only additional argument, and the plot will not be made. if nargin() < 1 error('Argument p is required!'); end % First, find the vertices if {A, b} given. if iscell(p) p = halfspace2vertex(p{1}, p{2})'; elseif isstruct(p) p = halfspace2vertex(p.A, p.b)'; end % Next, sort the vertices. ptilde = bsxfun(@rdivide, bsxfun(@plus, p, -mean(p, 2)), std(p, 0, 2)); x = ptilde(1,:); y = ptilde(2,:); [th, r] = cart2pol(x, y); thneg = (th < 0); th(thneg) = th(thneg) + 2*pi(); % Makes theta in [0, 2*pi] instaed of [-pi, pi]. [~, s] = sortrows([th', r']); % Sort on theta then r. p = p(:,s); % Duplicate first data point to give closed cycle. p = p(:,[1:end,1]); % Now plot. if length(varargin) == 0 || ~isequal(varargin{1}, false()) plot(p(1,:), p(2,:), varargin{:}); 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 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 | function [A, b, Aeq, beq] = fouriermotzkin(A, b, Aeq, beq, ielim) % [A, b, Aeq, beq] = fouriermotzkin(A, b, [Aeq], [beq], [ielim]) % % Perform one step of Fourier-Motzkin elimination for the set defined by % % {x \in R^n : A*x <= b, Aeq*x = beq} % % Note that the inequality constrants can be degenerate, although degeneracy % will create significantly more constraints compared to explicit equality % constraints. % % Optional argument ielim decides which column to eliminate. Default is the last % column. % Check arguments and get sizes. narginchk(2, 5); Nlt = size(A, 1); if ~isvector(b) || length(b) ~= Nlt error('Invalid size for b!'); end Nx = size(A, 2); if nargin() < 3 || isempty(Aeq) Aeq = zeros(0, Nx); beq = zeros(0, 1); Neq = 0; elseif nargin() < 4 error('beq is required if Aeq is given!'); else if size(Aeq, 2) ~= Nx error('Aeq has the wrong number of columns!'); end Neq = size(Aeq, 1); if ~isvector(beq) || length(beq) ~= Neq error('Invalid size for beq!'); end end if nargin() < 5 ielim = Nx; elseif ~isscalar(ielim) || ielim <= 0 || ielim > Nx || round(ielim) ~= ielim error('ielim must be a scalar positive integer less than Nx!'); end % Now decide what to do. First, look for an equality constraint with the % variable of interest. [pivval, pivrow] = max(abs(Aeq(:,ielim))); if isempty(pivrow) || pivval == 0 % No suitable equality constraint found. Need to change inequality % constraints. [A, b] = fmelim(A, b, ielim); Aeq(:,ielim) = []; else % An equality constraint is available. Handle that. a = Aeq(pivrow,:); c = a(ielim); % Make the pivots. Aeq = Aeq - Aeq(:,ielim)*a/c; beq = beq - Aeq(:,ielim)*beq(pivrow)/c; A = A - A(:,ielim)*a/c; b = b - A(:,ielim)*beq(pivrow)/c; % Get rid of the appropriate rows and columns. A(:,ielim) = []; Aeq(:,ielim) = []; Aeq(pivrow,:) = []; beq(pivrow) = []; end % Zap any rows that are all zeros. [A, b] = removezerorows(A, b); [Aeq, beq] = removezerorows(Aeq, beq); end%function % ***************************************************************************** % Helper Functions % ***************************************************************************** function [Ae, be] = fmelim(A, b, ielim) % Performs one step of Fourier-Motzkin elimination for inequality % constraints. c = A(:,ielim); I0 = find(c == 0); Ip = find(c > 0); Im = find(c < 0); Nx = size(A, 2); Ne = length(I0) + length(Ip)*length(Im); E = [A(:,1:ielim - 1), A(:,ielim + 1:end), b]; Ee = [E(I0,:); kron(c(Ip), E(Im,:)) - kron(E(Ip,:), c(Im))]; Ae = Ee(:,1:end-1); be = Ee(:,end); end%function function [A, b] = removezerorows(A, b) % Removes rows of A that are all zeros. keeprows = any(A, 2); A = A(keeprows,:); b = b(keeprows); 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 | function l = vertices2lines(p, xlims, ylims) % l = vertices2lines(p, xlims, ylims) % % Returns 2-column array of line segments corresponding to edges of the % polygon with vertices in columns of p (which must be in the proper order). % Each line segment has 101 interpolation points (to make gnuplot happy), % and there is a row of NaNs inserted between each line. dp = diff(p, 1, 2); N = size(p, 2) - 1; blocks = cell(N, 1); xlims = linspace(xlims(1), xlims(2), 101)'; ylims = linspace(ylims(1), ylims(2), 101)'; for i = 1:N slope = dp(2,i)/dp(1,i); if isinf(slope) || isnan(slope) x = p(1,i)*[1; 1]; y = ylims; else x = xlims; y = slope*(xlims - p(1,i)) + p(2,i); end blocks{i} = [[x, y]; NaN(1, 2)]; end l = vertcat(blocks{:}); |