Figure 6.10:

Contours of V(x(0),\mathbf {u}_1,\mathbf {u}_2) for N=1.

Code for Figure 6.10

Text of the GNU GPL.

main.m

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
% Iterations of nonlinear system for N = 1.
sys = nonlinsys();

% Define objective function.
x = [3; -3];
V = @(u) sys.l(x, u) + sys.Vf(sys.f(x, u));

% Simulate projected gradient.
u0 = [0; 0];
[~, uiter] = distributedgradient(V, u0, sys.ulb, sys.uub);

% Get cost function contours.
[u1, u2] = meshgrid(linspace(-3, 2, 101), linspace(-3, 1, 81));
V = arrayfun(@(u1, u2) V([u1; u2]), u1, u2);

% Make a plot.
figure();
hold('on');
c = contour(u1, u2, V, 25);
plot(uiter(1,:), uiter(2,:), '-ok');

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
40
function 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
149
function [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