clc
clear all
% State equations
syms x1 x2 p1 p2 u;
Dx1 = x2;
Dx2 = -x2 + u;
% Cost function inside the integral
syms g;
g = 0.5*u^2;
% Hamiltonian
syms p1 p2 H;
H = g + p1*Dx1 + p2*Dx2;
% Costate equations
Dp1 = -diff(H,x1);
Dp2 = -diff(H,x2);
% solve for control u
du = diff(H,u);
sol_u = solve(du, 'u');
% Substitute u to state equations
Dx2 = subs(Dx2, u, sol_u);
% convert symbolic objects to strings for using 'dsolve'
eq1 = strcat('Dx1=',char(Dx1));
eq2 = strcat('Dx2=',char(Dx2));
eq3 = strcat('Dp1=',char(Dp1));
eq4 = strcat('Dp2=',char(Dp2));
sol_h = dsolve(eq1,eq2,eq3,eq4)
% case a: (a) x1(0)=x2(0)=0; x1(2) = 5; x2(2) = 2;
conA1 = 'x1(0) = 0';
conA2 = 'x2(0) = 0';
conA3 = 'x1(2) = 5';
conA4 = 'x2(2) = 2';
sol_a = dsolve(eq1,eq2,eq3,eq4,conA1,conA2,conA3,conA4);
AA=sol_a.x2;
%subs(AA,0)
m=0;
% for i=0:.1:2
% m=m+1
% xx2(m)=subs(AA,i)
% tt(m)=i;
%
% end
% % plot(tt,xx2);
% case b: (a) x1(0)=x2(0)=0; p1(2) = x1(2) - 5; p2(2) = x2(2) -2;
eq1b = char(subs(sol_h.x1,'t',0));
eq2b = char(subs(sol_h.x2,'t',0));
eq3b = strcat(char(subs(sol_h.p1,'t',2)),...
'=',char(subs(sol_h.x1,'t',2)),'-5');
eq4b = strcat(char(subs(sol_h.p2,'t',2)),...
'=',char(subs(sol_h.x2,'t',2)),'-2');
sol_b = solve(eq1b,eq2b,eq3b,eq4b);
sol_b.C2
