Code for Figure 2.2

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% Exercise for ch2; Continuation of Example 2.5: Linear quadratic MPC
% jbr, March 1, 2009

H = [3, 1; 1, 2];
hr1 = H(1,:);
hr2 = H(2,:);

Hinv = (1/5)*[2 -1, -1 3];
n1 = 1/sqrt(5)*[2; -1];
% n2 = 1/sqrt(10)*[-1; 3];

[V,lam]=eig(H);
v1 = V(:,1);
v2 = V(:,2);

box = [-1, -1; 1, -1; 1, 1; -1, 1; -1, -1];

umin = -4.75;
av = -[3; 1];
line = [0*av, -umin/3*av]';

xc1 = 5/3;

xc2 = 3;
ac2 = xc2/5*av;
bc2 = 5/2*(3*xc2/5-1)^2;
[xec2,yec2] = ellipse(H, bc2,100,ac2);

x3 = 4.5;
a3 = x3/5*av;
b3 = ([-1;-1]-a3)'*H*([-1;-1]-a3);
[xe3,ye3] = ellipse(H, b3,100,a3);

x4 = 2.25;
a4 = x4/5*av;
b4 = 5/2*(3*x4/5-1)^2;
[xe4,ye4] = ellipse(H, b4,100,a4);
u0 = [-1; -(x4-1)/2];

figure();
plot(box(:,1),box(:,2),0, 0, 'o', -1, -1/3, 'o', line(:,1),line(:,2), ...
ac2(1),ac2(2),'x', xec2, yec2, ...
a3(1), a3(2), 'x', xe3, ye3, -1, -1, 'o', ...
a4(1), a4(2), 'x', xe4, ye4, u0(1), u0(2), 'o')

axis([umin, 1.5, -3.5, 1.5])

table1 = [xec2, yec2, xe3, ye3, xe4, ye4];
table2 = [0, 0, -1, -1/3, ac2', a3', a4', [-1, -1], u0'];

save constlq.dat table1 table2 box line



ellipse.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% Copyright (C) 2001, James B. Rawlings and John W. Eaton
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License as
% published by the Free Software Foundation; either version 2, or (at
% your option) any later version.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; see the file COPYING.  If not, write to
% the Free Software Foundation, 59 Temple Place - Suite 330, Boston,
% MA 02111-1307, USA.

% [x, y, major, minor, bbox] = ellipse (amat, level, n, shift)
%
% Given a 2x2 matrix, generate ellipse data for plotting.  The
% arguments N and SHIFT are optional.  If N is an empty matrix, a
% default value of 100 is used.

function [x, y, major, minor, bbox] = ellipse (amat, level, n, shift)

if (nargin < 3)
n = 100;
end

if (isempty (n))
n = 100;
end

if (nargin < 4)
shift = [0, 0];
end

ss = size (shift);

if (any (ss ~= [1, 2]))
if (ss == [2, 1])
shift = shift';
else
error ('shift must be a 2-element row vector');
end
end

if (nargin > 1)

[v, l] = eig (amat / level);

dl = diag(l);
if (any (imag (dl)) || any (dl <= 0))
error ('ellipse: amat must be positive definite');
end

% Generate contour data.

a = 1 / sqrt (l(1,1));
b = 1 / sqrt (l(2,2));

t = linspace (0, 2*pi, n)';

xt = a * cos (t);
yt = b * sin (t);

% Rotate the contours.

ra = atan2 (v(2,1), v(1,1));

cos_ra = cos (ra);
sin_ra = sin (ra);

x = xt * cos_ra - yt * sin_ra + shift(1);
y = xt * sin_ra + yt * cos_ra + shift(2);

% Endpoints of the major and minor axes.

minor = (v * diag ([a, b]))';
major = minor;

major(2,:) = -major(1,:);
minor(1,:) = -minor(2,:);

t = [1; 1] * shift;

major = major + t;
minor = minor + t;

% Bounding box for the ellipse using magic formula.

ainv = inv (amat);
xbox = sqrt (level * ainv(1,1));
ybox = sqrt (level * ainv(2,2));

bbox = [xbox ybox; xbox -ybox; -xbox -ybox; -xbox ybox; xbox ybox];

t = [1; 1; 1; 1; 1] * shift;
bbox = bbox + t;

else
error ('usage: ellipse (amat, level, n, shift)');
end

% end%function