Skip to content

Instantly share code, notes, and snippets.

@rodrigovidal
Created June 11, 2012 22:46
Show Gist options
  • Save rodrigovidal/2913207 to your computer and use it in GitHub Desktop.
Save rodrigovidal/2913207 to your computer and use it in GitHub Desktop.
Steepest Descent Method
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;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment