Steepest Descent Method

gistfile1.matlab

function steepest
% Exemplo 6.2-2 do livro Optimal Control Theory de D.E.Kirk
% Steepest Descent Method

eps = 1e-3;
options = odeset('RelTol', 1e-4, 'AbsTol',[1e-4 1e-4]); %permite ajustar os parametros de integracao para os solvers de EDOs
t0 = 0; 
tf = 0.78;
R = 0.1;
step = 0.4;
t_segment = 50;
Tu = linspace(t0, tf, t_segment);    % discretiza no tempo
u = ones(1,t_segment);               % cria um array de 1 fazendo u=1
initx = [0.05 0];                    % valores iniciais para estados x(0) = [0.05 ; 0]
initp = [0 0];                       % valores iniciais para co-estados
max_iteration = 100;                 % Maximo numero de iteraçoes

for i = 1:max_iteration
   
   [Tx,X] = ode45(@(t,x) stateEq(t,x,u,Tu), [t0 tf], initx, options);
  
   x1 = X(:,1); x2 = X(:,2);
   [Tp,P] = ode45(@(t,p) costateEq(t,p,u,Tu,x1,x2,Tx), [tf t0], initp, options);
   p1 = P(:,1);
   p1 = interp1(Tp,p1,Tx);
   
   % derivada Hamiltoniana em relacao a u
   dH = pH(x1,p1,Tx,u,Tu);
   H_Norm = dH'*dH;
   
   % Funcao de custo
   J(i,1) = tf*(((x1')*x1 + (x2')*x2)/length(Tx) + R*(u*u')/length(Tu));
   
   if H_Norm < eps
       J(i,1)
       break;
   else
       u_old = u;
       u = AdjControl(dH,Tx,u_old,Tu,step);
   end; 
end

figure(1);
plot(Tx, X ,'-');
hold on;
plot(Tu,u,'r:');
text(.2,0.08,'x_1(t)');
text(.25,-.1,'x_2(t)');
text(.2,.4, 'u(t)');
s = strcat('Final cost is: J=',num2str(J(end,1)));
text(.4,1,s);
xlabel('time');
ylabel('states');
hold off;
print -djpeg90 -r300 eg2_descent.jpg

figure(2);
plot(J,'x-');
xlabel('Iteration number');
ylabel('J');
print -djpeg90 -r300 eg2_iteration.jpg

if i == max_iteration
    disp('Parou antes que o resultado pudesse ser encontrado.');
end

% Equacoes de Estado
function dx = stateEq(t,x,u,Tu)
dx = zeros(2,1);
u = interp1(Tu,u,t); 
dx(1) = -2*(x(1) + 0.25) + (x(2) + 0.5)*exp(25*x(1)/(x(1)+2)) - (x(1) + 0.25).*u;
dx(2) = 0.5 - x(2) -(x(2) + 0.5)*exp(25*x(1)/(x(1)+2));

% Costate equations
function dp = costateEq(t,p,u,Tu,x1,x2,xt)
dp = zeros(2,1);
x1 = interp1(xt,x1,t);  
x2 = interp1(xt,x2,t);
u = interp1(Tu,u,t);  

dp(1) = p(1).*(u + exp((25*x1)/(x1 + 2)).*((25*x1)/(x1 + 2)^2 - ...
        25/(x1 + 2))*(x2 + 1/2) + 2) - ...
        2*x1 - p(2).*exp((25*x1)/(x1 + 2))*((25*x1)/(x1 + 2)^2 - ...
        25/(x1 + 2))*(x2 + 1/2);
    
dp(2) = p(2).*(exp((25*x1)/(x1 + 2)) + 1) - ...
        p(1).*exp((25*x1)/(x1 + 2)) - 2*x2;

% Derivada parcial de H em relacao a u
function dH = pH(x1,p1,tx,u,Tu)
u = interp1(Tu,u,tx);
R = 0.1;
dH = 2*R*u - p1.*(x1 + 0.25);

% Ajusta o controle
function u_new = AdjControl(pH,tx,u,tu,step)
pH = interp1(tx,pH,tu);
u_new = u - step*pH;