def plot_r_vs_time(self):
newfig('sqrt(x^2 + y^2) vs. Time', 'time [s]', 'North [m]')
t = self.v.state.t[self.v.state.istart:self.v.state.iend]
r = np.sqrt(self.v.state.north[self.v.state.istart:self.v.state.iend]**2 + \
self.v.state.east[self.v.state.istart:self.v.state.iend]**2 )
plot_traj(t, r, label="meas (Vicon)")
self._timeseries_postplot()
def plot_axis_control(self, axis,
show_mux_cmd=True, show_asctec_cmd=True, show_vicon_meas=True, show_imu_meas=True):
axismap = {'roll': (self.v.control.roll_cmd, 2, +1, self.v.asctec_ctrl_input.roll_cmd, -1),
'pitch': (self.v.control.pitch_cmd, 1, -1, self.v.asctec_ctrl_input.pitch_cmd, -1),
'yaw': (self.v.control.yaw_cmd, 0, -1, self.v.asctec_ctrl_input.yaw_rate_cmd, +1) # not sure about the multiplier here
}
control_mode_cmd, state_axis, imu_mult, asctec_cmd, asctec_cmd_mult = axismap[axis]
newfig("%s Axis Control" % axis.capitalize(), "time [s]", "%s [deg]" % axis.capitalize())
# np.clip() and the [1:] stuff in the following to attempt deal with bogus initial data points in IMU data:
if show_mux_cmd:
plt.plot(self.v.control.t[self.v.control.istart:self.v.control.iend],
control_mode_cmd[self.v.control.istart:self.v.control.iend], label='cmd (from mux)')
if show_vicon_meas:
plt.plot(self.v.state.t[self.v.state.istart:self.v.state.iend],
self.v.state.ori_ypr[self.v.state.istart:self.v.state.iend, state_axis], label='meas (Vicon)')
if show_imu_meas:
plt.plot(np.clip(self.v.imu.t[self.v.imu.istart:self.v.imu.iend], 0, np.inf),
imu_mult*self.v.imu.ori_ypr[self.v.imu.istart:self.v.imu.iend, state_axis], label='meas (IMU)')
if show_asctec_cmd and axis is not 'yaw':
plt.plot(self.v.asctec_ctrl_input.t[self.v.asctec_ctrl_input.istart:self.v.asctec_ctrl_input.iend],
asctec_cmd_mult*asctec_cmd[self.v.asctec_ctrl_input.istart:self.v.asctec_ctrl_input.iend],
label='cmd (AscTec)')
# Plot difference between vicon and imu: (broken, comment it out for now..)
# tout, data_out = uniform_resample((('linear', self.v.imu.t[self.v.asctec_ctrl_input.istart:self.v.asctec_ctrl_input.iend],
# self.v.imu.ori_ypr[self.v.asctec_ctrl_input.istart:self.v.asctec_ctrl_input.iend,state_axis]),
# ('linear', self.v.state.t[self.v.state.istart:self.v.state.iend],
# self.v.state.ori_ypr[self.v.state.istart:self.v.state.iend, state_axis])),
# 0.02)
# plt.plot(tout, imu_mult*data_out[0][0] - data_out[1][0], label='IMU - Vicon')
plt.legend()
self._timeseries_postplot()
def plot_axis_control(v, axis):
axismap = {'roll': (v.control.roll_cmd, 2, +1, v.asctec_ctrl_input.roll_cmd, -1),
'pitch': (v.control.pitch_cmd, 1, -1, v.asctec_ctrl_input.pitch_cmd, -1),
'yaw': (v.control.yaw_cmd, 0, -1, v.asctec_ctrl_input.yaw_rate_cmd, +1) # not sure about the multiplier here
}
control_mode_cmd, state_axis, imu_mult, asctec_cmd, asctec_cmd_mult = axismap[axis]
newfig("%s Axis Control" % axis.capitalize(), "time [s]", "%s [deg]" % axis.capitalize())
# np.clip() and the [1:] stuff in the following to attempt deal with bogus initial data points in IMU data:
plt.plot(v.control.t, control_mode_cmd, label='cmd (from mux)')
plt.plot(v.state.t[1:], v.state.ori_ypr[1:,state_axis], label='meas (Vicon)')
plt.plot(np.clip(v.imu.t[1:], 0, np.inf), imu_mult*v.imu.ori_ypr[1:,state_axis], label='meas (IMU)')
if axis is not 'yaw':
plt.plot(v.asctec_ctrl_input.t, asctec_cmd_mult*asctec_cmd, label='cmd (AscTec)')
# Plot difference between vicon and imu:
tout, data_out = uniform_resample((('linear', v.imu.t[1:], v.imu.ori_ypr[1:,state_axis]),
('linear', v.state.t[1:], v.state.ori_ypr[1:,state_axis])),
0.02)
plt.plot(tout, imu_mult*data_out[0][0] - data_out[1][0], label='IMU - Vicon')
plt.legend()
def plot_alt_control(v):
newfig("Altitude Control", "time [s]", "Alt [m]")
plt.plot(v.control.t, v.control.alt_cmd, label="cmd")
plt.plot(v.state.t[1:], v.state.up[1:], label="meas (Vicon)")
plt.legend()
def plot_r_histogram(self):
newfig('Histogram of sqrt(x^2 + y^2)', 'distance from origin [m]', 'Count')
t = self.v.state.t[self.v.state.istart:self.v.state.iend]
r = np.sqrt(self.v.state.north[self.v.state.istart:self.v.state.iend]**2 + \
self.v.state.east[self.v.state.istart:self.v.state.iend]**2 )
plt.hist(r,50)
def plot_north_vs_time(self):
newfig('North vs. Time', 'time [s]', 'North [m]')
plot_traj(self.v.state.t[self.v.state.istart:self.v.state.iend],
self.v.state.north[self.v.state.istart:self.v.state.iend], label="meas (Vicon)")
self._timeseries_postplot()
def plot_east_vs_time(self):
newfig('East vs. Time', 'time [s]', 'East [m]')
plot_traj(self.v.state.t[self.v.state.istart:self.v.state.iend],
self.v.state.east[self.v.state.istart:self.v.state.iend], label="meas (Vicon)")
self._timeseries_postplot()
def plot_side_view_north(self):
newfig('Side View (North)', 'North [m]', 'Up [m]', equal_axes=True)
plot_traj(self.v.state.north[self.v.state.istart:self.v.state.iend],
self.v.state.up[self.v.state.istart:self.v.state.iend], **TRAJ_PLOT_OPTIONS)
def plot_side_view_east(self):
newfig('Side View (East)', 'East [m]', 'Up [m]', equal_axes=True)
plot_traj(self.v.state.east[self.v.state.istart:self.v.state.iend],
self.v.state.up[self.v.state.istart:self.v.state.iend], **TRAJ_PLOT_OPTIONS)
def plot_top_view(self):
newfig('Top View', 'East [m]', 'North [m]', equal_axes=True)
plot_traj(self.v.state.east[self.v.state.istart:self.v.state.iend],
self.v.state.north[self.v.state.istart:self.v.state.iend], **TRAJ_PLOT_OPTIONS)
def plot_alt_control(self):
newfig("Altitude Control", "time [s]", "Alt [m]")
plt.plot(self.v.control.t[self.v.control.istart:self.v.control.iend], self.v.control.alt_cmd[self.v.control.istart:self.v.control.iend], label="cmd")
plt.plot(self.v.state.t[self.v.state.istart:self.v.state.iend], self.v.state.up[self.v.state.istart:self.v.state.iend], label="meas (Vicon)")
plt.legend()
self._timeseries_postplot()
