Gekko optimal control. How to create multiple termination objectives/conditions?

So I am simulating plane flight. The plane flies certain distance (pathx and pathy variables) and then the simulation stops, when certain pathx and pathy values are reached. The solver is trying to minimize the fuel consumption (m.Maximize(mass*tf*final), by maximizing the mass value. The solver controls the accelerator pedal position (Tcontr).

Right now the termination condition is defined like this:

For X axis:

m.Equation(x*final<=pathx)

and

m.Minimize(final*(x-pathx)**2)

And for Y axis:

m.Equation(y*final<=pathy)

and

m.Minimize(final*(y-pathy)**2)

And right now the simulation ends when the desired X value is achieved, while desired Y values is not being achieved.

How do I force the simulation to end when both desired (X and Y) values are achieved?

My code:

import numpy as np
import matplotlib.pyplot as plt
from gekko import GEKKO
import math
#Gekko model
m = GEKKO(remote=False)

#Time points
nt = 11
tm =  np.linspace(0,100,nt)
m.time = tm

# Variables
Ro = m.Var(value=1.1)#air density
g = m.Const(value=9.80665)
pressure = m.Var(value=101325)#
T = m.Var(value=281,lb=100)#temperature
T0 = m.Const(value=288)#temperature at see level
S = m.Const(value=122.6)
Cd = m.Const(value=0.1)#drag coef
Cl = m.Var(value=1)#lift couef
FuelFlow = m.Var()
D = m.Var(value=25000,lb=0)#drag
Thrmax = m.Const(value=200000)#maximum throttle
Thr = m.Var()#throttle
V = m.Var(value=100,lb=50,ub=240)#velocity
gamma = m.Var(value=0)# Flight-path angle
gammaa = gamma.value
Xi = m.Var(value=0)# Heading angle
Xii = Xi.value
x = m.Var(value=0,lb=0)#x position
y = m.Var(value=0,lb=0)#y position
h = m.Var(value=1000,lb=0)# height
mass = m.Var(value=60000,lb=10000)
pathx = m.Const(value=50000) #intended distance length
pathy = m.Const(value=50000)
L = m.Var(value=0.1)#lift

p = np.zeros(nt)
p[-1] = 1.0
final = m.Param(value=p)

m.options.MAX_ITER=10000 # iteration number

#Fixed Variable
tf = m.FV(value=1,lb=0.0001,ub=1000.0)#
tf.STATUS = 1

# Controlled parameters
Tcontr = m.MV(value=0.2,lb=0.1,ub=1)# solver controls throttle pedal position
Tcontr.STATUS = 1
Tcontr.DCOST = 0

#Mu = m.Var(value=0)
Mu = m.MV(value=0,lb=-1.5,ub=1.5)# solver controls bank angle 
Mu.STATUS = 1
Mu.DCOST = 0

Muu = Mu.value

# Equations
m.Equation(Thr==Tcontr*Thrmax)
m.Equation(FuelFlow==0.75882*(1+(V/2938.5)))
m.Equation(D==0.5*Ro*(V**2)*Cd*S)
m.Equation(mass.dt()==tf*(-Thr*(FuelFlow/60000)))#
m.Equation(V.dt()==tf*((Thr-D)/mass))#
m.Equation(x.dt()==tf*(V*(math.cos(gammaa.value))*(math.cos(Xii.value))))#
m.Equation(x*final<=pathx)
#pressure and density part
m.Equation(T==T0-(0.0065*h))
m.Equation(pressure==101325*(1-(0.0065*h)/T0)**((g*0.0289652)/(8.31446*0.0065)))#
m.Equation(Ro*(8.31446*T)==(pressure*0.0289652))
#2D addition part
m.Equation(L==0.5*Ro*(V**2)*Cl*S)
m.Equation(Xi.dt()==tf*((L*math.sin(Muu.value))/(mass*V)))
m.Equation(y.dt()==tf*(V*(math.cos(gammaa.value))*(math.sin(Xii.value))))#
m.Equation(y*final<=pathy)

# Objective Function
m.Minimize(final*(x-pathx)**2) #1D part
m.Minimize(final*(y-pathy)**2) #2D part
m.Maximize(mass*tf*final) #objective function
m.options.IMODE = 6
m.options.NODES = 2 # it was 3 before
m.options.MV_TYPE = 1
m.options.SOLVER = 3
#m.open_folder() # to search for infeasibilities
m.solve()

tm = tm * tf.value[0]
    
fig, axs = plt.subplots(8)
fig.suptitle('Results')
axs[0].plot(tm,Tcontr,'r-',LineWidth=2,label=r'$Tcontr$')
axs[0].legend(loc='best')
axs[1].plot(tm,V.value,'b-',LineWidth=2,label=r'$V$')
axs[1].legend(loc='best')
axs[2].plot(tm,x.value,'r--',LineWidth=2,label=r'$x$')
axs[2].legend(loc='best')
axs[3].plot(tm,D.value,'g-',LineWidth=2,label=r'$D$')
axs[3].legend(loc='best')
axs[4].plot(tm,mass.value,'g:',LineWidth=2,label=r'$mass$')
axs[4].legend(loc='best')
axs[5].plot(tm,T.value,'p-',LineWidth=2,label=r'$T$')
axs[5].legend(loc='best')
axs[6].plot(tm,Mu.value,'p-',LineWidth=2,label=r'$Mu$')
axs[6].legend(loc='best')
axs[7].plot(tm,y.value,'p-',LineWidth=2,label=r'$y$')
axs[7].legend(loc='best')
plt.xlabel('Time')
#plt.ylabel('Value')
plt.show()

Important Update

It looks like termination conditions (all of the above) work as intended. Y axis values can not be reached, because it looks like the math involving it is not working properly.

Y value is being calculated by this equation: I have turned into this: m.Equation(y.dt()==tf*(V*(math.cos(gammaa.value))*(math.sin(Xii.value))))

Where: V is true air speed (works properly);

tf is a variable that controls simulation time (works properly);

gammaa is derived from gamma, which is flight-path angle, which is 0 in this simulation (works properly);

Xii is derived from Xi, which is heading angle (the main culprit).

Xi value is defined by this equation:

I turned it into this: m.Equation(Xi.dt()==tf*((L*math.sin(Muu.value))/(mass*V)))

Where: L is lift force (works properly);

mass is mass (works properly);

V is true air speed (works properly);

Muu is derived from Mu, which is bank angle, the solver controlled variable (probably the bad apple here).

Y value calculation should be working like this: solver changes Mu, which affects The heading angle Xi, which then should affect Y value.

After some experiments it looks like the range of changes of the Mu value during the simulation is so small, that it barely affects Y value. But it should be a lot wider since Mu boundaries are quite wide (lb=-1.5,ub=1.5). I tried to remove those boundaries, but it did not affect the results of the simulation.

What could be messing up everything here?

Solution

Try adding a hard terminal constraint for both:

m.Equation((x-pathx)*final==0)
m.Equation((y-pathy)*final==0)

Alternatively, increase the weight on the terminal condition.

w = 1e3
m.Minimize(w*final*(x-pathx)**2) #1D part
m.Minimize(w*final*(y-pathy)**2) #2D part

You may need to use one or both strategies. Terminal conditions can be challenging to solve.

Response to Important Update

Use the Gekko functions with m.sin() and m.cos() instead of the math functions. Also, don't use .value in the equations. Gekko needs the full symbolic graph so that it can compile the equations into byte-code and produce function evaluations and the derivatives with automatic differentiation.

m.Equation(y.dt()==tf*(V*(m.cos(gammaa))*(m.sin(Xii))))

It also helps to avoid divide by zero by multiplying any denominator over to the other side.

m.Equation(mass*V*Xi.dt()==tf*((L*m.sin(Muu))))