Figure 7.8:

Solution times for explicit and implicit MPC for N = 20.

Code for Figure 7.8

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
 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
% Example explicit MPC for a 2D system From "Optimal control of constrained
% piecewise affine discrete-time systems" (Mayne and Rakovic, 2002).


pkg ('load', 'control')

% Choose system.
A = [1,0.1; 0,1];
B = [0; 0.0787];
Q = [1, 0; 0, 0];
R = 0.1;
ulb = -1;
uub = 1;
xlb = -inf(2, 1);
xub = inf(2, 1);
N = 20;
[Nx, Nu] = size(B);

% Find LQR terminal set.
[K, P] = dlqr(A, B, Q, R);
K = -K;
[Xn, Xnverts] = findXn(A, B, K, N, xlb, xub, ulb, uub, 'lqr');
A_lqr = Xn{1}.A;
b_lqr = Xn{1}.b;
feas = Xn{end}; % Gives region X_N.

% 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', false());


xbox = [-6, 6; -3, 3];
Npts = 10000;
Nmax = 20000;
xpts = NaN(Nx, Npts);
lookuptimes = NaN(Npts, 1);
qptimes = NaN(Npts, 1);
jx = 0;
rng(917);
for i = 1:Nmax
    xmin = xbox(:,1);
    dx = xbox(:,2) - xbox(:,1);
    xtry = xmin + dx.*rand(Nx, 1);
    if all(feas.A*xtry - feas.b <= 0)
        jx = jx + 1;
        if mod(jx, 500) == 1
            fprintf('Point %d of %d\n', jx, Npts);
        end
        xpts(:,jx) = xtry;

        % Do PWA lookup.
        tic();
        [zlookup, id] = pwalookup(xtry, regions, feas);
        lookuptimes(jx) = toc();

        % Solve QP.
        tic();
        H = mpqp.Q;
        q = mpqp.C*xtry;
        A = mpqp.A;
        b = mpqp.b + mpqp.S*xtry;
        if isOctave()
            z0 = zeros(Nu*N, 1);
            [zqp, ~, status] = qp(z0, H, q, [], [], [], [], [], A, b);
            okay = (status.info == 0);
        else
            H = 0.5*(H + H'); % Matlab is picky.
            options =  optimoptions('quadprog', 'display', 'off', ...
                                    'OptimalityTolerance', 1e-15, ...
                                    'StepTolerance', 1e-15, ...
                                    'MaxIterations', 500);
            prob = struct('H', H, 'f', q, 'Aineq', A, 'bineq', b, ...
                          'solver', 'quadprog', 'options', options);
            [zqp, ~, status] = quadprog(prob);
            okay = (status == 1);
        end
        if ~okay
            warning('QP failed at step %d!', jx);
        end
        qptimes(jx) = toc();

        % Make sure the solutions agree.
        if max(abs(zqp - zlookup)) > 1e-5
            warning('Mismatch for point %d!', jx);
        end

        % Stop if we've taken enough points.
        if jx == Npts
            break
        end
    end
end
fprintf('Sampled %d points.\n', jx);
xpts = xpts(:,1:jx);
lookuptimes = 1000*lookuptimes(1:jx); % Convert to ms.
qptimes = 1000*qptimes(1:jx); % Conver to ms.


sig = 1; % Kernel width in ms.
tplot = linspace(0, 100, 1001); % Plot range in ms.

lookupdensity = kerneldensity(lookuptimes, sig, tplot);
qpdensity = kerneldensity(qptimes, sig, tplot);

figure();
plot(tplot, lookupdensity, '-r', tplot, qpdensity, '-g');
legend('Explicit', 'Implicit', 'Location', 'NorthEast');
title(sprintf('%d Samples', size(xpts, 2)));
xlabel('Solution Time (ms)');
ylabel('Samples');

scatterplot(xpts, lookuptimes, tplot, 'Explicit');
scatterplot(xpts, qptimes, tplot, 'Implicit');

solvempqp.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
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

ss2mpqp.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
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

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

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

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

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

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

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

pwalookup.m


 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
function [z, id] = pwalookup(x, regions, feas)
    % [z, id] = pwalookup(x, regions, [feas])
    %
    % Returns optimal point z. If feas is provided, it should be a struct with
    % fields A and b to specify the feasible region. If not provided, all
    % regions will be searched if x is infeasible.
    z = [];
    id = '';
    if nargin() < 3 || all(feas.A*x - feas.b <= 0)
        for i = 1:length(regions)
            region = regions{i};
            if all(region.A*x <= region.b)
                z = region.Z*x + region.z0;
                id = region.id;
                found = true();
                break
            end
        end
    end

kerneldensity.m


1
2
3
4
5
6
7
8
function p = kerneldensity(xsamples, sigma, x)
    % Returns kernel density estimate at points x using samples in xsamples and
    % a Gaussian kernel with the given standard deviation.
    xsamples = xsamples(:);
    x = x(:)';
    den = sqrt(2*pi())*sigma*length(xsamples);
    pdf = @(x, mu) exp(-(x - mu).^2./(2*sigma.^2))./den;
    p = sum(bsxfun(pdf, x, xsamples));

scatterplot.m


1
2
3
4
5
6
function scatterplot(x, y, t, titlestr)
    figure();
    scatter(x(1,:), x(2,:), 5, y, 'filled');
    caxis([t(1), t(end)]);
    colorbar();
    title(titlestr);