def state_update(X, dt, mdl, eng):
nrow, ncol = X.shape
m, eta, x, y, z, vax, vay, vz, vwx, vwy, tau = X
vgx = vax + vwx
vgy = vay + vwy
vg = np.sqrt(vgx**2 + vgy**2)
va = np.sqrt(vax**2 + vay**2)
psi = np.arctan2(vax, vay)
gamma = np.arcsin(vz / va)
thrust = Thrust(mdl, eng)
T = thrust.climb(va / aero.kts, z / aero.ft, vz / aero.fpm)
drag = Drag(mdl)
D = drag.clean(m, va / aero.kts, z / aero.ft)
a = (eta * T - D) / m - aero.g0 * np.sin(gamma)
m1 = m
eta1 = eta
x1 = x + vgx * dt
y1 = y + vgy * dt
z1 = z + vz * dt
va1 = va + a * dt
vax1 = va1 * np.sin(psi)
vay1 = va1 * np.cos(psi)
vz1 = vz
vwx1 = vwx
vwy1 = vwy
tau1 = aero.temperature(z)
X = np.array([m1, eta1, x1, y1, z1, vax1, vay1, vz1, vwx1, vwy1, tau1])
return X
class CruiseOptimizer(object):
"""Optimizer for cursie mach number and altitude."""
def __init__(self, ac):
super(CruiseOptimizer, self).__init__()
self.ac = ac
self.aircraft = prop.aircraft(ac)
self.thrust = Thrust(ac)
self.fuelflow = FuelFlow(ac)
self.drag = Drag(ac)
# parameters to be optimized:
# Mach number, altitude
self.x0 = np.array([0.3, 25000 * aero.ft])
self.normfactor = calc_normfactor(self.x0)
self.bounds = None
self.update_bounds()
def update_bounds(self, **kwargs):
machmin = kwargs.get('machmin', 0.5)
machmax = kwargs.get('machmax', self.aircraft['limits']['MMO'])
hmin = kwargs.get('hmin', 25000 * aero.ft)
hmax = kwargs.get('hmax', self.aircraft['limits']['ceiling'])
self.bounds = np.array([[machmin, machmax], [hmin, hmax]
]) * self.normfactor.reshape(2, -1)
def func_fuel(self, x, mass):
mach, h = denormalize(x, self.normfactor)
va = aero.mach2tas(mach, h)
ff = self.fuelflow.enroute(mass, va / aero.kts, h / aero.ft)
ff_m = ff / (va + 1e-3) * 1000
# print("%.03f" % mach, "%d" % (h/aero.ft), "%.05f" % ff_m)
return ff_m
def func_time(self, x, mass):
mach, h = denormalize(x, self.normfactor)
va = aero.mach2tas(mach, h)
va_inv = 1 / (va + 1e-4) * 1000
# print("%.03f" % mach, "%d" % (h/aero.ft), "%.02f" % va)
return va_inv
def func_cons_lift(self, x, mass):
mach, h = denormalize(x, self.normfactor)
va = aero.mach2tas(mach, h)
Tmax = self.thrust.cruise(va / aero.kts, h / aero.ft)
qS = 0.5 * aero.density(h) * va**2 * self.aircraft['wing']['area']
cd0 = self.drag.polar['clean']['cd0']
k = self.drag.polar['clean']['k']
mach_crit = self.drag.polar['mach_crit']
if mach > mach_crit:
cd0 += 20 * (mach - mach_crit)**4
dL2 = qS**2 * (1 / k * (Tmax /
(qS + 1e-3) - cd0)) - (mass * aero.g0)**2
return dL2
def func_cons_thrust(self, x, mass):
mach, h = denormalize(x, self.normfactor)
va = aero.mach2tas(mach, h)
D = self.drag.clean(mass, va / aero.kts, h / aero.ft)
Tmax = self.thrust.cruise(va / aero.kts, h / aero.ft)
dT = Tmax - D
return dT
def optimize(self, goal, mass):
if goal == 'fuel':
func = self.func_fuel
elif goal == 'time':
func = self.func_time
else:
raise RuntimeError('Optimization goal [%s] not avaiable.' % goal)
x0 = self.x0 * self.normfactor
res = minimize(
func,
x0,
args=(mass, ),
bounds=self.bounds,
jac=lambda x, m: Jacobian(lambda x: func(x, m))(x),
options={'maxiter': 200},
constraints=(
{
'type':
'ineq',
'args': (mass, ),
'fun':
lambda x, m: self.func_cons_thrust(x, m),
'jac':
lambda x, m: Jacobian(lambda x: self.func_cons_thrust(
x, m))(x)
},
{
'type':
'ineq',
'args': (mass, ),
'fun':
lambda x, m: self.func_cons_lift(x, m),
'jac':
lambda x, m: Jacobian(lambda x: self.func_cons_lift(x, m))
(x)
},
))
return res
class FuelFlow(object):
"""Fuel flow model based on ICAO emmision databank."""
def __init__(self, ac, eng=None):
"""Initialize FuelFlow object.
Args:
ac (string): ICAO aircraft type (for example: A320).
eng (string): Engine type (for example: CFM56-5A3).
Leave empty to use the default engine specified
by in the aircraft database.
"""
self.aircraft = prop.aircraft(ac)
if eng is None:
eng = self.aircraft["engine"]["default"]
self.engine = prop.engine(eng)
self.thrust = Thrust(ac, eng)
self.drag = Drag(ac)
c3, c2, c1 = (
self.engine["fuel_c3"],
self.engine["fuel_c2"],
self.engine["fuel_c1"],
)
# print(c3,c2,c1)
self.fuel_flow_model = lambda x: c3 * x**3 + c2 * x**2 + c1 * x
@ndarrayconvert
def at_thrust(self, acthr, alt=0):
"""Compute the fuel flow at a given total thrust.
Args:
acthr (int or ndarray): The total net thrust of the aircraft (unit: N).
alt (int or ndarray): Aircraft altitude (unit: ft).
Returns:
float: Fuel flow (unit: kg/s).
"""
n_eng = self.aircraft["engine"]["number"]
engthr = acthr / n_eng
ratio = engthr / self.engine["max_thrust"]
ff_sl = self.fuel_flow_model(ratio)
ff_corr_alt = self.engine["fuel_ch"] * (engthr / 1000) * (alt *
aero.ft)
ff_eng = ff_sl + ff_corr_alt
fuelflow = ff_eng * n_eng
return fuelflow
@ndarrayconvert
def takeoff(self, tas, alt=None, throttle=1):
"""Compute the fuel flow at takeoff.
The net thrust is first estimated based on the maximum thrust model
and throttle setting. Then FuelFlow.at_thrust() is called to compted
the thrust.
Args:
tas (int or ndarray): Aircraft true airspeed (unit: kt).
alt (int or ndarray): Altitude of airport (unit: ft). Defaults to sea-level.
throttle (float or ndarray): The throttle setting, between 0 and 1.
Defaults to 1, which is at full thrust.
Returns:
float: Fuel flow (unit: kg/s).
"""
Tmax = self.thrust.takeoff(tas=tas, alt=alt)
fuelflow = throttle * self.at_thrust(Tmax)
return fuelflow
@ndarrayconvert
def enroute(self, mass, tas, alt, path_angle=0):
"""Compute the fuel flow during climb, cruise, or descent.
The net thrust is first estimated based on the dynamic equation.
Then FuelFlow.at_thrust() is called to compted the thrust. Assuming
no flap deflection and no landing gear extended.
Args:
mass (int or ndarray): Aircraft mass (unit: kg).
tas (int or ndarray): Aircraft true airspeed (unit: kt).
alt (int or ndarray): Aircraft altitude (unit: ft).
path_angle (float or ndarray): Flight path angle (unit: degrees).
Returns:
float: Fuel flow (unit: kg/s).
"""
D = self.drag.clean(mass=mass, tas=tas, alt=alt, path_angle=path_angle)
# Convert angles from degrees to radians.
gamma = np.radians(path_angle)
T = D + mass * aero.g0 * np.sin(gamma)
T_idle = self.thrust.descent_idle(tas=tas, alt=alt)
T = np.where(T < 0, T_idle, T)
fuelflow = self.at_thrust(T, alt)
# do not return value outside performance boundary, with a margin of 20%
T_max = self.thrust.climb(tas=0, alt=alt, roc=0)
fuelflow = np.where(T > 1.20 * T_max, np.nan, fuelflow)
return fuelflow
def plot_model(self, plot=True):
"""Plot the engine fuel model, or return the pyplot object.
Args:
plot (bool): Display the plot or return an object.
Returns:
None or pyplot object.
"""
import matplotlib.pyplot as plt
xx = np.linspace(0, 1, 50)
yy = self.fuel_flow_model(xx)
# plt.scatter(self.x, self.y, color='k')
plt.plot(xx, yy, "--", color="gray")
if plot:
plt.show()
else:
return plt
class SIR():
def __init__(self, **kwargs):
self.ac = kwargs.get('ac')
self.eng = kwargs.get('eng')
self.time = kwargs.get('time')
self.Y = kwargs.get('obs')
# slightly increase the cov matrix (factor of 0.2), for better convergency
self.noise = kwargs.get('noise')
self.stdn = stdns[self.noise] * 1.2
self.R = np.zeros((nY, nY))
np.fill_diagonal(self.R, self.stdn**2)
self.thrust = Thrust(self.ac, self.eng)
self.drag = Drag(self.ac)
aircraft = prop.aircraft(self.ac)
self.mmin = aircraft['limits']['OEW']
self.mmax = aircraft['limits']['MTOW']
self.mrange = (self.mmin, self.mmax)
self.eta_min = kwargs.get('eta_min', 0.80)
self.eta_max = kwargs.get('eta_max', 1)
self.kstd_m = kpm * (self.mmax - self.mmin)
self.kstd_eta = kpe * (1 - self.eta_min)
self.X = None
self.nP = None
self.now = 0
self.neff = None
self.X_true = None
logfn = kwargs.get('logfn', None)
self.xlabels = [
'm (kg)', '$\eta$ (-)', 'x (m)', 'y (m)', 'z (m)',
'$v_{ax}$ (m/s)', '$v_{ay}$ (m/s)', '$v_z$ (m/s)',
'$v_{wx}$ (m/s)', '$v_{wy}$ (m/s)', '$\\tau$ (K)'
]
self.ylabels = [
'x', 'y', 'z', '$v_{gx}$', '$v_{gy}$', '$v_z$', '$v_{wx}$',
'$v_{wy}$', '$\\tau$'
]
if (logfn is not None):
if ('.log' not in logfn):
raise RuntimeError('Log file must end with .log')
self.log = root + '/smclog/' + logfn
print('writing to log:', self.log)
header = ['time'] + self.ylabels \
+ [l+" avg" for l in self.xlabels] \
+ [l+" med" for l in self.xlabels] \
+ [l+" min" for l in self.xlabels] \
+ [l+" max" for l in self.xlabels]
with open(self.log, 'wt') as fcsv:
writer = csv.writer(fcsv, delimiter=',')
writer.writerow(header)
else:
self.log = None
def state_update(self, X, dt):
nrow, ncol = X.shape
m, eta, x, y, z, vax, vay, vz, vwx, vwy, tau = np.split(
X, X.shape[1], 1)
vgx = vax + vwx
vgy = vay + vwy
vg = np.sqrt(vgx**2 + vgy**2)
va = np.sqrt(vax**2 + vay**2)
psi = np.arctan2(vax, vay)
gamma = np.arcsin(vz / va)
T = self.thrust.climb(va / aero.kts, z / aero.ft, vz / aero.fpm)
D = self.drag.clean(m, va / aero.kts, z / aero.ft)
a = (eta * T - D) / m - aero.g0 * np.sin(gamma)
print(np.mean(a))
m1 = m
eta1 = eta
x1 = x + vgx * dt
y1 = y + vgy * dt
z1 = z + vz * dt
va1 = va + a * dt
# va1 = va + a * np.cos(gamma) * dt
vax1 = va1 * np.sin(psi)
vay1 = va1 * np.cos(psi)
evz = np.random.normal(0, sigma_vz, nrow)
vz1 = alpha_vz * vz + evz.reshape(-1, 1) * dt
# vz1 = alpha_vz * vz + a * np.sin(gamma) * dt + evz.reshape(-1, 1) * dt
evwx = np.random.normal(0, sigma_vwx, nrow)
vwx1 = alpha_vwx * vwx + evwx.reshape(-1, 1) * dt
evwy = np.random.normal(0, sigma_vwy, nrow)
vwy1 = alpha_vwy * vwy + evwy.reshape(-1, 1) * dt
etau = np.random.normal(0, sigma_tau, nrow)
tau1 = alpha_tau * tau + etau.reshape(-1, 1) * dt
X = np.hstack(
[m1, eta1, x1, y1, z1, vax1, vay1, vz1, vwx1, vwy1, tau1])
return X
def logsm(self):
if self.log is None:
return
t = self.now
if t in self.time:
idx = list(self.time).index(t)
measurement = self.Y[:, idx]
else:
measurement = np.ones(self.Y.shape[0]) * np.nan
state_avgs = np.average(self.X, weights=self.W.T, axis=0)
state_meds = np.median(self.X, axis=0)
# state_mins = np.percentile(self.X, 2.5, axis=0)
# state_maxs = np.percentile(self.X, 95.5, axis=0)
state_mins = np.min(self.X, axis=0)
state_maxs = np.max(self.X, axis=0)
row = np.hstack([[t], measurement, state_avgs, state_meds, state_mins,
state_maxs])
with open(self.log, 'at') as fcsv:
writer = csv.writer(fcsv, delimiter=',')
writer.writerow(row)
def compute_neff(self):
self.neff = 1 / np.sum(np.square(self.W))
def printstates(self, t):
# ---- Debug ----
obsv = Y2X(self.Y[:, t])
obsv[0:2] = ['*****', '****']
avgs = np.average(self.X, weights=self.W, axis=0)
meds = np.median(self.X, axis=0)
# mins = np.percentile(self.X, 2.5, axis=0)
# maxs = np.percentile(self.X, 95.5, axis=0)
mins = np.min(self.X, axis=0)
maxs = np.max(self.X, axis=0)
printarray(obsv, 'obsv')
printarray(avgs, 'avgs')
# printarray(meds, 'meds')
printarray(mins, 'mins')
printarray(maxs, 'maxs')
def pickle_particles(self, fname):
import os, pickle
root = os.path.dirname(os.path.realpath(__file__))
fpkl = open(root + '/data/' + fname, 'wb')
pickle.dump({'X': self.X, 'W': self.W}, fpkl)
def init_particles(self, at=0, n_particles=50000):
Mu0 = Y2X(self.Y[:, at])
Mu0[0:2] = [0, 0]
Var0 = np.zeros((nX, nX))
np.fill_diagonal(Var0, (np.append([0, 0], self.stdn))**2)
printarray(Mu0, 'Mu0')
printarray(np.diag(Var0), 'Var0')
self.X = np.random.multivariate_normal(Mu0, Var0, n_particles)
self.nP = n_particles
m_inits = np.random.uniform(self.mmin, self.mmax, n_particles)
# mass-related initialization (recommended)
er_mins = 1 - (1 - self.eta_min) * (self.mmax - m_inits) / (self.mmax -
self.mmin)
eta_inits = np.random.uniform(er_mins, self.eta_max, n_particles)
# # Uniform initialization
# eta_inits = np.random.uniform(self.eta_min, self.eta_max, n_particles)
self.X[:, 0] = m_inits
self.X[:, 1] = eta_inits
self.W = np.ones(n_particles) / (n_particles)
return
def resample(self):
"""
References: J. S. Liu and R. Chen. Sequential Monte Carlo methods for dynamic
systems. Journal of the American Statistical Association,
93(443):1032–1044, 1998.
"""
N = self.nP
W = self.W
idx = np.zeros(N, 'i')
# take int(N*w) copies of each weight, which ensures particles with the
# same weight are drawn uniformly
copyn = (np.floor(N * W)).astype(int)
a = np.where(copyn > 0)[0]
b = copyn[a]
c = np.append(0, np.cumsum(b))
for i, (i0, i1) in enumerate(zip(c[:-1], c[1:])):
idx[i0:i1] = a[i]
# use multinormal resample on the residual to fill up the rest. This
# maximizes the variance of the samples
k = c[-1]
residual = W - copyn
residual /= sum(residual)
cumulative_sum = np.cumsum(residual)
cumulative_sum[
-1] = 1. # avoid round-off errors: ensures sum is exactly one
idx[k:N] = np.searchsorted(cumulative_sum, np.random.random(N - k))
return idx
def run(self, processdt=1, use_bada=True):
if self.X is None:
raise RuntimeError(
'Particles not initialized. Run SIR.init_particles() first.')
self.now = self.time[0]
self.logsm()
for it, tm in enumerate(self.time):
print('-' * 100)
self.now = tm
# ===== SIR update =====
print("SIR / measurement update, time", self.now)
self.printstates(it)
# ---- weight update ----
Y_true = X2Y(self.X)
Yt = self.Y[:, it]
Y = np.ones(Y_true.shape) * Yt
DY = Y - Y_true
Simga_inv = np.linalg.inv(self.R)
self.W *= np.exp(-0.5 *
np.einsum('ij,ij->i', np.dot(DY, Simga_inv), DY))
self.W = np.where(self.X[:, 1] < self.eta_min, 0, self.W)
self.W = np.where(self.X[:, 1] > self.eta_max, 0, self.W)
self.W = np.where(self.X[:, 0] < self.mmin, 0, self.W)
self.W = np.where(self.X[:, 0] > self.mmax, 0, self.W)
self.W = self.W / np.sum(self.W) # normalize
# ---- resample ----
idx = self.resample()
self.X = self.X[idx, :]
self.W = np.ones(self.nP) / self.nP
if tm == self.time[-1]:
break
# ---- apply kernel ----
print((bcolors.OKGREEN + "Kernel applied: [m:%d, eta:%f]" +
bcolors.ENDC) % (self.kstd_m, self.kstd_eta))
self.X[:, 0] = self.X[:, 0] + np.random.normal(
0, self.kstd_m, self.nP)
self.X[:, 1] = self.X[:, 1] + np.random.normal(
0, self.kstd_eta, self.nP)
epsi = np.random.normal(0, kpsi, self.nP)
va = np.sqrt(self.X[:, 5]**2 + self.X[:, 6]**2)
psi = np.arctan2(self.X[:, 5], self.X[:, 6])
self.X[:, 5] = va * np.sin(psi + epsi)
self.X[:, 6] = va * np.cos(psi + epsi)
# ===== states evolution =====
dtm = self.time[it + 1] - self.time[it]
dtp = min(processdt, dtm) # process update time interval
for j in range(int(dtm / dtp)): # number of process update
print("process update / state evolution, time", self.now)
self.now = tm + (j + 1) * dtp
self.X = self.state_update(self.X, dtp)
self.logsm()
return
class FuelFlow(object):
"""Fuel flow model based on ICAO emmision databank."""
def __init__(self, ac, eng=None):
"""Initialize FuelFlow object.
Args:
ac (string): ICAO aircraft type (for example: A320).
eng (string): Engine type (for example: CFM56-5A3).
Leave empty to use the default engine specified
by in the aircraft database.
"""
self.aircraft = prop.aircraft(ac)
if eng is None:
eng = self.aircraft['engine']['default']
self.engine = prop.engine(eng)
self.thrust = Thrust(ac, eng)
self.drag = Drag(ac)
c3, c2, c1 = self.engine['fuel_c3'], self.engine[
'fuel_c2'], self.engine['fuel_c1']
# print(c3,c2,c1)
self.fuel_flow_model = lambda x: c3 * x**3 + c2 * x**2 + c1 * x
@ndarrayconvert
def at_thrust(self, acthr, alt=0):
"""Compute the fuel flow at a given total thrust.
Args:
acthr (int or ndarray): The total net thrust of the aircraft (unit: N).
alt (int or ndarray): Aicraft altitude (unit: ft).
Returns:
float: Fuel flow (unit: kg/s).
"""
ratio = acthr / (self.engine['max_thrust'] *
self.aircraft['engine']['number'])
fuelflow = self.fuel_flow_model(ratio) * self.aircraft['engine']['number'] \
+ self.engine['fuel_ch'] * (alt*aero.ft) * (acthr/1000)
return fuelflow
@ndarrayconvert
def takeoff(self, tas, alt=None, throttle=1):
"""Compute the fuel flow at takeoff.
The net thrust is first estimated based on the maximum thrust model
and throttle setting. Then FuelFlow.at_thrust() is called to compted
the thrust.
Args:
tas (int or ndarray): Aircraft true airspeed (unit: kt).
alt (int or ndarray): Altitude of airport (unit: ft). Defaults to sea-level.
throttle (float or ndarray): The throttle setting, between 0 and 1.
Defaults to 1, which is at full thrust.
Returns:
float: Fuel flow (unit: kg/s).
"""
Tmax = self.thrust.takeoff(tas=tas, alt=alt)
fuelflow = throttle * self.at_thrust(Tmax)
return fuelflow
@ndarrayconvert
def enroute(self, mass, tas, alt, path_angle=0):
"""Compute the fuel flow during climb, cruise, or descent.
The net thrust is first estimated based on the dynamic equation.
Then FuelFlow.at_thrust() is called to compted the thrust. Assuming
no flap deflection and no landing gear extended.
Args:
mass (int or ndarray): Aircraft mass (unit: kg).
tas (int or ndarray): Aircraft true airspeed (unit: kt).
alt (int or ndarray): Aircraft altitude (unit: ft).
path_angle (float or ndarray): Flight path angle (unit: ft).
Returns:
float: Fuel flow (unit: kg/s).
"""
D = self.drag.clean(mass=mass, tas=tas, alt=alt, path_angle=path_angle)
gamma = np.radians(path_angle)
T = D + mass * aero.g0 * np.sin(gamma)
T_idle = self.thrust.descent_idle(tas=tas, alt=alt)
T = np.where(T < 0, T_idle, T)
fuelflow = self.at_thrust(T, alt)
return fuelflow
def plot_model(self, plot=True):
"""Plot the engine fuel model, or return the pyplot object.
Args:
plot (bool): Display the plot or return an object.
Returns:
None or pyplot object.
"""
import matplotlib.pyplot as plt
xx = np.linspace(0, 1, 50)
yy = self.fuel_flow_model(xx)
# plt.scatter(self.x, self.y, color='k')
plt.plot(xx, yy, '--', color='gray')
if plot:
plt.show()
else:
return plt
from openap import Drag
drag = Drag(ac='A320')
print('-'*70)
D = drag.clean(mass=60000, tas=200, alt=20000)
print("drag.clean(mass=60000, tas=200, alt=20000)")
print(D)
print('-'*70)
D = drag.clean(mass=60000, tas=250, alt=20000)
print("drag.clean(mass=60000, tas=250, alt=20000)")
print(D)
print('-'*70)
D = drag.nonclean(mass=60000, tas=150, alt=1000, flap_angle=20, path_angle=10, landing_gear=False)
print("drag.nonclean(mass=60000, tas=150, alt=1000, flap_angle=20, path_angle=10, landing_gear=False)")
print(D)
print('-'*70)
D = drag.nonclean(mass=60000, tas=150, alt=200, flap_angle=20, path_angle=10, landing_gear=True)
print("drag.nonclean(mass=60000, tas=150, alt=1000, flap_angle=20, path_angle=10, landing_gear=True)")
print(D)
print('-'*70)
D = drag.clean(mass=[60000], tas=[200], alt=[20000])
print("drag.clean(mass=[60000], tas=[200], alt=[20000])")
print(D)
print('-'*70)