def __init__(self, plotting_freq=1):
self.plotter = Plotter(plotting_freq, time_window=30)
self.plotter.set_plots_per_row(2)
# Define plot names
plots = self._define_plots()
# Add plots to the window
for p in plots:
self.plotter.add_plotboxes(p)
# Define state vectors for simpler input
self._define_input_vectors()
self.R_err_thresh = 0.01
self.R_thresh_reached = False
self.az_err_thresh = 0.001
self.az_thresh_reached = False
def __init__(self, update_freq=30, use_degrees=True):
# Store parameters
self.use_degrees = use_degrees
# Setup the plotter window
self.plotter = Plotter()
# Define plot names
plots = [
'n _n_g', 'e _e_g', 'h _h_g', 'Va _Va_g', 'chi _chi_g', 'e n --2d'
]
#plots = [['n', 'n_g'], ['e', 'e_g'], ['h', 'h_g'],
# ['Va', 'Va_g'], ['chi', 'chi_g'], ['traj']]
# Add plots to the window
for p in plots:
self.plotter.add_plot(p)
self.tf_listener = tf.TransformListener()
# Define input vectors for easier input
self.plotter.define_input_vector('neh', ['n', 'e', 'h'])
self.plotter.define_input_vector('neh_g', ['n_g', 'e_g', 'h_g'])
# Subscribe to relevant ROS topics
rospy.Subscriber('ned_g', Vector3Stamped, self.ned_g_cb_)
rospy.Subscriber('twist', TwistStamped, self.velocity_cb_)
rospy.Subscriber('chi_g', Float32, self.chi_g_cb_)
rospy.Subscriber('Va_g', Float32, self.va_g_cb_)
self.crab = 0.0
self.wind = np.zeros(3)
# Update the plots
rate = rospy.Rate(update_freq)
while not rospy.is_shutdown():
self.tick()
self.plotter.update_plots()
rate.sleep()
def __init__(self):
time_window_length=100
self.plotter = Plotter(plotting_frequency=100, # refresh plot every 100 time steps
time_window=time_window_length, # plot last time_window seconds of data
window_title="Sensors")
# set up the plot window
# define first row
gyro_x_plots = PlotboxArgs(plots=['gyro_x'],
labels={'left': 'gyro_x(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
gyro_y_plots = PlotboxArgs(plots=['gyro_y'],
labels={'left': 'gyro_y(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
gyro_z_plots = PlotboxArgs(plots=['gyro_z'],
labels={'left': 'gyro_z(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
static_pressure_plots = PlotboxArgs(plots=['static_pressure'],
labels={'left': 'pressure(Pa)', 'bottom': 'Time (s)'},
time_window=time_window_length)
first_row = [gyro_x_plots, gyro_y_plots, gyro_z_plots, static_pressure_plots]
# define second row
accel_x_plots = PlotboxArgs(plots=['accel_x'],
labels={'left': 'accel_x(m/s^2)', 'bottom': 'Time (s)'},
time_window=time_window_length)
accel_y_plots = PlotboxArgs(plots=['accel_y'],
labels={'left': 'accel_y(m/s^2)', 'bottom': 'Time (s)'},
time_window=time_window_length)
accel_z_plots = PlotboxArgs(plots=['accel_z'],
labels={'left': 'accel_z(m/s^2)', 'bottom': 'Time (s)'},
time_window=time_window_length)
diff_pressure_plots = PlotboxArgs(plots=['diff_pressure'],
labels={'left': 'pressure(Pa)', 'bottom': 'Time (s)'},
time_window=time_window_length)
second_row = [accel_x_plots, accel_y_plots, accel_z_plots, diff_pressure_plots]
# define third row
gps_n_plots = PlotboxArgs(plots=['gps_n'],
labels={'left': 'distance(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
gps_e_plots = PlotboxArgs(plots=['gps_e'],
labels={'left': 'distance(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
gps_h_plots = PlotboxArgs(plots=['gps_h'],
labels={'left': 'distance(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
third_row = [gps_n_plots, gps_e_plots, gps_h_plots]
# define fourth row
gps_Vg_plots = PlotboxArgs(plots=['gps_Vg'],
labels={'left': 'gps_Vg(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
gps_course_plots = PlotboxArgs(plots=['gps_course'],
labels={'left': 'gps_course (deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
fourth_row = [gps_Vg_plots, gps_course_plots]
plots = [first_row,
second_row,
third_row,
fourth_row
]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('sensors', ['gyro_x', 'gyro_y', 'gyro_z',
'static_pressure',
'accel_x', 'accel_y', 'accel_z',
'diff_pressure',
'gps_n', 'gps_e', 'gps_h',
'gps_Vg', 'gps_course'])
# plot timer
self.time = 0.
def __init__(self):
time_window_length = 100
self.plotter = Plotter(
plotting_frequency=20, # refresh plot every 20 time steps
time_window=time_window_length
) # plot last time_window seconds of data
light_theme = True
if light_theme:
self.plotter.use_light_theme()
axis_color = 'k'
min_hue = 0 #260
max_hue = 900 #500
else:
axis_color = 'w'
min_hue = 0
max_hue = 900
# set up the plot window
# define first row
pn_plots = PlotboxArgs(plots=['pn', 'pn_c'],
labels={
'left': 'pn(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
pe_plots = PlotboxArgs(plots=['pe', 'pe_c'],
labels={
'left': 'pe(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
pd_plots = PlotboxArgs(plots=['pd', 'pd_c'],
labels={
'left': 'pd(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
first_row = [pn_plots, pe_plots, pd_plots]
# define second row
u_plots = PlotboxArgs(plots=['u', 'u_c'],
labels={
'left': 'u(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
v_plots = PlotboxArgs(plots=['v', 'v_c'],
labels={
'left': 'v(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
w_plots = PlotboxArgs(plots=['w', 'w_c'],
labels={
'left': 'w(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
second_row = [u_plots, v_plots, w_plots]
# define third row
phi_plots = PlotboxArgs(plots=['phi', 'phi_c'],
labels={
'left': 'phi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
theta_plots = PlotboxArgs(plots=['theta', 'theta_c'],
labels={
'left': 'theta(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
psi_plots = PlotboxArgs(plots=['psi', 'psi_c'],
labels={
'left': 'psi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
third_row = [phi_plots, theta_plots, psi_plots]
# define fourth row
p_plots = PlotboxArgs(plots=['p', 'p_c'],
labels={
'left': 'p(deg/s)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
q_plots = PlotboxArgs(plots=['q', 'q_c'],
labels={
'left': 'q(deg/s)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
r_plots = PlotboxArgs(plots=['r', 'r_c'],
labels={
'left': 'r(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
fourth_row = [p_plots, q_plots, r_plots]
plots = [first_row, second_row, third_row, fourth_row]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('true_state', [
'pn', 'pe', 'pd', 'u', 'v', 'w', 'phi', 'theta', 'psi', 'p', 'q',
'r'
])
self.plotter.define_input_vector('estimated_state', [
'pn_e', 'pe_e', 'pd_e', 'u_e', 'v_e', 'w_e', 'phi_e', 'theta_e',
'psi_e', 'p_e', 'q_e', 'r_e'
])
self.plotter.define_input_vector('commands', [
'pn_c', 'pe_c', 'pd_c', 'u_c', 'v_c', 'w_c', 'phi_c', 'theta_c',
'psi_c', 'p_c', 'q_c', 'r_c'
])
# plot timer
self.time = 0.
def __init__(self):
time_window_length = 100
self.plotter = Plotter(
plotting_frequency=10, # refresh plot every 100 time steps
time_window=time_window_length
) # plot last time_window seconds of data
# set up the plot window
# define first row
pn_plots = PlotboxArgs(plots=['pn', 'pn_c'],
labels={
'left': 'pn(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
pe_plots = PlotboxArgs(plots=['pe', 'pe_c'],
labels={
'left': 'pe(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
h_plots = PlotboxArgs(plots=['h', 'h_c'],
labels={
'left': 'h(m)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
first_row = [pn_plots, pe_plots, h_plots]
u_plots = PlotboxArgs(plots='u',
labels={
'left': 'u(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
v_plots = PlotboxArgs(plots='v',
labels={
'left': 'v(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
w_plots = PlotboxArgs(plots='w',
labels={
'left': 'w(m/s)',
'bottom': 'Time (s)'
},
time_window=time_window_length)
second_row = [u_plots, v_plots, w_plots]
# define second row
phi_plots = PlotboxArgs(plots=['phi'],
labels={
'left': 'phi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
theta_plots = PlotboxArgs(plots=['theta'],
labels={
'left': 'theta(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
psi_plots = PlotboxArgs(plots=['psi', 'psi_c'],
labels={
'left': 'psi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
third_row = [phi_plots, theta_plots, psi_plots]
# define third row
p_plots = PlotboxArgs(plots=['p'],
labels={
'left': 'p(deg/s)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
q_plots = PlotboxArgs(plots=['q'],
labels={
'left': 'q(deg/s)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
r_plots = PlotboxArgs(plots=['r'],
labels={
'left': 'r(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
fourth_row = [p_plots, q_plots, r_plots]
plots = [first_row, second_row, third_row, fourth_row]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('true_state', [
'pn', 'pe', 'h', 'u', 'v', 'w', 'phi', 'theta', 'psi', 'p', 'q',
'r'
])
self.plotter.define_input_vector('commanded_state',
['pn_c', 'pe_c', 'h_c', 'psi_c'])
# plot timer
self.time = 0.
def __init__(self):
time_window_length = 100
self.plotter = Plotter(
plotting_frequency=20, # refresh plot every 20 time steps
time_window=time_window_length
) # plot last time_window seconds of data
light_theme = True
if light_theme:
self.plotter.use_light_theme()
axis_color = 'k'
min_hue = 0 #260
max_hue = 900 #500
else:
axis_color = 'w'
min_hue = 0
max_hue = 900
# set up the plot window
# define first row
ft_plots = PlotboxArgs(plots=['ft', 'ft_z'],
labels={
'left': 'thrust',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
first_row = [ft_plots]
# define second row
tau_x_plots = PlotboxArgs(plots=['tau_x', 'tau_x_z'],
labels={
'left': 'tau_x',
'bottom': 'Time (s)'
},
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
second_row = [tau_x_plots]
# define third row
tau_y_plots = PlotboxArgs(plots=['tau_y', 'tau_y_z'],
labels={
'left': 'tau_y',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
third_row = [tau_y_plots]
# define fourth row
tau_z_plots = PlotboxArgs(plots=['tau_z', 'tau_z_z'],
labels={
'left': 'tau_z',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length,
plot_min_hue=min_hue,
plot_max_hue=max_hue,
axis_color=axis_color)
fourth_row = [tau_z_plots]
plots = [first_row, second_row, third_row, fourth_row]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('true_state',
['ft', 'tau_x', 'tau_y', 'tau_z'])
self.plotter.define_input_vector(
'commands', ['ft_z', 'tau_x_z', 'tau_y_z', 'tau_z_z'])
# plot timer
self.time = 0.
#!/usr/bin/env python
import time
from builtins import input
from IPython.core.debugger import set_trace
import numpy as np
from state_plotter.Plotter import Plotter
from state_plotter.plotter_args import *
plotter = Plotter(plotting_frequency=1)
### Define plot names
## Simple string definitions
first_row = ['x', 'y', 'z']
## Multiple plots in a plotbox (using PlotboxArgs)
# -Simply add multiple plot names
phi_plots = PlotboxArgs(plots=['phi', 'phi_e'])
# -Add title to the plotbox
theta_plots = PlotboxArgs(title="Multiple theta plots",
plots=['theta', 'theta_e'])
# -Use Plot args to name different curves in the legend,
# -Use 'labels' to get more detailed x and y labels
# -Use rad2deg to automatically wrap angles and convert radians to degrees
psi_plots = PlotboxArgs(title="Multiple psi plots",
plots=[
PlotArgs('True psi', states=['psi']),
PlotArgs('Estimated psi', states='psi_e')
],
def __init__(self):
time_window_length=100
self.plotter = Plotter(plotting_frequency=100, # refresh plot every 100 time steps
time_window=time_window_length) # plot last time_window seconds of data
# set up the plot window
# define first row
pn_plots = PlotboxArgs(plots=['pn', 'pn_e'],
labels={'left': 'pn(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
pe_plots = PlotboxArgs(plots=['pe', 'pe_e'],
labels={'left': 'pe(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
h_plots = PlotboxArgs(plots=['h', 'h_e', 'h_c'],
labels={'left': 'h(m)', 'bottom': 'Time (s)'},
time_window=time_window_length)
wind_plots = PlotboxArgs(plots=['wn', 'wn_e', 'we', 'we_e'],
labels={'left': 'wind(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
first_row = [pn_plots, pe_plots, h_plots, wind_plots]
# define second row
Va_plots = PlotboxArgs(plots=['Va', 'Va_e', 'Va_c'],
labels={'left': 'Va(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
alpha_plots = PlotboxArgs(plots=['alpha', 'alpha_e'],
labels={'left': 'alpha(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
beta_plots = PlotboxArgs(plots=['beta', 'beta_e'],
labels={'left': 'beta(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
Vg_plots = PlotboxArgs(plots=['Vg', 'Vg_e'],
labels={'left': 'Vg(m/s)', 'bottom': 'Time (s)'},
time_window=time_window_length)
second_row = [Va_plots, alpha_plots, beta_plots, Vg_plots]
# define third row
phi_plots = PlotboxArgs(plots=['phi', 'phi_e', 'phi_c'],
labels={'left': 'phi(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
theta_plots = PlotboxArgs(plots=['theta', 'theta_e', 'theta_c'],
labels={'left': 'theta(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
psi_plots = PlotboxArgs(plots=['psi', 'psi_e'],
labels={'left': 'psi(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
chi_plots = PlotboxArgs(plots=['chi', 'chi_e', 'chi_c'],
labels={'left': 'chi(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
third_row = [phi_plots, theta_plots, psi_plots, chi_plots]
# define fourth row
p_plots = PlotboxArgs(plots=['p', 'p_e'],
labels={'left': 'p(deg/s)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
q_plots = PlotboxArgs(plots=['q', 'q_e'],
labels={'left': 'q(deg/s)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
r_plots = PlotboxArgs(plots=['r', 'r_e'],
labels={'left': 'r(deg)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
gyro_plots = PlotboxArgs(plots=['bx', 'bx_e', 'by', 'by_e', 'bz', 'bz_e'],
labels={'left': 'bias(deg/s)', 'bottom': 'Time (s)'},
rad2deg=True,
time_window=time_window_length)
fourth_row = [p_plots, q_plots, r_plots, gyro_plots]
plots = [first_row,
second_row,
third_row,
fourth_row
]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('true_state', ['pn', 'pe', 'h', 'Va', 'alpha', 'beta', 'phi', 'theta', 'chi',
'p', 'q', 'r', 'Vg', 'wn', 'we', 'psi', 'bx', 'by', 'bz'])
self.plotter.define_input_vector('estimated_state', ['pn_e', 'pe_e', 'h_e', 'Va_e', 'alpha_e', 'beta_e',
'phi_e', 'theta_e', 'chi_e', 'p_e', 'q_e', 'r_e',
'Vg_e', 'wn_e', 'we_e', 'psi_e', 'bx_e', 'by_e', 'bz_e'])
self.plotter.define_input_vector('commands', ['h_c', 'Va_c', 'phi_c', 'theta_c', 'chi_c'])
# plot timer
self.time = 0.
def __init__(self, update_freq=30, use_degrees=True):
self.t0 = rospy.Time.now().to_sec()
# Store parameters
self.use_degrees = use_degrees
# Setup the plotter window
self.plotter = Plotter()
# Define plot names
plots = [
'n n_hat n_gps -l', 'e e_hat e_gps -l', 'h h_hat h_gps h_c -l',
'phi phi_hat -l', 'theta theta_hat -l', 'psi psi_hat -l',
'u u_hat -l', 'v v_hat -l', 'w w_hat -l', 'p p_hat p_gyro -l',
'q q_hat q_gyro -l', 'r r_hat r_gyro -l', 'ax', 'ay', 'az',
'Va Va_c -l', 'chi chi_hat chi_c chi_gps -l', 'p_diff p_abs -l'
]
self.wind = np.zeros(3)
self.crab = 0.0
# Add plots to the window
for p in plots:
self.plotter.add_plot(p)
# Add legends
# self.plotter.add_legend('n')
# self.plotter.add_legend('e')
# self.plotter.add_legend('h')
# self.plotter.add_legend('p')
# self.plotter.add_legend('q')
# self.plotter.add_legend('r')
# self.plotter.add_legend('Va')
# self.plotter.add_legend('chi')
self.tf_listener = tf.TransformListener()
# Define input vectors for easier input
self.plotter.define_input_vector('neh', ['n', 'e', 'h'])
self.plotter.define_input_vector('lin_velocity', ['u', 'v', 'w'])
self.plotter.define_input_vector('ang_velocity', ['p', 'q', 'r'])
self.plotter.define_input_vector('euler', ['phi', 'theta', 'psi'])
self.plotter.define_input_vector('neh_gps',
['n_gps', 'e_gps', 'h_gps'])
self.plotter.define_input_vector('ang_gyro',
['p_gyro', 'q_gyro', 'r_gyro'])
self.plotter.define_input_vector('acc', ['ax', 'ay', 'az'])
self.plotter.define_input_vector('euler_est',
['phi_hat', 'theta_hat', 'psi_hat'])
self.plotter.define_input_vector('neh_est',
['n_hat', 'e_hat', 'h_hat'])
self.plotter.define_input_vector('vb_est', ['u_hat', 'v_hat', 'w_hat'])
# Subscribe to relevant ROS topics
rospy.Subscriber('twist', TwistStamped, self.velocity_cb_)
rospy.Subscriber('imu_lpf', Imu, self.imu_cb_)
rospy.Subscriber('Va_c', Float32, self.va_cb_)
rospy.Subscriber('gps_neh', Vector3Stamped, self.gps_neh_cb_)
rospy.Subscriber('gps_chi', Float32, self.gps_chi_cb_)
rospy.Subscriber('gps_vg', Float32, self.gps_vg_cb_)
rospy.Subscriber('h_c', Float32, self.h_cb_)
rospy.Subscriber('chi_c', Float32, self.chi_cb_)
rospy.Subscriber('p_diff', Float32, self.p_diff_cb_)
rospy.Subscriber('p_static', Float32, self.p_static_cb_)
rospy.Subscriber('euler_est', Vector3Stamped, self.euler_est_cb_)
rospy.Subscriber('ned_est', Vector3Stamped, self.ned_est_cb_)
rospy.Subscriber('chi_est', Float32, self.chi_est_cb_)
rospy.Subscriber('vb_est', Vector3Stamped, self.vb_est_cb_)
self.va_c = None
self.h_c = None
self.chi_c = None
# Update the plots
rate = rospy.Rate(update_freq)
while not rospy.is_shutdown():
self.tick()
self.plotter.update_plots()
rate.sleep()
def __init__(self):
time_window_length = 100
self.plotter = Plotter(
plotting_frequency=100, # refresh plot every 100 time steps
time_window=time_window_length
) # plot last time_window seconds of data
# set up the plot window
# define first row
pn_plots = PlotboxArgs(
plots=['pn', 'pn_EKF', 'pn_FSD_EKF', 'pn_FSI_EKF'],
labels={'left': 'pn(m)'},
time_window=time_window_length)
pe_plots = PlotboxArgs(
plots=['pe', 'pe_EKF', 'pe_FSD_EKF', 'pe_FSI_EKF'],
labels={'left': 'pe(m)'},
time_window=time_window_length)
h_plots = PlotboxArgs(plots=['h', 'h_EKF', 'h_FSD_EKF', 'h_FSI_EKF'],
labels={'left': 'h(m)'},
time_window=time_window_length)
first_row = [pn_plots, pe_plots, h_plots]
# define second row
Va_plots = PlotboxArgs(
plots=['Va', 'Va_EKF', 'Va_FSD_EKF', 'Va_FSI_EKF'],
labels={'left': 'Va(m/s)'},
time_window=time_window_length)
Vg_plots = PlotboxArgs(
plots=['Vg', 'Vg_EKF', 'Vg_FSD_EKF', 'Vg_FSI_EKF'],
labels={'left': 'Vg(m/s)'},
time_window=time_window_length)
chi_plots = PlotboxArgs(
plots=['chi', 'chi_EKF', 'chi_FSD_EKF', 'chi_FSI_EKF'],
labels={'left': 'chi(deg)'},
rad2deg=True,
time_window=time_window_length)
second_row = [Va_plots, Vg_plots, chi_plots]
# define third row
phi_plots = PlotboxArgs(
plots=['phi', 'phi_EKF', 'phi_FSD_EKF', 'phi_FSI_EKF'],
labels={
'left': 'phi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
theta_plots = PlotboxArgs(
plots=['theta', 'theta_EKF', 'theta_FSD_EKF', 'theta_FSI_EKF'],
labels={
'left': 'theta(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
psi_plots = PlotboxArgs(
plots=['psi', 'psi_EKF', 'psi_FSD_EKF', 'psi_FSI_EKF'],
labels={
'left': 'psi(deg)',
'bottom': 'Time (s)'
},
rad2deg=True,
time_window=time_window_length)
third_row = [phi_plots, theta_plots, psi_plots]
plots = [first_row, second_row, third_row]
# Add plots to the window
self.plotter.add_plotboxes(plots)
# Define and label vectors for more convenient/natural data input
self.plotter.define_input_vector('true_state', [
'pn', 'pe', 'h', 'Va', 'alpha', 'beta', 'phi', 'theta', 'chi', 'p',
'q', 'r', 'Vg', 'wn', 'we', 'psi', 'bx', 'by', 'bz'
])
self.plotter.define_input_vector('EKF', [
'pn_EKF', 'pe_EKF', 'h_EKF', 'Va_EKF', 'alpha_EKF', 'beta_EKF',
'phi_EKF', 'theta_EKF', 'chi_EKF', 'p_EKF', 'q_EKF', 'r_EKF',
'Vg_EKF', 'wn_EKF', 'we_EKF', 'psi_EKF', 'bx_EKF', 'by_EKF',
'bz_EKF'
])
self.plotter.define_input_vector('FSD_EKF', [
'pn_FSD_EKF', 'pe_FSD_EKF', 'h_FSD_EKF', 'Va_FSD_EKF',
'alpha_FSD_EKF', 'beta_FSD_EKF', 'phi_FSD_EKF', 'theta_FSD_EKF',
'chi_FSD_EKF', 'p_FSD_EKF', 'q_FSD_EKF', 'r_FSD_EKF', 'Vg_FSD_EKF',
'wn_FSD_EKF', 'we_FSD_EKF', 'psi_FSD_EKF2', 'bx_FSD_EKF',
'by_FSD_EKF', 'bz_FSD_EKF'
])
self.plotter.define_input_vector('FSI_EKF', [
'pn_FSI_EKF', 'pe_FSI_EKF', 'h_FSI_EKF', 'Va_FSI_EKF',
'alpha_FSI_EKF', 'beta_FSI_EKF', 'phi_FSI_EKF', 'theta_FSI_EKF',
'chi_FSI_EKF', 'p_FSI_EKF', 'q_FSI_EKF', 'r_FSI_EKF', 'Vg_FSI_EKF',
'wn_FSI_EKF', 'we_FSI_EKF', 'psi_FSI_EKF2', 'bx_FSI_EKF',
'by_FSI_EKF', 'bz_FSI_EKF'
])
# plot timer
self.time = 0.