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()