def plot(time,
X,
U,
Yc=None,
P=None,
window_title="trajectory",
figure=None,
axs=None,
label=None):
#print('in plot {} {}'.format(figure, axs))
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = figure or p3_pu.prepare_fig(figure,
window_title,
figsize=(0.75 * 20.48, 0.75 * 10.24),
margins=margins)
plots = [("x", "m", X[:, pmip.s_x]),
("$\\theta$", "deg", np.rad2deg(X[:, pmip.s_theta])),
("$\\tau$", "N.m", U[:, pmip.iv_t])]
axs = axs if axs is not None else figure.subplots(3, 1)
#pdb.set_trace()
for ax, (title, ylab, data) in zip(axs, plots):
ax.plot(time, data, linewidth=2, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab)
if Yc is not None:
axs[0].plot(time, Yc, 'g', label='Setpoint')
if P is not None and P.tsat != float('inf'):
axs[2].plot(time, P.tsat * np.ones_like(time), 'k', label='sat')
axs[2].plot(time, -P.tsat * np.ones_like(time), 'k', label='-sat')
return figure, axs
def fit_va_and_phi(atm, dm, compute=False):
savefile_name = '/home/poine/tmp/pat_glider_trims.npz'
h = 0.
if compute:
phis = np.deg2rad(np.arange(-45, 46, 1.))
vas = np.arange(6, 20, 1.)
vzs = np.zeros((len(vas), len(phis)))
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
Xe, Ue = dm.foo_trim_aero_banked_cst_throttle(h=h, va=va, throttle=0., flaps=0., phi=phi, debug=True)
vzs[iva, iphi] = Xe[p3_fr.SixDOFEuclidianEuler.sv_zd]
np.savez(savefile_name, phis=phis, vas=vas, vzs=vzs)
print('saved {}'.format(savefile_name))
else:
_data = np.load(savefile_name)
phis, vas, vzs = [_data[k] for k in ['phis', 'vas', 'vzs']]
# fit
rho = p3_atm.get_rho(h)
K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref
H,Y = [],[]
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
CL = K/va**2
#H = np.array([[va**3/K, K/va] for va in vas])
H.append([va/CL, va*CL/np.cos(phi)**2])
Y.append(vzs[iva, iphi])
H=np.array(H)
Y=np.array(Y)
CD0, B = np.dot(np.linalg.pinv(H), Y)
#pdb.set_trace()
print('K {} CD0 {} B {}'.format(K, CD0, B))
plt.subplot(1,2,1)
for iva, va in enumerate(vas):
plt.plot(np.rad2deg(phis), vzs[iva,:], label='va {}'.format(va))
p3_pu.decorate(plt.gca(), title='zd', xlab='phi (deg)', ylab='m/s', legend=True)
plt.subplot(1,2,2)
for iphi, phi in enumerate(phis):
plt.plot(vas, vzs[:,iphi], label='phi {}'.format(np.rad2deg(phi)))
p3_pu.decorate(plt.gca(), title='zd', xlab='va (m/s)', ylab='m/s', legend=True)
vzs2 = np.zeros((len(vas), len(phis)))
for iphi, phi in enumerate(phis):
for iva, va in enumerate(vas):
CL = K/va**2
vzs2[iva, iphi] = va*(CD0/CL+B*CL/np.cos(phi)**2)
fig = plt.figure()
ax = fig.gca(projection='3d')
_phis, _vas = np.meshgrid(phis, vas)
surf = ax.plot_surface(_vas, np.rad2deg(phis), vzs)
surf2 = ax.plot_surface(_vas, np.rad2deg(phis), vzs2)
ax.set_xlabel('va (m/s)')
ax.set_ylabel('phi (deg)')
ax.set_zlabel('vz (m/s)')
plt.show()
def plot2(time,
X,
U,
Yc=None,
P=None,
window_title="trajectory",
figure=None,
axs=None):
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = figure or p3_pu.prepare_fig(figure,
window_title,
figsize=(0.75 * 20.48, 0.75 * 10.24),
margins=margins)
plots = [("$x$", "m", X[:, pmip.s_x]),
("$\\theta$", "deg", np.rad2deg(X[:, pmip.s_theta])),
("$\dot{x}$", "m/s", X[:, pmip.s_xd]),
("$\dot{\\theta}$", "deg/s", np.rad2deg(X[:, pmip.s_thetad])),
("$\\tau$", "N.m", U[:, pmip.iv_t])]
axs = axs if axs is not None else figure.subplots(3, 2).flatten()
for ax, (title, ylab, data) in zip(axs, plots):
ax.plot(time, data, linewidth=2)
p3_pu.decorate(ax, title=title, ylab=ylab)
if Yc is not None:
axs[0].plot(time, Yc, 'g', label='Setpoint')
return figure
def plot_trajectory(cls,
time,
X,
U,
figure=None,
window_title="Trajectory be",
legend=None,
label='',
filename=None,
atm=None):
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
plots = [("x", "m", X[:, cls.sv_x]), ("y", "m", X[:, cls.sv_y]),
("z", "m", X[:, cls.sv_z]), ("$u$", "m/s", X[:, cls.sv_u]),
("$v$", "m/s", X[:, cls.sv_v]), ("$w$", "m/s", X[:,
cls.sv_w]),
("$\phi$", "deg", np.rad2deg(X[:, cls.sv_phi])),
("$\\theta$", "deg", np.rad2deg(X[:, cls.sv_theta])),
("$\\psi$", "deg", np.rad2deg(X[:, cls.sv_psi])),
("$p$", "deg/s", np.rad2deg(X[:, cls.sv_p])),
("$q$", "deg/s", np.rad2deg(X[:, cls.sv_q])),
("$r$", "deg/s", np.rad2deg(X[:, cls.sv_r]))]
nrow = 5 if U is not None else 4
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(nrow, 3, i + 1)
plt.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab, legend=legend)
for i, min_yspan in enumerate(
[.5, .5, .5, .1, .1, 1., 1., 1., 1., 1., 1., 1.]):
p3_pu.ensure_yspan(plt.subplot(nrow, 3, i + 1), min_yspan)
return figure
def plot_polar(dm):
alpha1, alpha2 = np.deg2rad([-1., 15])
beta, rvel, Usfc = 0, [0, 0,0], [0, 0, 0, 0]
alphas = np.linspace(alpha1, alpha2, 100)
ACoefs = np.array([p1_fw_dyn.get_f_aero_coef(alpha, beta, rvel, Usfc, dm.P) for alpha in alphas])
CLs, CDs = ACoefs[:,0], ACoefs[:,2]
#plt.plot(alphas, CLs)
LovDs = CLs/CDs; i_max = np.argmax(LovDs)
print('max L over D {:.1f} (alpha {:.2f} deg)'.format(LovDs[i_max], np.rad2deg(alphas[i_max])))
plt.plot(CDs, CLs)
p3_pu.decorate(plt.gca(), xlab='CD', ylab='CL')
plt.plot
def plot_input(time, U, figure):
Fb, Mb = U[:, :3], U[:, 3:]
ax = figure.add_subplot(5, 3, 13)
plt.plot(time, Fb[:, 0], label='Fx')
plt.plot(time, Fb[:, 1], label='Fy')
plt.plot(time, Fb[:, 2], label='Fz')
p3_pu.decorate(ax, title="$F_b$", ylab="N", min_yspan=0.001, legend=True)
ax = figure.add_subplot(5, 3, 14)
plt.plot(time, Mb[:, 0], label='Mx')
plt.plot(time, Mb[:, 1], label='My')
plt.plot(time, Mb[:, 2], label='Mz')
p3_pu.decorate(ax, title="$M_b$", ylab="Nm", min_yspan=0.001, legend=True)
def main():
# Read aircraft trajectory and control variables from a file
ctl_logger = p3_guid.GuidancePurePursuitLogger()
time, X, U = ctl_logger.load('/tmp/pat_glider_ds.npz')
# This is needed :(
atm = p3_atm.AtmosphereShearX(wind1=7.0,
wind2=-1.0,
xlayer=60.0,
zlayer=40.0)
# Convert aircraft trajectory to euclidian/euler state vector
Xee = np.array([
p3_fr.SixDOFAeroEuler.to_six_dof_euclidian_euler(_X, atm, _t)
for _X, _t in zip(X, time)
])
# Compute energy
inertial_vel_ned = Xee[:, p3_fr.SixDOFEuclidianEuler.sv_slice_vel]
inertial_vel_norm = np.linalg.norm(inertial_vel_ned, axis=1)
mass, g = 1.7, 9.81 # we can get that from the cularis dynamic model if needed, as well as inertia
kinetic_energy = 0.5 * mass * inertial_vel_norm**2
pos_ned = Xee[:, p3_fr.SixDOFEuclidianEuler.sv_slice_pos]
potential_energy = mass * g * -Xee[:, p3_fr.SixDOFEuclidianEuler.sv_z]
#pdb.set_trace()
# Plot aircraft trajectory
#p1_fw_dyn.plot_trajectory_ae(time, X, U, window_title='chronogram', atm=atm)
_ctl = None # not needed
ctl_logger.plot_chronograms(time, X, U, _ctl, atm)
ctl_logger.plot3D(time, X, _ctl, atm)
ctl_logger.plot_slice_nu(time,
X,
U,
_ctl,
atm,
ref_traj=None,
n0=0,
n1=210,
dn=10,
h0=0,
h1=80,
dh=10)
# Plot energy
plt.figure()
plt.plot(time, kinetic_energy, label='kinetic energy')
plt.plot(time, potential_energy, label='potential energy')
plt.plot(time, potential_energy + kinetic_energy, label='total energy')
p3_pu.decorate(plt.gca(),
title='Energy',
xlab='time in s',
ylab='E in joules',
legend=True)
plt.show()
def test_alpha(P):
# alpha variation (de, rvel = 0)
alpha, beta, rvel = 0., 0., [0., 0., 0.]
Usfc = [0, 0, 0, 0]
alphas = np.deg2rad(np.linspace(-2, 15, 100))
coefs = np.array(
[get_both_coefs(P, alpha, beta, rvel, Usfc) for alpha in alphas])
ax = plt.subplot(3, 2, 1)
plt.plot(np.rad2deg(alphas), coefs[:, 0, 0], label='CL avl')
plt.plot(np.rad2deg(alphas), coefs[:, 1, 0], label='CL XFLR5')
p3_pu.decorate(ax, title='CL', xlab='alpha (deg)', ylab=None, legend=True)
ax = plt.subplot(3, 2, 2)
plt.plot(np.rad2deg(alphas), coefs[:, 0, 2], label='CD avl')
plt.plot(np.rad2deg(alphas), coefs[:, 1, 2], label='CD XFLR5')
p3_pu.decorate(ax, title='CD', xlab='alpha (deg)', ylab=None, legend=True)
# elevator variation (alpha, rvel = 0)
deles = np.deg2rad(np.linspace(-15, 15, 100))
Usfcs = np.zeros((len(deles), 4))
Usfcs[:, 1] = deles
coefs = np.array(
[get_both_coefs(P, alpha, beta, rvel, Usfc) for Usfc in Usfcs])
ax = plt.subplot(3, 2, 3)
plt.plot(np.rad2deg(deles), coefs[:, 0, 0], label='CL avl')
plt.plot(np.rad2deg(deles), coefs[:, 1, 0], label='CL XFLR5')
p3_pu.decorate(ax, title='CL', xlab='d_ele (deg)', ylab=None, legend=True)
ax = plt.subplot(3, 2, 4)
plt.plot(np.rad2deg(deles), coefs[:, 0, 2], label='CD avl')
plt.plot(np.rad2deg(deles), coefs[:, 1, 2], label='CD XFLR5')
p3_pu.decorate(ax, title='CD', xlab='d_ele (deg)', ylab=None, legend=True)
# pdb.set_trace()
plt.show()
def plot_chronograms(self,
time,
X,
_ctl,
atm,
title=None,
window_title=None,
fig=None,
axes=None):
_s = p3_fr.SixDOFEuclidianEuler
fig = plt.figure(tight_layout=True, figsize=[6., 4.8
]) if fig is None else fig
if window_title is not None: fig.canvas.set_window_title(window_title)
axes = fig.subplots(4, 1) if axes is None else axes
axes[0].plot(time, self.energies, label='energy')
p3_pu.decorate(axes[0], title='energy', ylab='J/kg', legend=True)
axes[1].plot(time, self.d_energies, label='d_energy')
p3_pu.decorate(axes[1], title='d_energy', ylab='J/kg/s', legend=True)
axes[2].plot(time, self.d2_energies, label='d2_energy')
p3_pu.decorate(axes[2],
title='d2_energy',
ylab='J/kg/s/s',
legend=True)
axes[3].plot(time, np.rad2deg(self.phi_sps), label='sp')
axes[3].plot(time, np.rad2deg(X[:, _s.sv_phi]), label='real')
p3_pu.decorate(axes[3], title='phi', ylab='deg', legend=True)
return fig, axes
def fit_phi(atm, dm):
phis = np.deg2rad(np.arange(-30, 31, 1.))
h, va = 0, 14
Xes, Ues = [], []
for phi in phis:
Xe, Ue = dm.foo_trim_aero_banked_cst_throttle(h=h, va=va, throttle=0., flaps=0., phi=phi, debug=True)
Xes.append(Xe); Ues.append(Ue)
Xes, Ues = np.array(Xes), np.array(Ues)
plt.figure()
plt.subplot(4,1,1)
#plt.plot(np.rad2deg(phis), np.rad2deg(trims[:,0]), label='gamma')
#p3_pu.decorate(plt.gca(), title='gamma', xlab='phi (deg)', ylab='deg')
plt.plot(np.rad2deg(phis), np.rad2deg(Xes[:,p3_fr.SixDOFEuclidianEuler.sv_theta]), label='theta')
p3_pu.decorate(plt.gca(), title='theta', xlab='phi (deg)', ylab='deg')
plt.subplot(4,1,2)
plt.plot(np.rad2deg(phis), np.rad2deg(Ues[:,dm.iv_de()]), label='ele')
p3_pu.decorate(plt.gca(), title='ele', xlab='phi (deg)', ylab='deg')
plt.subplot(4,1,3)
plt.plot(np.rad2deg(phis), np.rad2deg(Ues[:,dm.iv_da()]), label='ail')
plt.plot(np.rad2deg(phis), np.rad2deg(Ues[:,dm.iv_dr()]), label='rud')
p3_pu.decorate(plt.gca(), title='ail/rud', xlab='phi (deg)', ylab='deg', legend=True)
plt.subplot(4,1,4)
#plt.plot(np.rad2deg(phis), np.rad2deg(trims[:,4]), label='alpha')
#p3_pu.decorate(plt.gca(), title='alpha', xlab='phi (deg)', ylab='deg')
plt.plot(np.rad2deg(phis), Xes[:,p3_fr.SixDOFEuclidianEuler.sv_zd], label='zd')
p3_pu.decorate(plt.gca(), title='zd', xlab='phi (deg)', ylab='m/s')
plt.show()
def plot_gradient(atm, xmax=50, dx=5.):
h = 0
xlist, ylist = np.arange(-xmax, xmax, dx), np.arange(-xmax, xmax, dx)
x, y = np.meshgrid(xlist, ylist)
wz = np.zeros_like(y)
nx, ny = x.shape
for ix in range(x.shape[0]):
for iy in range(x.shape[1]):
wz[ix, iy] = -atm.get_wind([x[ix, iy], y[ix, iy], -h], t=0)[2]
#_c, _r = [10, 0], 20.
#_cs = [[-10, 0], [0, 0], [10, 0], [20, 0], [30, 0]]
_cs = [[-10, 0], [10, 0], [30, 0]]
#_cs = [[-40, 0]]
ax = plt.subplot(1, 3, 1)
ax.axis('equal')
cp = ax.contourf(x, y, wz, alpha=0.5)
plt.gcf().colorbar(cp)
ax.set_title('Updraft')
ax = plt.subplot(2, 3, 2)
p3_pu.decorate(ax,
title='Vz',
xlab='alpha in deg',
ylab='vz in m/s',
legend=True)
ax = plt.subplot(2, 3, 5)
p3_pu.decorate(ax,
title='Vz',
xlab='time in s',
ylab='vz in m/s',
legend=True)
for _c in _cs:
_r = 20.
va, dt = 9., 0.01
time = np.arange(0, 40., dt)
alphas = va * time / _r #np.arange(0, 4*np.pi, 0.1)
c_pts = np.array([pts_on_circle(_alpha, _c, _r) for _alpha in alphas])
ax = plt.subplot(1, 3, 1)
plt.plot(c_pts[:, 0], c_pts[:, 1])
ax = plt.subplot(2, 3, 2)
wc1 = [
-atm.get_wind([_x, _y, -h], t=0)[2]
for _x, _y in zip(c_pts[:, 0], c_pts[:, 1])
]
plt.plot(np.rad2deg(alphas), wc1)
ax = plt.subplot(2, 3, 5)
plt.plot(time, wc1)
ax = plt.subplot(2, 3, 6)
_foo(alphas, wc1, dt, time, _c)
def plot(self, time, X, U=None, figure=None, window_title="Trajectory"):
Ueng, Usfc = U[:, :self.P.eng_nb], U[:, self.P.eng_nb:]
Usolid = np.zeros((len(time), pat_dyn.SolidDM3.iv_size))
figure = pat_dyn.SolidDM3.plot(self, time, X, None, extra_rows=1)
ax = plt.subplot(5, 3, 13)
for i in range(self.P.eng_nb):
plt.plot(time, 100 * Ueng[:, i], label='prop_{}'.format(i))
ppu.decorate(ax, "Propulsion", xlab="time", ylab="%", legend=True)
ax = plt.subplot(5, 3, 14)
for i in range(self.P.sfc_nb):
plt.plot(time,
np.rad2deg(Usfc[:, i]),
label='sfc_{}'.format(self.P.sfc_name[i]))
ppu.decorate(ax, "Surfaces", legend=True, xlab="time", ylab="deg")
return figure
def plot(self, time):
eulers = np.array(self.eulers)
euler_ds = np.array(self.euler_ds)
euler_dds = np.array(self.euler_dds)
plots = [("$\phi$", "deg", np.rad2deg(eulers[:, 0])),
("$\\theta$", "deg", np.rad2deg(eulers[:, 1])),
("$\\psi$", "deg", np.rad2deg(eulers[:, 2])),
("$\dot{\phi}$", "deg/s", np.rad2deg(euler_ds[:, 0])),
("$\dot{\\theta}$", "deg/s", np.rad2deg(euler_ds[:, 1])),
("$\dot{\\psi}$", "deg/s", np.rad2deg(euler_ds[:, 2])),
("$\ddot{\phi}$", "deg/s2", np.rad2deg(euler_dds[:, 0])),
("$\ddot{\\theta}$", "deg/s2", np.rad2deg(euler_dds[:, 1])),
("$\ddot{\\psi}$", "deg/s2", np.rad2deg(euler_dds[:, 2]))]
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(3, 3, i + 1)
plt.plot(time, data)
p3_pu.decorate(ax, title=title, ylab=ylab)
plt.figure()
rvel_bodys = np.array(self.rvel_bodys)
raccel_bodys = np.array(self.raccel_bodys)
plots = [("$\phi$", "deg", np.rad2deg(eulers[:, 0])),
("$\\theta$", "deg", np.rad2deg(eulers[:, 1])),
("$\\psi$", "deg", np.rad2deg(eulers[:, 2])),
("$p$", "deg/s", np.rad2deg(rvel_bodys[:, 0])),
("$q$", "deg/s", np.rad2deg(rvel_bodys[:, 1])),
("$r$", "deg/s", np.rad2deg(rvel_bodys[:, 2])),
("$\dot{p}$", "deg/s2", np.rad2deg(raccel_bodys[:, 0])),
("$\dot{q}$", "deg/s2", np.rad2deg(raccel_bodys[:, 1])),
("$\dot{r}$", "deg/s2", np.rad2deg(raccel_bodys[:, 2]))]
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(3, 3, i + 1)
plt.plot(time, data)
p3_pu.decorate(ax, title=title, ylab=ylab)
# test num integ
dt = time[1] - time[0]
ps = np.cumsum(raccel_bodys[:, 0]) * dt + rvel_bodys[0, 0]
plt.subplot(3, 3, 4)
plt.plot(time, np.rad2deg(ps))
qs = np.cumsum(raccel_bodys[:, 1]) * dt + rvel_bodys[0, 1]
plt.subplot(3, 3, 5)
plt.plot(time, np.rad2deg(qs))
rs = np.cumsum(raccel_bodys[:, 2]) * dt + rvel_bodys[0, 2]
plt.subplot(3, 3, 6)
plt.plot(time, np.rad2deg(rs))
def fit_va(atm, dm, h=0):
vas, vzs = np.arange(6, 20, 1.), []
# compute simulator trims
for va in vas:
Xae_e, Uae_e = dm.trim({'h':h, 'va':va, 'throttle':0}, report=False, debug=False)
vzs.append(-Xae_e[p3_fr.SixDOFEuclidianEuler.sv_zd])
# fit polynomial
rho = p3_atm.get_rho(h)
K = 2.*dm.P.m*dm.P.g/rho/dm.P.Sref
H = np.array([[va**3/K, K/va] for va in vas])
CD0, B = np.dot(np.linalg.pinv(H), vzs)
print(f'K {K} CD0 {CD0} B {B}')
# display simulator points and polynomial model
vas2 = np.arange(6, 20, 0.1)
vzs2 = [va**3/K*CD0+K/va*B for va in vas2]
plt.plot(vas, vzs, '*')
plt.plot(vas2, vzs2)
p3_pu.decorate(plt.gca(), xlab='va (m/s)', ylab='vz (m/s)')
plt.show()
def plot_chronograms(self, time, X, U, _ctl, atm):
''' FIXME... X is Xae, is that mandatory? '''
_s = p3_fr.SixDOFEuclidianEuler
figure, axes = p1_fw_dyn.plot_trajectory_ae(time,
X,
U,
figure=None,
window_title='chronogram',
legend=None,
label='',
filename=None,
atm=atm)
Xee = np.array([
p3_fr.SixDOFAeroEuler.to_six_dof_euclidian_euler(_X, atm, _t)
for _X, _t in zip(X, time)
])
# x,y carrot
axes[0, 0].plot(time, np.asarray(self.carrot)[:, 0], label='carrot')
axes[0, 1].plot(time, np.asarray(self.carrot)[:, 1], label='carrot')
# h(up) instead of z(down)
ax = axes[0, 2]
ax.cla()
ax.plot(time, -Xee[:, _s.sv_z], label='plant')
ax.plot(time, -np.asarray(self.carrot)[:, _s.sv_z], label='setpoint')
p3_pu.decorate(ax, title="$h$", ylab="m", min_yspan=1., legend=True)
# hdot (instead of beta)
ax = axes[1, 2]
ax.cla()
ax.plot(time, -Xee[:, _s.sv_zd], label="plant")
ax.plot(time, -np.asarray(self.zd_sp), label="setpoint")
ax.plot(time, -np.asarray(self.zd_sp_traj), label="setpoint_ff")
p3_pu.decorate(ax,
title="$\dot{h}$",
ylab="m/s",
min_yspan=1.,
legend=True)
# attitude setpoint
phi_sp, theta_sp = np.asarray(self.att_sp)[:, 0], np.asarray(
self.att_sp)[:, 1]
axes[2, 0].plot(time, np.rad2deg(phi_sp), label="setpoint")
axes[2, 1].plot(time, np.rad2deg(theta_sp), label="setpoint")
def plot(self, time, Yc, U=None, figure=None, window_title="Trajectory"):
if figure is None: figure = plt.gcf()
plt.subplot(5, 3, 3)
plt.plot(time, Yc[:, 0], lw=2., label='setpoint')
euler_c = np.array([pal.euler_of_quat(_sp[1:]) for _sp in Yc])
plt.subplot(5, 3, 7)
plt.plot(time, np.rad2deg(euler_c[:, 0]), label='setpoint')
plt.subplot(5, 3, 8)
plt.plot(time, np.rad2deg(euler_c[:, 1]), label='setpoint')
plt.subplot(5, 3, 9)
plt.plot(time, np.rad2deg(euler_c[:, 2]), label='setpoint')
Uzpqr = np.array([np.dot(self.invH, Uk) for Uk in U])
ax = plt.subplot(5, 3, 14)
plt.plot(time, Uzpqr[:, 0], label='Uz')
ppu.decorate(ax, title='$U_z$', xlab='s', ylab='N', legend=True)
ax = plt.subplot(5, 3, 15)
plt.plot(time, Uzpqr[:, 1], label='Up')
plt.plot(time, Uzpqr[:, 2], label='Uq')
plt.plot(time, Uzpqr[:, 3], label='Ur')
ppu.decorate(ax, title='$U_{pqr}$', xlab='s', ylab='N', legend=True)
return figure
def plot_debug_chronograms(self, time, X, U, _ctl, atm):
_s = p3_fr.SixDOFEuclidianEuler
fig = plt.figure()
nrow, ncol = 2, 2
axes = fig.subplots(nrow, ncol, sharex=True)
axes[0, 0].plot(time, self.sum_err_v, label='')
p3_pu.decorate(axes[0, 0],
title="sum err v",
ylab="m/s",
min_yspan=1.,
legend=True)
theta_sp = np.asarray(self.att_sp)[:, 1]
axes[0, 1].plot(time, np.rad2deg(X[:, _s.sv_theta]), label='plant')
axes[0, 1].plot(time, np.rad2deg(theta_sp), label='sp')
p3_pu.decorate(axes[0, 1],
title="theta",
ylab="deg",
min_yspan=1.,
legend=True)
axes[1, 1].plot(time, np.rad2deg(U[:, 2]), label='ele')
p3_pu.decorate(axes[1, 1],
title="ele",
ylab="deg",
min_yspan=1.,
legend=True)
def plot_bag(bag_path='2019-12-03-13-53-31.bag'):
bag = rosbag.Bag(bag_path, "r")
_t, _z, _zd = [], [], []
for topic, msg, ts in bag.read_messages(topics=['/guidance/status']):
_t.append(ts.to_sec())
_z.append(msg.h)
_zd.append(msg.hdot)
_t = np.array(_t)
_z = np.array(_z)
_zd = np.array(_zd)
ax = plt.subplot(2, 1, 1)
plt.plot(_t, _z)
p3_pu.decorate(ax, title='height', xlab='time in s', ylab='z m')
ax = plt.subplot(2, 1, 2)
plt.plot(_t, _zd)
p3_pu.decorate(ax,
title='vz',
xlab='time in s',
ylab='vz in m/s',
legend=True)
plt.show()
def test_grid():
nc_f = '/home/poine/work/glider_experiments/data/extr_IHODC.1.RK4DI.007.nc'
atm = p3_atm.AtmosphereNC(nc_f)
print(f'range n:{atm.x0} {atm.x1} resolution: {atm.dx/(len(atm.ni)-1)}')
print(f'range e:{atm.y0} {atm.y1} resolution: {atm.dy/(len(atm.nj)-1)}')
print(
f'range d:{atm.z0} {atm.z1} resolution: {atm.dz/(len(atm.level)-1)}')
# North oriented line
ns, e, h = np.arange(0, 200), 25, 100
wzs = [atm.get_wind_ned1([n, e, -h], 0)[2] for n in ns]
wzs2 = [atm.get_wind_ned3([n, e, -h], 0)[2] for n in ns]
plt.plot(ns, wzs, '.', label='1')
plt.plot(ns, wzs2, '.', label='interp')
p3_plu.decorate(plt.gca(),
f'east: {e} height:{h}',
'north in m',
'vz in m/s',
legend=True)
if 0:
p3_plu.plot_slice_wind_nu(atm,
ns[0],
ns[-1],
dn=5.,
e0=e,
h0=0,
h1=100,
dh=2.,
zdir=-1.,
show_quiver=True,
show_color_bar=True,
figure=None,
ax=None)
plt.figure()
# East oriented line
n, es, h = 25, np.arange(0, 200), 100
wzs = [atm.get_wind_ned1([n, e, -h], 0)[2] for e in es]
wzs2 = [atm.get_wind_ned3([n, e, -h], 0)[2] for e in es]
plt.plot(es, wzs, '.', label='1')
plt.plot(es, wzs2, '.', label='interp')
p3_plu.decorate(plt.gca(),
f'north: {n} height:{h}',
'east in m',
'vz in m/s',
legend=True)
plt.figure()
# up oriented line
n, e, hs = 25, 25, np.arange(0, 300)
wzs = [atm.get_wind_ned1([n, e, -h], 0)[2] for h in hs]
wzs2 = [atm.get_wind_ned3([n, e, -h], 0)[2] for h in hs]
plt.plot(hs, wzs, '.', label='1')
plt.plot(hs, wzs2, '.', label='interp')
p3_plu.decorate(plt.gca(),
f'north: {n} east:{e}',
'h in m',
'vz in m/s',
legend=True)
def plot_ref(_time, Xr, _id="", _sp=None, fig=None, axs=None):
fig, axs = plt.subplots(3, 1) if fig is None else (fig, axs)
axs[0].plot(_time, Xr[:, 0], label=_id)
if _sp is not None: axs[0].plot(_time, _sp, label='setpoint')
p3_pu.decorate(axs[0], 'pos', legend=True)
axs[1].plot(_time, Xr[:, 1])
p3_pu.decorate(axs[1], 'vel')
axs[2].plot(_time, Xr[:, 2])
p3_pu.decorate(axs[2], 'accel')
return fig, axs
def plot_chronograms(self, time, X, _ctl, atm):
_s = p3_fr.SixDOFEuclidianEuler
plt.figure()
ax = plt.subplot(2, 1, 1)
plt.plot(time, self.vs_reading, label='vario')
plt.plot(time, self.vs_flt, label='vario_flt')
plt.plot(time, self.vs_disp, label='vario_disp')
p3_pu.decorate(ax, title='netto vario', ylab='m/s', legend=True)
ax = plt.subplot(2, 1, 2)
plt.plot(time, self.inovs, label='innovation')
p3_pu.decorate(ax, title='innovation', ylab='m/s', legend=True)
plt.figure()
Xs, Ps = np.array(self.Xs), np.array(self.Ps)
plots = [("strength", "m/s", Xs[:, ThermalFilter.sv_strength]),
("P_s", "m/s**2", Ps[:, ThermalFilter.sv_strength,
ThermalFilter.sv_strength]),
("radius", "m", Xs[:, ThermalFilter.sv_radius]),
("P_r", "m**2", Ps[:, ThermalFilter.sv_radius,
ThermalFilter.sv_radius]),
("xc", "m", Xs[:, ThermalFilter.sv_x]),
("P_x", "m**2", Ps[:, ThermalFilter.sv_x,
ThermalFilter.sv_x]),
("yc", "m", Xs[:, ThermalFilter.sv_y]),
("P_y", "m**2", Ps[:, ThermalFilter.sv_y,
ThermalFilter.sv_y])]
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(4, 2, i + 1)
plt.plot(time, data)
p3_pu.decorate(ax, title=title, ylab=ylab) #, legend=True)
try: # compute truth
X_flt_truth = np.zeros((len(time), ThermalFilter.sv_size))
X_flt_truth[:, ThermalFilter.sv_strength] = atm.strength
X_flt_truth[:, ThermalFilter.sv_radius] = atm.radius
for i in range(len(time)):
X_flt_truth[i, ThermalFilter.sv_x:ThermalFilter.sv_y +
1] = (atm.center -
X[i, _s.sv_slice_pos])[:2] # FIXME... time?
ax = plt.subplot(4, 2, 1)
plt.plot(time, X_flt_truth[:, ThermalFilter.sv_strength])
ax = plt.subplot(4, 2, 3)
plt.plot(time, X_flt_truth[:, ThermalFilter.sv_radius])
ax = plt.subplot(4, 2, 5)
plt.plot(time, X_flt_truth[:, ThermalFilter.sv_x])
ax = plt.subplot(4, 2, 7)
plt.plot(time, X_flt_truth[:, ThermalFilter.sv_y])
except AttributeError:
pass
def plot_trims(dm, throttle=0., force_recompute=False, nvs=10, nhs=10):
vs = np.linspace(5, 30, nvs, endpoint=True)
hs = np.linspace(0, 10000, nhs, endpoint=True)
trims = np.zeros((len(hs), len(vs), 2))
alphas, gammas, zdots = np.zeros((len(hs), len(vs))), np.zeros((len(hs), len(vs))), np.zeros((len(hs), len(vs)))
filename = '/tmp/foo2.npz'
if force_recompute or not os.path.exists(filename):
for i,v in enumerate(vs):
for j,h in enumerate(hs):
Xe, Ue = dm.trim({'va': vs[i], 'h': hs[i], 'throttle': throttle})
alphas[j, i], gammas[j, i] = Xe[dm.sv_alpha], Xe[dm.sv_theta]-Xe[dm.sv_alpha]
Xe_ee = p3_fr.SixDOFAeroEuler.to_six_dof_euclidian_euler(Xe)
zdots[j, i] = Xe_ee[ p3_fr.SixDOFEuclidianEuler.sv_zd]
np.savez(filename, alphas=alphas, gammas=gammas, zdots=zdots)
else:
data = np.load(filename)
alphas, gammas, zdots = data['alphas'], data['gammas'], data['zdots']
ax = plt.subplot(1,3,1)
alphas_deg = np.rad2deg(alphas)
v = np.linspace(np.min(alphas_deg), np.min([np.max(alphas_deg), 15]), 32, endpoint=True)
plt.contour(vs, hs, alphas_deg, v, linewidths=0.5, colors='k')
plt.contourf(vs, hs, alphas_deg, v, cmap=plt.cm.jet)
plt.colorbar(ticks=v)
p3_pu.decorate(ax, title='alpha', xlab='velocity (m/s)', ylab='height (m)')
ax = plt.subplot(1,3,2)
gammas_deg = np.rad2deg(gammas)
v = np.linspace(np.min(gammas_deg), np.max(gammas_deg), 32, endpoint=True)
plt.contour(vs, hs, gammas_deg, v, linewidths=0.5, colors='k')
plt.contourf(vs, hs, gammas_deg, v, cmap=plt.cm.jet)
plt.colorbar(ticks=v)
p3_pu.decorate(ax, title='gamma', xlab='velocity (m/s)', ylab='height (m)')
ax = plt.subplot(1,3,3)
v = np.linspace(np.min(zdots), 1., 32)#np.max(zdots), 32, endpoint=True)
plt.contour(vs, hs, zdots, v, linewidths=0.5, colors='k')
plt.contourf(vs, hs, zdots, v, cmap=plt.cm.jet)
plt.colorbar(ticks=v)
p3_pu.decorate(ax, title='zdot', xlab='velocity (m/s)', ylab='height (m)')
def plot_chronograms(self,
time,
X,
_ctl,
atm,
title=None,
window_title=None,
fig=None,
axes=None):
_s = p3_fr.SixDOFEuclidianEuler
fig = plt.figure(tight_layout=True, figsize=[6., 4.8
]) if fig is None else fig
if window_title is not None: fig.canvas.set_window_title(window_title)
axes = fig.subplots(3, 1) if axes is None else axes
center = np.array(self.center)
axes[0].plot(time, center[:, 0], label='est')
axes[1].plot(time, center[:, 1], label='est')
try: # plot center of atmosphere for analytic model that have them
foo = atm.radius
axes[0].plot(time,
np.ones(len(time)) * atm.center[0],
label='truth')
axes[1].plot(time,
np.ones(len(time)) * atm.center[1],
label='truth')
except AttributeError:
pass
p3_pu.decorate(axes[1], title='cy', ylab='m', legend=True)
p3_pu.decorate(axes[0], title='cx', ylab='m', legend=True)
axes[2].plot(time, self.meas_netto, label='netto')
axes[2].plot(time, self.meas_vz, label='vz')
p3_pu.decorate(axes[2],
title='measurement (vz)',
ylab='m/s',
legend=True)
return fig, axes
def old_thing(dm):
om_eps, xi_eps = 10., 0.7 # perturbation rejection
om_r, xi_r = 5., 0.9 # reference model
cl_poles = p3_u.omxi_to_lambda(om_eps, xi_eps)
#vas = [5., 10., 15., 20., 30.]
vas = np.arange(5., 30., 1.)
a0s, a1s, bs = [], [], []
Ks = []
for va in vas:
trim_args = {'h':0, 'va':va, 'gamma':0.}
Ac, Bc, Ad, Bd = _get_longi_lti_ssr(dm, trim_args)
a0s.append(-Ac[1,0]); a1s.append(-Ac[1,1]); bs.append(Bc[1,0])
Ks.append(control.place(Ac, Bc, cl_poles))
Ks = np.array(Ks).squeeze()
om2_eps, txiom_eps = om_eps**2, 2.*xi_eps*om_eps
K2s = np.array([[-(a0-om2_eps)/b, -(a1-txiom_eps)/b] for a0, a1, b in zip(a0s, a1s, bs)])
om2_r, txiom_r = om_r**2, 2.*xi_r*om_r
Hs = np.array([[(om2_eps-om2_r)/b, (txiom_eps-txiom_r)/b, om2_r/b] for b in bs])
#pdb.set_trace()
ax = plt.subplot(3,3,1)
plt.plot(vas, a0s)
p3_pu.decorate(ax, title='a0', xlab='va (m/s)')
ax = plt.subplot(3,3,2)
plt.plot(vas, a1s)
p3_pu.decorate(ax, title='a1', xlab='va (m/s)')
ax = plt.subplot(3,3,3)
plt.plot(vas, bs)
p3_pu.decorate(ax, title='b', xlab='va (m/s)')
ax = plt.subplot(3,3,4)
plt.plot(vas, Ks[:,0])
plt.plot(vas, K2s[:,0])
p3_pu.decorate(ax, title='k0', xlab='va (m/s)')
ax = plt.subplot(3,3,5)
plt.plot(vas, Ks[:,1])
plt.plot(vas, K2s[:,1])
p3_pu.decorate(ax, title='k1', xlab='va (m/s)')
ax = plt.subplot(3,3,7)
plt.plot(vas, Hs[:,0])
p3_pu.decorate(ax, title='h0', xlab='va (m/s)')
ax = plt.subplot(3,3,8)
plt.plot(vas, Hs[:,1])
p3_pu.decorate(ax, title='h1', xlab='va (m/s)')
ax = plt.subplot(3,3,9)
plt.plot(vas, Hs[:,2])
p3_pu.decorate(ax, title='h2', xlab='va (m/s)')
# plt.subplot(3,3,6)
# plt.plot(vas, K2s[:,0]*vas**2)
if 1:
#spl = scipy.interpolate.UnivariateSpline(vas, K2s[:,0])
k0_spl = scipy.interpolate.InterpolatedUnivariateSpline(vas, K2s[:,0])
k1_spl = scipy.interpolate.InterpolatedUnivariateSpline(vas, K2s[:,1])
h0_spl = scipy.interpolate.InterpolatedUnivariateSpline(vas, Hs[:,0])
h1_spl = scipy.interpolate.InterpolatedUnivariateSpline(vas, Hs[:,1])
h2_spl = scipy.interpolate.InterpolatedUnivariateSpline(vas, Hs[:,2])
plt.subplot(3,3,4)
plt.plot(vas, k0_spl(vas))
plt.subplot(3,3,5)
plt.plot(vas, k1_spl(vas))
plt.subplot(3,3,7)
plt.plot(vas, h0_spl(vas))
def plot_trajectory_ae(time,
X,
U=None,
figure=None,
window_title="Trajectory",
legend=None,
label='',
filename=None,
atm=None):
s = p3_fr.SixDOFAeroEuler
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
nrow, ncol = 5 if U is not None else 4, 3
axes = figure.subplots(nrow, ncol, sharex=True)
plots = [("x", "m", X[:, s.sv_x]), ("y", "m", X[:, s.sv_y]),
("z", "m", X[:, s.sv_z]), ("v", "m/s", X[:, s.sv_va]),
("$\\alpha$", "deg", np.rad2deg(X[:, s.sv_alpha])),
("$\\beta$", "deg", np.rad2deg(X[:, s.sv_beta])),
("$\phi$", "deg", np.rad2deg(X[:, s.sv_phi])),
("$\\theta$", "deg", np.rad2deg(X[:, s.sv_theta])),
("$\\psi$", "deg", np.rad2deg(X[:, s.sv_psi])),
("$p$", "deg/s", np.rad2deg(X[:, s.sv_p])),
("$q$", "deg/s", np.rad2deg(X[:, s.sv_q])),
("$r$", "deg/s", np.rad2deg(X[:, s.sv_r]))]
for (title, ylab, data), ax in zip(plots, axes.flatten()):
ax.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab)
if legend != None:
plt.legend(legend, loc='best')
for min_yspan, ax in zip([1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.],
axes.flatten()):
p3_pu.ensure_yspan(ax, min_yspan)
iv_da, iv_de, iv_dr, iv_df = 1, 2, 3, 4 # FIXME... needs P?
if U is not None:
ax = axes[4, 0]
ax.plot(time, 100 * U[:, 0])
p3_pu.decorate(ax, title="$d_{th}$", ylab="%", min_yspan=1.)
ax = axes[4, 1]
ax.plot(time, np.rad2deg(U[:, iv_da]), label="aileron")
ax.plot(time, np.rad2deg(U[:, iv_dr]), label="rudder")
p3_pu.decorate(ax,
title="$d_a/d_r$",
ylab="deg",
min_yspan=1.,
legend=True)
ax = axes[4, 2]
ax.plot(time, np.rad2deg(U[:, iv_de]), label="elevator")
if U.shape[1] > iv_df:
ax.plot(time, np.rad2deg(U[:, iv_df]), label="flap")
p3_pu.decorate(ax,
title="$d_e/d_f$",
ylab="deg",
min_yspan=1.,
legend=True)
return figure, axes
def plot_trajectory_ee(time,
Xee,
U=None,
figure=None,
window_title="Trajectory",
legend=None,
label='',
filename=None,
atm=None):
s = p3_fr.SixDOFEuclidianEuler
margins = (0.04, 0.05, 0.98, 0.96, 0.20, 0.34)
figure = p3_pu.prepare_fig(figure,
window_title,
figsize=(20.48, 10.24),
margins=margins)
plots = [("x", "m", Xee[:, s.sv_x]), ("y", "m", Xee[:, s.sv_y]),
("z", "m", Xee[:, s.sv_z]), ("$\dot{x}$", "m/s", Xee[:, s.sv_xd]),
("$\dot{y}$", "m/s", Xee[:, s.sv_yd]),
("$\dot{z}$", "m/s", Xee[:, s.sv_zd]),
("$\phi$", "deg", np.rad2deg(Xee[:, s.sv_phi])),
("$\\theta$", "deg", np.rad2deg(Xee[:, s.sv_theta])),
("$\\psi$", "deg", np.rad2deg(Xee[:, s.sv_psi])),
("$p$", "deg/s", np.rad2deg(Xee[:, s.sv_p])),
("$q$", "deg/s", np.rad2deg(Xee[:, s.sv_q])),
("$r$", "deg/s", np.rad2deg(Xee[:, s.sv_r]))]
nrow = 5 if U is not None else 4
for i, (title, ylab, data) in enumerate(plots):
ax = plt.subplot(nrow, 3, i + 1)
plt.plot(time, data, label=label)
p3_pu.decorate(ax, title=title, ylab=ylab, legend=True)
#if legend!=None:
# plt.legend(legend, loc='best')
for i, min_yspan in enumerate(
[1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.]):
p3_pu.ensure_yspan(plt.subplot(nrow, 3, i + 1), min_yspan)
iv_da, iv_de, iv_dr, iv_df = 1, 2, 3, 4 # FIXME... needs P?
if U is not None:
ax = figure.add_subplot(5, 3, 13)
ax.plot(time, 100 * U[:, 0])
p3_pu.decorate(ax, title="$d_{th}$", ylab="%", min_yspan=0.01)
ax = figure.add_subplot(5, 3, 14)
ax.plot(time, np.rad2deg(U[:, iv_da]), label="aileron")
ax.plot(time, np.rad2deg(U[:, iv_dr]), label="rudder")
p3_pu.decorate(ax,
title="$d_a/d_r$",
ylab="deg",
min_yspan=1.,
legend=True)
ax = figure.add_subplot(5, 3, 15)
ax.plot(time, np.rad2deg(U[:, iv_de]), label="elevator")
ax.plot(time, np.rad2deg(U[:, iv_df]), label="flap")
p3_pu.decorate(ax,
title="$d_e/d_f$",
ylab="deg",
min_yspan=1.,
legend=True)
return figure
def plot_solution_chronogram(planner, max_idx=None):
if max_idx is None: max_idx=len(planner.sol_time)
fig, axes = plt.subplots(3, 2)
axes[0,0].plot(planner.sol_time[:max_idx], planner.sol_e[:max_idx])
p3_plu.decorate(axes[0,0], 'e', 't in s', 'e in m')
axes[1,0].plot(planner.sol_time[:max_idx], planner.sol_n[:max_idx])
p3_plu.decorate(axes[1,0], 'n', 't in s', 'n in m')
axes[2,0].plot(planner.sol_time[:max_idx], planner.sol_u[:max_idx])
p3_plu.decorate(axes[2,0], 'u', 't in s' , 'h in m')
axes[0,1].plot(planner.sol_time[:max_idx], np.rad2deg(planner.sol_psi[:max_idx]))
p3_plu.decorate(axes[0,1], '$\psi$', 't in s' , 'psi in deg')
axes[1,1].plot(planner.sol_time[:max_idx], planner.sol_v[:max_idx])
p3_plu.decorate(axes[1,1], 'v', 't in s' , 'v in m/s')
axes[2,1].plot(planner.sol_time[:max_idx], np.rad2deg(planner.sol_phi[:max_idx]))
p3_plu.decorate(axes[2,1], '$\phi$', 't in s', 'phi in deg')
def find_best_radius(dm,
atm,
va=9.,
compute=False,
plot_atm=False,
plot_traj=False):
if plot_atm:
p3_pu.plot_slice_wind(atm, xmax=100, dx=2.5)
plt.show()
savefile_name = '/tmp/thermal_optim_{:.1f}.npz'.format(va)
if compute:
time, Xe, Ue, phi_sp, theta_sp = test_02_att_ctl.get_sim_defaults(
dm, tf=15, trim_args={
'h': 0,
'va': va,
'throttle': 0
})
vzs, phis = [], []
for _phi_sp in np.deg2rad(np.arange(0, 45, 1.)):
phi_sp = _phi_sp * np.ones(len(time))
time, X, U = test_02_att_ctl.run_simulation(dm,
time,
Xe,
Ue,
phi_sp,
theta_sp,
plot=plot_traj)
if plot_traj: plt.show()
Xee = dm.state_six_dof_euclidian_euler(X[-1], atm=None)
vzs.append(Xee[p3_fr.SixDOFEuclidianEuler.sv_zd])
phis.append(X[-1][dm.sv_phi])
#print('vz {:.2f}m/s'.format(Xee[p3_fr.SixDOFEuclidianEuler.sv_zd]))
#print('phi {}'.format(np.rad2deg(X[-1][dm.sv_phi])))
phis, vzs = np.array(phis), np.array(vzs)
g = 9.81
Rs = va**2 / g / np.tan(phis)
#atm = p3_atm.AtmosphereThermal1()
updrafts = np.array(
[atm.get_wind(pos_ned=[_r, 0, 0], t=0) for _r in Rs])
np.savez(savefile_name, vzs=vzs, phis=phis, Rs=Rs, updrafts=updrafts)
else:
_data = np.load(savefile_name)
vzs, phis, Rs, updrafts = [
_data[k] for k in ['vzs', 'phis', 'Rs', 'updrafts']
]
climb_rate = updrafts[1:, 2] + vzs[1:]
best_climb_idx = np.argmin(climb_rate)
res_txt = 'best vz {:.2f} radius {:.1f} roll {:.1f}'.format(
climb_rate[best_climb_idx], Rs[best_climb_idx],
np.rad2deg(phis[best_climb_idx]))
print('va {:.1f} '.format(va) + res_txt)
good_Rs = Rs < 100.
fig = p3_pu.prepare_fig(window_title='va = {:.1f} m/s'.format(va))
ax = plt.subplot(1, 3, 1)
plt.plot(np.rad2deg(phis), vzs, label='aircraft')
plt.plot(np.rad2deg(phis), updrafts[:, 2], label='updraft')
p3_pu.decorate(ax,
title='Vz',
xlab='roll in deg',
ylab='vz in m/s',
legend=True)
plt.subplot(1, 3, 2)
if 1:
plt.plot(np.rad2deg(phis[good_Rs]), Rs[good_Rs])
p3_pu.decorate(plt.gca(),
title='Radius',
xlab='roll in deg',
ylab='radius in m')
if 0:
plt.plot(np.rad2deg(phis), 1 / Rs)
p3_pu.decorate(plt.gca(),
title='Curvature',
xlab='roll in deg',
ylab='curvature in m^-1')
#pdb.set_trace()
ax = plt.subplot(1, 3, 3)
#plt.plot(Rs[good_Rs], vzs[good_Rs], label='aircraft')
#plt.plot(Rs[good_Rs], updrafts[good_Rs,2], label='updrafts')
plt.plot(Rs[1:], vzs[1:], label='aircraft')
plt.plot(Rs[1:], updrafts[1:, 2], label='updrafts')
plt.plot(Rs[1:], updrafts[1:, 2] + vzs[1:], label='sum')
p3_pu.decorate(ax,
title='Vz\n{}'.format(res_txt),
xlab='R in m',
ylab='vz in m/s',
legend=True)
