MPC_with_multiple_shooting

function Example5
% очистить command window, workspace, закрыть графику
clc, clear all, close all
import casadi.*

% Горизонт управления
T = 1.5;
mu = 0.5;

% Количество интервалов и период дискретизации
delta = 0.1;
N = T / delta;

% Переменные модели
x = MX.sym('x', 2);
u = MX.sym('u', 1);

% Стоимость этапа: L(x,u)
L = (x(1)^2 + x(2)^2) / 2 + u^2;

% Динамика системы: правая часть ДУ
xdot = [x(2) + u * (mu + (1 - mu) * x(1));
        x(1) + u * (mu - 4 * (1 - mu) * x(2))];
% Начальное состояние
x0 = [0.8; 0.12];

% Создаем интегратор с помощью CVODES из SUNDIALS Suite
% использование: x(delta | 0, x0, u) = F('x0', x0, 'p', u)
ode = struct('x', x, 'p', u, 'ode', xdot, 'quad', L);
F = integrator('F', 'cvodes', ode, struct('tf', delta));

% Количество итераций алгоритма MPC
iter = 30;
% Конечная траектория, управление и значение критерия оптимальности
x_final = [];
u_final = [];
J_final = 0;
for i = 1:iter
% Собираем переменные и ограничения задачи
u = MX.sym('u',N,1);
x = MX.sym('x',N,2);
% Формирование вектора cons, который в структуре NLP будет входить в
% условия g. Этот вектор должен равняться нулю, так как траектория x
% непрерывна
cons = vec(x(1,:))-x0;
J   = 0;
for k = 1:(N-1)
    res = F('x0', x(k,:), 'p', u(k));    % x(delta | 0, s, u(k))
    cons = [cons; vec(x(k+1,:)) - res.xf];
    J   = J + res.qf;
end
res = F('x0', x(N,:), 'p', u(N));
% Терминальное слагаемое
E = 16.5926*(res.xf(1)^2+res.xf(2)^2)+2*11.5926*res.xf(1)*res.xf(2);
% Создаем NLP-solver
% Теперь переменные оптимизации - u и x. Также добавляются новые условия
% непрерывности cons
nlp = struct('x', [u; x(:)], 'f', J + E, 'g', [cons; E]);
solver = nlpsol('solver', 'ipopt', nlp);
% Решаем задачу нелинейного программирования
tic
sol = solver('x0', zeros(3*N,1),...
    'lbx', [repmat(-2,N,1); repmat(-Inf,2*N,1)],...
    'ubx', [repmat(2,N,1); repmat(+Inf,2*N,1)],...
    'lbg', [zeros(2*N,1); -Inf],...
    'ubg', [zeros(2*N,1); 0.7]);
t_Elapsed = toc;
opt = full(sol.x);

% Первая фаза оптимальной траектории сохраняется как очередная фаза
% окончательной, затем формируется новое начальное условие в переменной x0,
% обновляются окончательное управление, критерий оптимальности
x_final = [x_final [opt(N+1);opt(2*N+1)]];
x0 = [opt(N+2);opt(2*N+2)];
u_final = [u_final opt(1)];
res = F('x0', [opt(N+1) opt(2*N+1)], 'p', opt(1));
J_final = J_final + res.qf;
end
% Вывод окончательных результатов
results(delta*(0:iter-1),x_final,u_final(1:iter-1));
% -----------------------------------------------------------------
% Вывод результатов
% -----------------------------------------------------------------
function results(T,X,U)
    fprintf('   k  |      u(k)        x(1)        x(2)     \n');
    fprintf('----------------------------------------------\n');
    for k = 1:length(T)-1
        fprintf(' %3.1f  | %+11.6f %+11.6f %+11.6f \n', T(k), U(k),...
            X(1,k), X(2,k));
    end
    fprintf('Термин.состояние  = %+11.6f %+11.6f \n\n', X(1,end),...
        X(2,end));
    % Тут происходит ошибка, типа не может преобразовать из casadi.MX в
    % double, поэтому следующая строка закомментирована
    %fprintf('Оптим. значение   = %6.4f \n\n', J_final);
    fprintf('Время решения     = %6.4f \n\n', t_Elapsed);

    fig = figure(1);
    set(fig,'Position', [50    100   1000   400]);

    subplot(1,2,1); hold on; grid on;
    plot(X(1,:),X(2,:),'-k', 'Linewidth', 1);
    xlabel('x1')
    ylabel('x2')
    title('фазовый портрет')

    subplot(1,2,2); hold on; grid on;
    stairs(T,[U nan],'-k', 'Linewidth', 1);
    plot(T, X(1,:), '-', 'Linewidth', 1)
    plot(T, X(2,:), '--', 'Linewidth', 1)
    xlabel('t')
    legend('Location', 'southwest')
    legend('u','x1','x2')
    title('оптимальное управление, траектория')
end

end