## Code for Figure 2.12

### 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
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% Exercise "Terminal penalty with and without terminal constraint"

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

% Bounds on variables.
xlb = [-15; -inf()];
xub = [15; inf()];
ulb = [-5; -5];
uub = [5; 5];

% Find LQR.

[K, P] = dlqr(A, B, Q, R);
K = -K; % Sign convention.

% Also calculate the control law with increased penalty.
P3 = 3*P;
for i = 1:N
[P3, K3] = riciter(A, B, Q, R, P3);
end

% Calculate Xn and Xf (maximum LQR-invariant set) using normal penalty.
[Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, 'lqr');
title('Terminal Penalty P');

% Also calculate Xf using increased penalty.
[Xn3, V3] = findXn(A, B, K3, 0, xlb, xub, ulb, uub, 'lqr', false());
legstrs = [get(legend, 'string')(:); {'Xf w/ 3P'}]; %make a column vector;
%6.1/6.2 compatibility
hold('on');
plot(V3{1}(1,:), V3{1}(2,:), '--xk');
legend(legstrs{:});

% Get MPC matrices.
model = struct('A', A, 'B', B, 'N', N);
constraint = Z;
penalty = struct('Q', Q, 'R', R, 'P', P);
terminal1 = Xn{1}; % Terminal set.
terminal3 = Xn3{1}; % Terminal set with increased penalty.

% Remove terminal constraint and see if MPC is still stabilizing.
[x1, x2] = meshgrid(linspace(-10, 10, 25), linspace(-5, 5, 25));
x0 = [x1(:), x2(:)]';
Npts = size(x0, 2);
Nstab = 10; % Number of steps to try to declare stabilizing.
Ps = {P, P3};
names = {'one', 'three'};
Xf = {terminal1, terminal3};
x0stabilizing = struct();
terminal = []; % Remove terminal constraint.
for n = 1:length(names)
stab = false(Npts, 1);
penalty.P = Ps{n};
mpcmats = []; % Need to rebuild matrices.
for i = 1:Npts
% Get initial condition.
model.x0 = x0(:,i);

% Otherwise, simulate MPC.
for k = 0:Nstab
% If initial condition is inside Xf, then the trajectory is
% stabilized.
if all(Xf{n}.A*model.x0 <= Xf{n}.b);
stab(i) = true();
break;
end

% Give up if we aren't stabilizing after Nstab steps.
if k == Nstab
break;
end

% Otherwise, solve MPC problem.
[xk, uk, ~, status, mpcmats] = linearmpc(model, constraint, ...
penalty, terminal, ...
mpcmats);
model.x0 = xk(:,2); % Advance state.
if ~status.optimal
% Problem is infeasible. Give up.
break
end
end
end
x0stabilizing.(names{n}) = x0(:,stab); % Save results to struct.
end

% Check where MPC gives infinite-horizon control law. Only need to check
% feasible points.
[x1, x2] = meshgrid(linspace(-10, 10, 50), linspace(-5, 5, 50));
x0 = [x1(:), x2(:)]';
X3 = Xn{end}; % Feasible set.
x0 = x0(:,all(bsxfun(@le, X3.A*x0, X3.b))); % Keep only feasible points.
Npts = size(x0, 2);
infhorizon = false(Npts, 1);
terminal = terminal1; % Use original terminal penalty.
mpcmats = [];
for i = 1:Npts
model.x0 = x0(:,i);
[xk, uk, ~, status, mpcmats] = linearmpc(model, constraint, penalty, ...
terminal, mpcmats);
if status.optimal
% Check whether the ending state is in the interior (with tolerance).
infhorizon(i) = all(terminal.A*xk(:,end) < terminal.b - 1e-5);
else
warning('%d: not optimal (%s = %d)', i, status.solver, status.flag);
end
end
x0infhorizon = x0(:,infhorizon);

% Make plots.
datasets = {
x0infhorizon', 'Control Law = Inf. Horizon';
x0stabilizing.one', 'Stabilizing with penalty 1/2*P';
x0stabilizing.three', 'Stabilizing with penalty 3/2*P';
};
for i = 1:size(datasets, 1)
figure();
pts = datasets{i, 1};
plot(V{end}(1,:), V{end}(2,:), '-k', pts(:,1), pts(:,2), 'xk');
xlabel('x_1');
ylabel('x_2');
title(datasets{i, 2});
end

% Save data files.
pa = V{1}'; % Vertices for Xf.
pb = V{end}'; % Vertices for Xn (with n = 3).
pe = datasets{1, 1};
pg = datasets{2, 1};
pf = datasets{3, 1};

save('NoTerminal.dat', 'pa', 'pb', 'pe', 'pg', 'pf');



### linearmpc.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
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
233function [Xk, Uk, phi, status, matrices] = linearmpc(model, constraint, penalty, terminal, matrices)
% [Xk, Uk, phi, status, matrices] = linearmpc(model, constraint, penalty, terminal, [matrices])
%
% Solves the following optimization problem:
%
%      N-1
% min  sum [ 1/2(x_k' Q x_k + u_k' R u_k ) ] + 1/2(x_N' P x_n)
% u_k  k=1
%
%    s.t    x_k+1 = A x_k + B u_k
%           G x_k + H u_k <= psi
%           A_f x_N <= b_f
%
% Linear time-invariant MPC.
%
% Input 'model' has the following fields:
% - A: A matrix for system.
% - B: B matrix for system.
% - N: horizon length in number of stemps.
% - x0: initial state of system.
%
% Input 'constraint' has the following fields:
% - G: G matrix for constraints.
% - H: G matrix for constraints.
% - psi: psi vector for constraints.
% Either all or none must be specified.
%
% Input 'penalty' has the following fields:
% - Q: Penalty matricies for state.
% - R: Penalty matrix for input.
% - P: Terminal penalty matrix.
%
% Input terminal can be a vector of length x to specify a terminal equality
% constraint, or a struct with fields A and b (corresponding to A_f and b_f in
% the optimization given above).
%
% The final output matrices is a struct with fields H, f, Alt, blt, Aeq, beq,
% lb, and ub that define the QP formulation. If these are supplied in a struct
% as a fifth argument, only the initial condition model.x0 is updated, and the
% matrices are passed directly to the QP solver. This can save a lot of time if
% you're solving the same problem over and over but with different initial
% conditions. Note that minimal error checking is done

% Check arguments.
narginchk(4, 5);

% Build matrices if not supplied.
if nargin() < 5 || isempty(matrices)
matrices = buildmatrices(model, constraint, penalty, terminal);
end

% Get sizes.
[numx, numu] = size(model.B);
N = model.N;
numVar = N*(numx + numu) + numx;

% Update initial condition.
matrices.lb(1:numx) = model.x0;
matrices.ub(1:numx) = model.x0; % Fix x0.

% Get vectors to extract x and u from big column vector.
allinds = mod(0:numVar - 1,numx + numu);
xinds = allinds < numx;
uinds = allinds >= numx;

% Solve QP using Octave/Matlab default solver.
if isOctave()
% Helps to generate a feasible guess.
uguess = zeros(numu,N);
xguess = zeros(numx,N+1);
x0 = model.x0;
if ~isempty(x0)
A = model.A;
B = model.B;
xguess(:,1) = x0;
for i = 1:N
xguess(:,i+1) = A*xguess(:,i) + B*uguess(:,i);
end
end
guess = sparse(numVar, 1);
guess(xinds) = xguess(:);
guess(uinds) = uguess(:);
[X, FVAL, INFO] = qp(guess, matrices.H, matrices.f, ...
matrices.Aeq, matrices.beq, matrices.lb, matrices.ub, [], ...
matrices.Alt, matrices.blt);
status = struct('solver', 'qp', 'flag', INFO.info);
status.optimal = (status.flag == 0);
else
% Options for Matlab solver.

% Need to disable a warning.
stupidwarningstate = warning('query', STUPIDWARNING);
warning('off',STUPIDWARNING)

% Matlab is very fussy about the hessian being exactly symmetric.
H = matrices.H;
H = 0.5*(H + H');

% call solver.
[X, FVAL, solverflag] = ...
matrices.Aeq, matrices.beq, matrices.lb, matrices.ub, [], qpoptions);
status = struct('solver', 'quadprog', 'flag', solverflag);
status.optimal = (status.flag == 1);
if ~status.optimal
X = NaN(numVar, 1);
end

% reset the state of the warning.
warning(stupidwarningstate.state, STUPIDWARNING);
end

% Extract x and u from quadprog X.
Xk = reshape(X(xinds), numx, N + 1);
Uk = reshape(X(uinds), numu, N);

phi = FVAL;

end%function

% ******************************************************************************
% Helper function for building matrices.
% ******************************************************************************

function matrices = buildmatrices(model, constraint, penalty, terminal)
% matrices = buildmatrices(model, constraint, penalty, terminal)
%
% Returns the matrices struct used by the main function.

N = model.N;
A = model.A;

if diff(size(A)) ~= 0
error('model.Ak must be a square matrix.');
end
numx = size(A,1); % Number of states.

B = model.B;
numu = size(B,2); % Number of inputs.

if isfield(constraint,'G')
G = constraint.G;
H = constraint.H;
psi = constraint.psi;
else
G = zeros(0, numx);

H = zeros(0, numu);
psi = zeros(0, 1);
end

% Penalty matrices.
Q = penalty.Q;
R = penalty.R;
P = penalty.P;
M = zeros(numx, numu);

littleH = sparse([Q, M; M', R]);
bigH = kron(eye(N),littleH);
bigH = blkdiag(bigH, P); % Add final penalty matrix.

bigf = zeros(size(bigH, 1), 1);

% Structure of big A (both Aeq and Alt) is
% +-                  -+
% |  A1 A2  0  0  ...  |
% |   0 A1 A2  0  ...  |
% |   0  0 A1 A2  ...  |
% |  ...         A2  0 |
% |  ...         A1 A2 |
% +-                  -+
%
% We construct it first as
%
% +-             -+
% |  ... A1 A2  0 |
% |  ...  0 A1 A2 |
% |  ...  0  0 A1 | <= Note exta A1 that has to be removed.
% +-             -+
%
% and then get rid of extra rows (for last A1) and columns (for
% nonexistant variable u_N).

% For Equalities:
% A1 = [A, B], A2 = [-I, 0].
littleAeq1 = sparse([A, B]);
littleAeq2 = sparse([-eye(size(A)), zeros(size(B))]);

bigAeq = kron(eye(N+1),littleAeq1) + kron(diag(ones(1,N),1),littleAeq2);

bigAeq = bigAeq(1:end-numx,1:end-numu);
% Remove columns for u_N and for rows of x_N+1 = A x_N + B u_N

bigbeq = zeros(numx*N,1);

% For Inequalities:
% A1 = [G, H], A2 = [0, 0].
littleAlt1 = sparse([G, H]);
littleAlt2 = sparse([zeros(size(G)), zeros(size(G))]);

bigAlt = kron(eye(N+1),littleAlt1) + kron(diag(ones(1,N),1), ...
littleAlt2);
bigAlt = bigAlt(1:end-size(littleAlt1,1),1:end-numu);
% Remove columns for u_N and for rows of G x_N + D u_N <= d.
bigblt = repmat(psi, N, 1);

% Variable bounds.
numVar = length(bigf);
LB = -inf*ones(numVar,1);
UB = inf*ones(numVar,1);

% Decide terminal constraint.
if isstruct(terminal) && all(isfield(terminal, {'A', 'b'}))
Af = terminal.A;
bf = terminal.b;
bigAlt = [bigAlt; [sparse(size(Af, 1), numVar - numx), sparse(Af)]];
bigblt = [bigblt; bf];
elseif isvector(terminal) && length(terminal) == numx
LB(end-numx+1:end) = terminal;
UB(end-numx+1:end) = terminal;
elseif isempty(terminal)
% No terminal constraint. Pass
else
error('Unknown input for terminal!');
end

% Build struct with appropriate names.
matrices = struct('H', bigH, 'f', bigf, 'Aeq', bigAeq, 'beq', bigbeq, ...
'Alt', bigAlt, 'blt', bigblt, 'lb', LB, 'ub', UB);

end%function



### 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
84function [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
19function [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
93function [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
107function [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);
error('A has infeasible trivial rows.')
end

% 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
62function 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
39function 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
104function [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
88function [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
92function [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



### riciter.m


1
2
3
4function [P, K] = riciter(A, B, Q, R, P)
% Performs one step if riccati iteration.
P = Q + A'*P*A - A'*P*B*((B'*P*B + R)\(B'*P*A));
K = -(B'*P*B + R)\(B'*P*A);