## Code for Figure 6.9

### 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% Closed-loop evolution of nonlinear system for N = 2.
sys = nonlinsys();

% Define objective function and bounds.
Nt = 2;
Nu = sys.Nu;
Nx = sys.Nx;
V = @(x, u) sys.Vf(x);
for t = (Nt - 1):-1:0
i = (t*Nu + 1):((t + 1)*Nu); % Index of current u.
V = @(x, u) sys.l(x, u(i)) + V(sys.f(x, u(i)), u);
end

ulb = repmat(sys.ulb, Nt, 1);
uub = repmat(sys.uub, Nt, 1);
I = cell(Nu, 1);
for i = 1:length(I)
I{i} = (i:Nu:(Nt*Nu))';
end

% Simulate closed loop for different numbers of iterations.
Nsim = 10;
Ps = [3, 10];
options = struct();
options.p3 = {'alphamax', 50, 'beta', 0.95};

x0 = [3; -3];
data = struct();
for P = Ps
% Get options.
key = sprintf('p%d', P);
opts = structGet(options, key, {});

% Preallocate simulation vectors.
x = NaN(Nx, Nsim + 1);
x(:,1) = x0;
u = NaN(Nu, Nsim);

% Generate an initial guess using the linear control law.
[xguess, uguess] = warmstart(sys, x0, NaN(Nu, Nt), 'linear');

% Simulate in closed loop.
for t = 1:Nsim
% Perform optimization.
[useq, uiter] = distributedgradient(@(u) V(x(:,t), u), uguess(:), ...
ulb, uub, 'I', I, 'Niter', P, ...
opts{:});
u(:,t) = useq(1:Nu);

% Simulate system and update warm start.
x(:,t + 1) = sys.f(x(:,t), u(:,t));
uguess = [reshape(useq((Nu + 1):end), Nu, Nt - 1), NaN(Nu, 1)];
[xguess, uguess] = warmstart(sys, x(:,t + 1), uguess, 'nonlinear');

% Stop if something went wrong.
if norm(x(:,t + 1)) > 100
warning('x has diverged!');
break
end
end

% Make a plot.
figure();
t = 0:Nsim;

subplot(2, 1, 1);
hold('on');
plot(t, x(1,:), '-or');
plot(t, x(2,:), '-sb');
title(sprintf('p = %d', P));

subplot(2, 1, 2);
hold('on');
stairs(t, u(1,[1:end,end]), '-or');
stairs(t, u(2,[1:end,end]), '-sb');
plot([0, Nsim], ulb(1)*[1, 1], ':k', [0, Nsim], uub(1)*[1, 1], ':k');

% Store data set.
data.(key) = struct('t', t, 'x', x, 'u', u, 'ulb', ulb, 'uub', uub);
end

```

### nonlinsys.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
40function sys = nonlinsys()
% sys = nonlinsys()
%

% Returns a struct of parameters for the unstalbe nonlinear system.
sys = struct();

% Define the model and stage cost.
sys.Nx = 2;
sys.Nu = 2;
sys.f = @(x, u) [x(1)^2 + x(2) + u(1)^3 + u(2); x(1) + x(2)^2 + u(1) + u(2)^3];
sys.l = @(x, u) 0.5*(x'*x + u'*u);

% Define steady state and bounds.
sys.xs = zeros(sys.Nx, 0);
sys.us = zeros(sys.Nu, 0);
sys.ulb = [-2.5; -2.5];
sys.uub = [2.5; 2.5];

% Linearize the ODE
fcasadi = mpctools.getCasadiFunc(sys.f, [sys.Nx, sys.Nu], {'x', 'u'}, {'f'});
Jfcasadi = fcasadi.factory('Jfcasadi', {'x','u'}, {'jac:f:x', 'jac:f:u'});
[A,B] = Jfcasadi(sys.xs, sys.us);
sys.A = full(A);
sys.B = full(B);

% Linearize the objective
lcasadi = mpctools.getCasadiFunc(sys.l, [sys.Nx, sys.Nu], {'x', 'u'}, {'l'});
Hlcasadi = lcasadi.factory('Hlcasadi', {'x','u'}, {'hess:l:x:x', 'hess:l:u:u'});
[Q,R] = Hlcasadi(sys.xs, sys.us);
sys.Q = full(Q);
sys.R = full(R);

[K, sys.P] = dlqr(sys.A, sys.B, sys.Q, sys.R);
sys.K = -K;
sys.Vf = @(x) 0.5*x'*sys.P*x;

end%function

```

### distributedgradient.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
149function [ustar, uiter] = distributedgradient(V, u0, ulb, uub, varargin)
% [ustar, uiter] = distributedgradient(V, u0, ulb, uub, ...)
%
% Runs a distributed gradient algorithm for a problem with box constraints.
%
% V should be a function handle defining the objective function. u0 should be
% the starting point. ulb and uub should give the box constraints for u.
%
% Additional arguments are passed as 'key' value pairs. They are as follows:
%
% - 'beta', 'sigma' : parameters for line search (default 0.8 and 0.01)
% - 'alphamax' : maximum step to take in any iteration (default 10)
% - 'Niter' : Maximum number of iterations (default 25)
% - 'steptol', 'gradtol' : Absolute tolerances on stepsize and gradient.
%                          Algorithm terminates if the stepsize is less than
%                          steptol and the gradient is less than gradtol
%                          (defaults 1e-5 and 1e-6). Set either to a negative
%                          value to never stop early.
% - 'Nlinesearch' : maximum number of backtracking steps (default 100)
% - 'I' : cell array defining the variables for each system. Default is
%         each variable is a separate system.
%
% Return values are ustar, the final point, and uiter, a matrix of iteration
% progress.
narginchk(4, inf());

% Get options.
param = struct('beta', 0.8, 'sigma', 0.01, 'alphamax', 15, 'Niter', 50, ...
'steptol', 1e-5, 'gradtol', 1e-6, 'Nlinesearch', 100, ...
'I', []);
for i = 1:2:length(varargin)
param.(varargin{i}) = varargin{i + 1};
end
Nu = length(u0);
if isempty(param.I)
param.I = num2cell((1:Nu)');
end

% Project initial condition to the feasible space.
u0 = min(max(u0, ulb), uub);

% Get gradient function via CasADi.
usym = casadi.SX.sym('u', Nu);
vsym = V(usym);
Jsym = jacobian(vsym,usym);
dVcasadi = casadi.Function('V', {usym}, {Jsym, vsym});

% Start the loop.
k = 1;
uiter = NaN(Nu, param.Niter + 1);
uiter(:,1) = u0;
[g, gred] = calcgrad(dVcasadi, u0, ulb, uub);
v = inf(Nu, 1);
while k <= param.Niter && (norm(gred) > param.gradtol || norm(v) > param.steptol)
% Take step.
[uiter(:,k + 1), v] = coopstep(param.I, V, g, uiter(:,k), ulb, uub, param);
k = k + 1;

% Update gradient.
[g, gred] = calcgrad(dVcasadi, uiter(:,k), ulb, uub);
end

% Get final point and strip off any extra points in uiter.
uiter = uiter(:,1:k);
ustar = uiter(:,end);

end%function

function [u, v] = coopstep(I, V, g, u0, ulb, uub, param)
% [u, v] = coopstep(I, V, g, u0, ulb, uub, param)
%
% Takes one step of cooperative distributed gradient projection subject
% to box constraints on u.
%
% I should be the cell array of partitions. V should be the function handle
% and g its gradient at u0.
narginchk(7, 7);
Nu = length(u0);
Ni = length(I);
us = NaN(Nu, Ni);
Vs = NaN(1, Ni);

% Run each optimization.
V0 = V(u0);
for j = 1:Ni
% Compute step.
v = zeros(Nu, 1);
i = I{j};
v(i) = -g(i);
ubar = min(max(u0 + v, ulb), uub);
v = ubar - u0;

% Choose alpha.
if any(ubar == ulb) || any(ubar == uub)
alpha = 1;
else
alpha = [(ulb(i) - u0(i))./v(i); (uub(i) - u0(i))./v(i)];
alpha = min(alpha(alpha >= 0));
if isempty(alpha)
alpha = 0; % Nowhere to go. Possibly at a stationary point.
else
alpha = min(alpha, param.alphamax); % Don't step too far.
end
end

% Backtracking line search.
k = 1;
while V(u0 + alpha*v) > V0 + param.sigma*alpha*(g(i)'*v(i)) ...
&& k <= param.Nlinesearch
alpha = param.beta*alpha;
k = k + 1;
end
us(:,j) = u0 + alpha*v;
Vs(j) = V(us(:,j));
end

% Choose step to take.
w = ones(Ni, 1)/Ni;
for j = 1:Ni
% Check for descent.
u = us*w;
if V(u) <= Vs*w
break
elseif j == Ni
u = u0;
warning('No descent directions!');
break
end

% Get rid of worst player.
[~, jmax] = max(Vs);
Vs(jmax) = -realmax(); % Exclude from later steps.
w(jmax) = 0;
w = w/sum(w);
end

% Calculate actual step.
v = u - u0;
end%function

function [g, gred] = calcgrad(dV, u, ulb, uub)
% Calculates the gradient and reduced gradient at a given point.
narginchk(4, 4);
g = full(dV(u));
g = g(:); % Column vector.
gred = g;
gred((u == ulb) & (g > 0)) = 0;
gred((u == uub) & (g < 0)) = 0;
end%function

```

### warmstart.m

```
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17function [x, u] = warmstart(sys, x, u, model)
% Fills in a warm start using the terminal control law.
Nt = size(u, 2);
x = [x, NaN(length(x), Nt)];
for t = 1:Nt
if any(isnan(u(:,t)))
u(:,t) = min(max(sys.K*x(:,t), sys.ulb), sys.uub);
end
switch model
case 'nonlinear'
x(:,t + 1) = sys.f(x(:,t), u(:,t));
case 'linear'
x(:,t + 1) = sys.A*x(:,t) + sys.B*u(:,t);
otherwise
error('Invalid choice for model!');
end
end

```