def PlotAccelerometer(self, e, c, s, params): mplot.PlotVec3(c['time'], c['control_input']['imus']['acc'][:, 0], linestyle='-', label='A') mplot.PlotVec3(c['time'], c['control_input']['imus']['acc'][:, 1], linestyle=':', label='B') mplot.PlotVec3(c['time'], c['control_input']['imus']['acc'][:, 2], linestyle='-.', label='C')
def PlotSpecificForce(self, e, c, s, params): mplot.PlotVec3(c['time'], c['estimator']['acc_b_estimates'][:, 0], linestyle='-', label='A') mplot.PlotVec3(c['time'], c['estimator']['acc_b_estimates'][:, 1], linestyle=':', label='B') mplot.PlotVec3(c['time'], c['estimator']['acc_b_estimates'][:, 2], linestyle='-.', label='C')
def PlotMotorMoments(self, ti, c, s): mplot.PlotVec3(c['time'], c['thrust_moment_avail']['moment'], labels=['Roll Avail', 'Pitch Avail', 'Yaw Avail']) mplot.PlotVec3(c['time'], c['thrust_moment']['moment'], linestyle=':', labels=['Roll Cmd', 'Pitch Cmd', 'Yaw Cmd'])
def PlotEulerAngleError(self, ti, c, s): mplot.PlotVec3(c['time'], ti['eulers_ti2b'], scale=180.0 / np.pi, labels=['Roll', 'Pitch', 'Yaw']) mplot.PlotVec3(c['time'], ti['eulers_ti2cmd'], scale=180.0 / np.pi, labels=['Roll Cmd', 'Pitch Cmd', 'Yaw Cmd'], linestyle=':')
def PlotGroundEstimatePosition(self, ci, c, s): mplot.PlotVec3(c['time'], ci['ground_estimate']['Xg'], label='vessel_pos_g [estimate]') if s is not None: # Note: These can only be expected to agree if the simulator and # vessel estimator agree on the origin of the g-frame. That's a # good reason to standardize the ned origin and express these in the # ned frame. mplot.PlotVec3(s['time'], s['buoy']['Xg'], label='vessel_pos_g [sim]', linestyle='--')
def _PlotGpsVelocityEcef(self, time, vel_ecef, vel_sigma_ecef, label): dims = ['x', 'y', 'z'] vel = {d: vel_ecef[d] for d in dims} labels = ['%s %s ECEF' % (label, d) for d in dims] mplot.PlotVec3(time, vel, labels=labels) mplot.PlotVec3(time, {d: vel[d] + vel_sigma_ecef[d] for d in dims}, linestyle=':', labels=[None for d in dims]) mplot.PlotVec3(time, {d: vel[d] - vel_sigma_ecef[d] for d in dims}, linestyle=':', labels=[None for d in dims])
def _PlotGpsPositionEcef(self, time, pos_ecef, pos_sigma_ecef, label): dims = ['x', 'y', 'z'] pos_offset = {d: np.median(pos_ecef[d]) for d in dims} pos = {d: pos_ecef[d] - pos_offset[d] for d in dims} labels = ['%s %s ECEF -(%g)' % (label, d, pos_offset[d]) for d in dims] mplot.PlotVec3(time, pos, labels=labels) mplot.PlotVec3(time, {d: pos[d] + pos_sigma_ecef[d] for d in dims}, linestyle=':', labels=[None for d in dims]) mplot.PlotVec3(time, {d: pos[d] - pos_sigma_ecef[d] for d in dims}, linestyle=':', labels=[None for d in dims])
def PlotVesselPosition(self, g, s, params): mplot.PlotVec3(g['estimate']['time'], g['estimate']['Xg'], label='[est]', linestyle='--') mplot.PlotVec3(g['estimate']['time'], g['estimator']['gps']['Xg'], label='GPS (adjusted)', linestyle=':') if s is not None: mplot.PlotVec3(s['time'], s['buoy']['Xg'], label='[sim]', linestyle='-')
def PlotMagnetometer(self, g, s, params): mplot.PlotVec3(g['estimate']['time'], g['input']['imu']['mag'], label='mag') plot(g['estimate']['time'], numpy_utils.Vec3Norm(g['input']['imu']['mag']), 'k--', label='mag norm')
def PlotAccBiases(self, e, c, s, params, imu_index=0): mplot.PlotVec3(c['time'], e['acc_b_estimates'][:, imu_index], label='IMU %d' % imu_index)
def PlotBodyRates(self, e, c, s, params): mplot.PlotVec3(c['time'], c['state_est']['pqr_f'])
def PlotFilteredVelocity(self, e, c, s, params): mplot.PlotVec3(c['time'], c['state_est']['Vg'], label='Vg', linestyle='-') mplot.PlotVec3(c['time'], c['state_est']['Vg_f'], label='Vg_f', linestyle='-.') if s is not None: mplot.PlotVec3(s['time'], s['wing']['Vg'], label='sim', linestyle=':')
def PlotBodyRates(self, ti, c, s): labels = ['Roll Rate', 'Pitch Rate', 'Yaw Rate'] mplot.PlotVec3(c['time'], c['state_est']['pqr_f'], labels=labels) mplot.PlotVec3(c['time'], ti['pqr_cmd'], labels=labels, linestyle=':') plot(c['time'], ti['pitch_rate_b_cmd'], 'k:', label='Pitch Rate Cmd')
def PlotAttitudeError(self, ti, c, s): labels = ['Roll Error', 'Pitch Error', 'Yaw Error'] mplot.PlotVec3(c['time'], ti['axis_b2cmd'], scale=180.0 / np.pi, labels=labels)
def PlotGpsBias(self, e, c, s, params): mplot.PlotVec3(c['time'], e['Xg_gps_biases'][:, 0], label='GPS A bias') mplot.PlotVec3(c['time'], e['Xg_gps_biases'][:, 1], label='GPS B bias')
def PlotTetherAnchorPoint(self, e, c, s, params): mplot.PlotVec3(c['time'], c['state_est']['tether_anchor']['pos_g'], label='pos_g [est]', linestyle='-') mplot.PlotVec3(c['time'], c['state_est']['tether_anchor']['pos_g_f'], label='pos_g_f [est]', linestyle='--')
def PlotGsGpsPositionSigmas(self, g, s, params): mplot.PlotVec3(g['estimate']['time'], g['input']['gs_gps']['pos_sigma'])
def PlotGyros(self, e, c, s, params, imu_index=0): mplot.PlotVec3(c['time'], c['control_input']['imus']['gyro'][:, imu_index])
def PlotAccelerometer(self, g, s, params): mplot.PlotVec3(g['estimate']['time'], g['input']['imu']['acc'], label='accelerometer')
def PlotGyroBiases(self, e, c, s, params, imu_index=0): mplot.PlotVec3(c['time'], e['gyro_biases'][:, imu_index], label='IMU %d' % imu_index) if s is not None: mplot.PlotVec3(s['time'], s['imus']['gyro_bias_b'][:, imu_index], linestyle=':')
def PlotGyro(self, g, s, params): mplot.PlotVec3(g['estimate']['time'], g['input']['imu']['gyro'], label='gyro')
def PlotBodyRates(self, cw, c, s): mplot.PlotVec3(c['time'], cw['pqr_cmd'], linestyle='--', label='pqr_cmd') mplot.PlotVec3(c['time'], c['state_est']['pqr_f'], label='pqr_f')
def PlotWingGpsVelocitySigmas(self, ci, c, s): for i in range(control_types.kNumWingGpsReceivers): mplot.PlotVec3(c['time'], ci['wing_gps']['vel_sigma'][:, i], label=_WING_GPS_HELPER.ShortName(i))