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)