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