Figure 2.10:

Region of attraction (shaded region) for constrained MPC controller of Exercise 2.6.

Code for Figure 2.10

Text of the GNU GPL.

main.m


 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');

findXn.m


 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

hyperrectangle.m


 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

calcOinf.m


 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

removeredundantcon.m


  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

computeX1.m


 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

plotpoly.m


 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

fouriermotzkin.m


  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

halfspace2vertex.m


 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

findinteriorpoint.m


 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

vertices2lines.m


 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{:});