direct_multiple_shooting

% Метод: Direct 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.7; -0.8];

% Создаем интегратор с помощью 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));

% Собираем переменные и ограничения задачи
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);
u_opt = opt(1:N);

% Восстанавливаем траекторию. Хотя она уже и так найдена в результате
% выполнения метода direct multiple shooting, но я хочу сравнить
% траектории, чтобы они совпадали
J_opt = full(sol.f);
x_opt = x0;
for k = 1:N
    res = F('x0', x_opt(:,k), 'p', u_opt(k));
    x_opt = [x_opt full(res.xf)];
end

% Оптимальная траектория, полученная по методу direct multiple shooting
x_opt2 = transpose(reshape(opt(N+1:3*N),[N,2]));
res = F('x0', x_opt2(:,N), 'p', u_opt(N));
x_opt2 = [x_opt2 full(res.xf)];
% Выводим результаты и графики
results(delta*(0:N),x_opt2,u_opt)

% Вывод, проверяющий условие непрерывности траектории
x_opt2 - x_opt
% -----------------------------------------------------------------
% Вывод результатов
% -----------------------------------------------------------------
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_opt(k),...
            X(1,k), X(2,k));
    end
    fprintf('Термин.состояние  = %+11.6f %+11.6f \n\n', X(1,end),...
        X(2,end));
    fprintf('Оптим. значение   = %6.4f \n\n', J_opt);
    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;
    ineqplot('16.5926*(x.^2+y.^2)+2*11.5926*x.*y<0.7',[-0.8 -0.2 -0.8...
        0.2]);
    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', 'west')
    legend('u','x1','x2')
    title('оптимальное управление, траектория')
end
function ineqplot(I,R,c)
% Plotting inequalities, simple and easy.
%
% Input arguments
% I - Inequality as string, i.e.  'x+y>10'
% R - Vector of four components defined by: [xmin, xmax, ymin, ymax],
%       if two components are passed: [min, max], the defined region
%       will be a square and xmin=ymin=min, xmax=ymax=max.
% c - A three-element RGB vector, or one of the MATLAB
%       predefined names, specifying the plot color.
%
% Output arguments
% h - returns the handle of the scattergroup
%       object created.
%
% Examples:
% >> ineqplot('x.^2+y.^2<10',[-5 5], 'r');
% >> h = ineqplot('y<x+3',[0 10 -5 5]);
% >> set(h,'MarkerFaceColor','r'); % Change color
%
%
% $ Author:   Jorge De Los Santos $
% $ Version:  0.1.0 (initial version) $
%
% Default color
if nargin<3, c='b'; end % blue default
% Length of R vector
if length(R)==4
    xmin = R(1); xmax = R(2);
    ymin = R(3); ymax = R(4);
elseif length(R)==2
    xmin = R(1); xmax = R(2);
    ymin = R(1); ymax = R(2);
end
% hold
set(gca,'NextPlot','add');
% Limits
axis([xmin xmax ymin ymax]);
% Number of points (Change N value for more resolution)
N = 150;
dx=(xmax-xmin)/N; %
dy=(ymax-ymin)/N;
[x,y]=meshgrid(xmin:dx:xmax, ymin:dy:ymax);
% Eval condition
idx = find(~eval(I));
x(idx) = NaN; %#ok
plot(x(:),y(:),c,...
    'LineStyle','-',...
    'MarkerSize',1);
end
end