Figure 6.9

% 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

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

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

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

Figure 6.9:

Closed-loop state and control evolution with (x_1(0),x_2(0)) = (3,-3).

Code for Figure 6.9

main.m

nonlinsys.m

distributedgradient.m

warmstart.m