Using ode45 + shooting method to solve constrained optimal control problem

Hello. I am Giap, and I am looking for help on using ode45 + shooting method to solve a constrained optimal control problem.

The problem is quite simple as below.

% dx1 = x2; dx2 = u; t0 = 0; tf = 1;
% J = integral of 0.5*(x1^2+u^2) from t0 to tf
% inital condition x(0) = [4;1]
% constraint: 0.6 <= x2 <= 1.5

This problem can be solve by using the Various of calculation approach + Heaviside step function with bvp4c, but I want to practise on using ode45 + shooting method.

By introducing a new state x3 and costate p3 to the system, now I have the modified state:

% x = [x1; x2; x3; p1; p2; p3];

My purpose of using shooting method is to find a good initial values of p1, p2, p3 for this problem.

The costate p3 has its deravative dp3 = 0, so it is a constant over the specified period.

The problem that I ran into was that ode45 requires initial values to process, so I need to find a way to properly initialize p3.

If I choose a constant for p3, since its derivative is 0, it mostly will not change its value the entire time.

If I choose a vector so that p3 is in a range like p3 = -1e4 : 1e4, it will not work.

So I am grateful to receive any comments or opinions with this problem.

%% With constraints + shooting method
% dx2 = u;
% J = 0.5*(x1^2+u^2) from t0 to tf
% t0 = 0; tf = 1;
% x(0) =[4;1]
% 0.6 <= x2 <= 1.5;

clear; clf
global x2l x2u
x2l = 0.6; x2u = 1.5;
for p0 = -1e4:1e4
    x0 = [0 0.5 0.5 p0];
    xopt = fsolve(@solver,x0);
end
function F = solver(xt)
options = odeset('Stats','on','RelTol',1e-4,'AbsTol',1e-4);

[tt,yt] = ode45(@odefcn,[0 1],[4 1 xt(1) xt(2) xt(3) xt(4)],options);

tt = tt';
n = length(tt);
yt = yt';

F = yt(3:5,n)-[0;0;0];

ut = -yt(5,:);

J = 1*0.5*(sum(yt(1,:).^2)+sum(ut.^2))/n;
% J = 1*0.5*(xt(1,:)*xt(1,:)' + ut*ut')/n;
%% Plots
subplot(3,1,1)
plot(tt,yt(1:3,:),'-');
hold on
plot(tt,ut', 'r:');
s = strcat('Final cost is: J=',num2str(J));
text(0.6,2,s);
legend('x1','x2','x3','u')
grid
hold off

subplot(3,1,2)
plot(tt,yt(4:5,:),'-');
legend('p1','p2')
grid

subplot(3,1,3)
plot(tt,yt(6,:),'-')
legend('p3')
grid
end

%% system + boundary conditions
function dxdt =  odefcn(t,x)
global x2l x2u
u = -x(5);
dxdt = [x(2)
        u
        (x(2)-x2l)^2*HvSfcn(x2l-x(2))+(x2u-x(2))^2*HvSfcn(x(2)-x2u)
        -x(1)
        -x(4)-2*x(6)*(x(2)-x2l)*HvSfcn(x2l-x(2))+2*x(6)*(x2u-x(2))*HvSfcn(x(2)-x2u)
        0];
end

% function res = bcfcn(xa,xb)
% res = [xa(1)-4
%        xa(2)-1
%        xa(3)-0
%        xb(3)-0
%        xb(4)-0
%        xb(5)-0];
% end

function HvS = HvSfcn(f)
if (-f) >= 0
    HvS = 0;
else
    HvS = 1;
end
end

[Duc Giap Nguyen]

I have been able to solve the problem myself.

Still, I am happy to receive any suggestion, comment, or opinion on this problem as well as on the Multiple shooting method applied for this case.