This is a standard optimal control problem, I am using the shooting method, I solved the hjb equation and used the co-state values for initialization and it is working perfectly, but I want to use newton method for solving initial value, where final values are given (indirect shooting(x1,x2 final known)).
from pyparsing.helpers import ZeroOrMore
import numpy as np
import math
from scipy.integrate import odeint
import matplotlib.pyplot as plt
from scipy.optimize import fsolve
from scipy.integrate import solve_ivp
def eqn11(z,t,phi):
lamda1=z[0]
lamda2=z[1]
y1=z[2]
y2=z[3]
dlamda1=0#costate equation
dlamda2=0#state equation
dy1=math.cos(phi)
dy2=math.sin(phi)
dzdt=[dlamda1,dlamda2,dy1,dy2]
return dzdt
def eqn22(z,t,phi):
lamda1=z[0]
lamda2=z[1]
y1=z[2]
y2=z[3]
y22=(y2-1)**2
dlamda1=0 #costate-function
dlamda2=-(lamda1*2*(y2-1)*math.cos(phi))-(lamda2*2*(y2-1)*math.sin(phi))#state function
dy1=(y22+1)*math.cos(phi)
dy2=(y22+1)*math.sin(phi)
dzdt=[dlamda1,dlamda2,dy1,dy2]
return dzdt
def cont(l1,l2):
u1=math.atan2(l2,l1)
#atan2 gives values between -pi to +pi
return u1
n=550
lamda0=[0.05,1]
y0=[-2.5,0]
t=np.linspace(0,5,n)
lamda11=np.empty_like(t)
lamda21=np.empty_like(t)
y11=np.empty_like(t)
y21=np.empty_like(t)
u=np.empty_like(t)
# record initial conditions
lamda11[0] = lamda0[0]
lamda21[0] = lamda0[1]
y11[0]=y0[0]
y21[0]=y0[1]
y_0=[lamda0[0],lamda0[1],y0[0],y0[1]]
u0=cont(lamda0[0],lamda0[1])
u[0]=cont(lamda0[0],lamda0[1])
for i in range(1,n):
#print(i)
tspan = [t[i-1],t[i]]
if i>550:
break
else:
if y_0[3]<1:
z=odeint(eqn11,y_0,tspan,args=(u[i-1],))
lamda11[i]=z[1][0]
lamda21[i]=z[1][1]
y11[i]=z[1][2]
y21[i]=z[1][3]
u[i]=cont(lamda11[i],lamda21[i])#control
y_0[0]=z[1][0]
y_0[1]=z[1][1]
y_0[2]=z[1][2]
y_0[3]=z[1][3]
#print(y_0[3])
else:
z=odeint(eqn22,y_0,tspan,args=(u[i-1],))
lamda11[i]=z[1][0]
lamda21[i]=z[1][1]
y11[i]=z[1][2]
y21[i]=z[1][3]
u[i]=cont(lamda11[i],lamda21[i])
y_0[0]=z[1][0]
y_0[1]=z[1][1]
y_0[2]=z[1][2]
y_0[3]=z[1][3]
#print(lamda0)
#print(z1)
#print(z)
#print(lamda0)
#u0=u[i]
#plt.plot(t[0:440],lamda11[0:440])
#plt.plot(t,u)
plt.plot(t[0:540],y21[0:540])
#plt.plot(t[0:440],y11[0:440])
#plt.plot(t[0:440],lamda21[0:440])
#plt.plot(y11[0:540],y21[0:540])
#print(max(y2))
#print(y1)
#print(y11)
#print(lamda1)
#print(min(lamda2))
print(y11[537])
If anybody have any suggestion, please weigh in
