def calculate_nees(ex: MissionExperiment):
if len(ex.fp_msgs) < 5:
print('too few fp messages')
return
print('{} fp messages spanning {} seconds'.format(
len(ex.fp_msgs),
seconds(ex.fp_msgs[-1].header.stamp) -
seconds(ex.fp_msgs[0].header.stamp)))
if len(ex.gt_msgs) < 5:
print('too few ground truth messages')
return
print('{} ground truth messages spanning {} seconds'.format(
len(ex.gt_msgs),
seconds(ex.gt_msgs[-1].header.stamp) -
seconds(ex.gt_msgs[0].header.stamp)))
nees_values = nees_fp.nees(ex.fp_msgs, ex.gt_msgs)
if nees_values:
print('average NEES value {}'.format(np.mean(nees_values)))
else:
print('no NEES values')
def process_messages(ex: MissionExperiment):
if len(ex.co_msgs) < 2:
print('too few control messages, wrong topic?')
return
print('{} recorded {} control messages spanning {:.2f} seconds'.format(
ex.mission_info, len(ex.co_msgs), seconds(
ex.co_msgs[-1].header.stamp) - seconds(ex.co_msgs[0].header.stamp)))
x_kp = get_param_double(ex.auv_params, 'auv_x_pid_kp')
y_kp = get_param_double(ex.auv_params, 'auv_y_pid_kp')
z_kp = get_param_double(ex.auv_params, 'auv_z_pid_kp')
yaw_kp = get_param_double(ex.auv_params, 'auv_yaw_pid_kp')
times = [seconds(msg.header.stamp) for msg in ex.co_msgs]
x_values = [msg.estimate.pose.pose.position.x for msg in ex.co_msgs]
y_values = [msg.estimate.pose.pose.position.y for msg in ex.co_msgs]
z_values = [msg.estimate.pose.pose.position.z for msg in ex.co_msgs]
yaw_values = [get_yaw(msg.estimate.pose.pose.orientation) for msg in ex.co_msgs]
# co_msgs[0].mission.pose.fp.pose.pose is (0, 0, 0) (why?), select co_msgs[1]
x_kp, x_stop = adjust_kp(x_kp, ex.co_msgs[1].mission.pose.fp.pose.pose.position.x,
TIME_TO_CORRECT, False, times, x_values)
if x_stop:
print('x_kp is good at', x_kp)
else:
print('bump x_kp to', x_kp)
set_param_double(ex.auv_params, 'auv_x_pid_kp', x_kp)
y_kp, y_stop = adjust_kp(y_kp, ex.co_msgs[1].mission.pose.fp.pose.pose.position.y,
TIME_TO_CORRECT, False, times, y_values)
if y_stop:
print('y_kp is good at', y_kp)
else:
print('bump y_kp to', y_kp)
set_param_double(ex.auv_params, 'auv_y_pid_kp', y_kp)
z_kp, z_stop = adjust_kp(z_kp, ex.co_msgs[1].mission.pose.fp.pose.pose.position.z,
TIME_TO_CORRECT, False, times, z_values)
if z_stop:
print('z_kp is good at', z_kp)
else:
print('bump z_kp to', z_kp)
set_param_double(ex.auv_params, 'auv_z_pid_kp', z_kp)
yaw_kp, yaw_stop = adjust_kp(yaw_kp,
get_yaw(ex.co_msgs[1].mission.pose.fp.pose.pose.orientation),
TIME_TO_CORRECT, True, times, yaw_values)
if yaw_stop:
print('yaw_kp is good at', yaw_kp)
else:
print('bump yaw_kp to', yaw_kp)
set_param_double(ex.auv_params, 'auv_yaw_pid_kp', yaw_kp)
return x_stop and y_stop and z_stop and yaw_stop
def plot_msgs(self):
# Create a figure and 1 subplot
fig, (plot_pressure) = plt.subplots(1)
# X axis is seconds since the 1st message in the series
start_time = seconds(self._baro_msgs[0].header.stamp)
# For convenience Y axis is depth... but we don't have absolute depth w/o
# a calibrated air pressure. So, assume the mean filtered pressure is depth==0.
reference = statistics.mean(
[msg.pressure for msg in self._filtered_baro_msgs])
plot_pressure.set_title(
'Black is mean filtered pressure, magenta is target')
depth_x = [
seconds(msg.header.stamp) - start_time for msg in self._baro_msgs
]
depth_y = [depth(msg.pressure - reference) for msg in self._baro_msgs]
filt_depth_x = [
seconds(msg.header.stamp) - start_time
for msg in self._filtered_baro_msgs
]
filt_depth_y = [
depth(msg.pressure - reference) for msg in self._filtered_baro_msgs
]
plot_pressure.plot(depth_x, depth_y, label='depth')
plot_pressure.plot(filt_depth_x, filt_depth_y, label='filtered depth')
# Draw line at y=0
plot_pressure.axhline(y=0, color='k')
# Draw line at the target depth
if len(self._control_msgs) > 0:
plot_pressure.axhline(
y=depth(self._control_msgs[0].target_pressure - reference),
color='m')
plot_pressure.legend()
# Y limits +- 1cm
plot_pressure.set_ylim(-0.2, 0.2)
# set_ylim_with_min_range(plot_pressure, 0.15)
# Set figure title
fig.suptitle('{} messages'.format(self._num_messages))
# [Over]write PDF to disk
plt.savefig('plot_baro.pdf')
# Close the figure to reclaim the memory
plt.close(fig)
def plot(self):
print('plotting {}'.format(self._filename))
start_time = time.process_time()
# Create a figure with 8 subplots
fig, ((axx0, axx1, axx2, axx3), (axy0, axy1, axy2,
axy3)) = plt.subplots(2, 4)
subplots = [axx0, axx1, axx2, axx3, axy0, axy1, axy2, axy3]
names = ['x0', 'x1', 'x2', 'x3', 'y0', 'y1', 'y2', 'y3']
# x axis for all plots == time
# For most plots all all messages will be plotted
all_stamps = [seconds(msg.header.stamp) for msg in self._msgs]
# Warn on a large gap in timestamps -- this shouldn't happen with threading!
gaps = [
second - first
for first, second in zip(all_stamps[:-1], all_stamps[1:])
]
largest_gap = max(gaps)
if largest_gap > 0.1:
print('WARNING large time gap {:.2f}s'.format(largest_gap))
all_valuess = [[msg.observations[0].x0 for msg in self._msgs],
[msg.observations[0].x1 for msg in self._msgs],
[msg.observations[0].x2 for msg in self._msgs],
[msg.observations[0].x3 for msg in self._msgs],
[msg.observations[0].y0 for msg in self._msgs],
[msg.observations[0].y1 for msg in self._msgs],
[msg.observations[0].y2 for msg in self._msgs],
[msg.observations[0].y3 for msg in self._msgs]]
# Plot all corner values
for subplot, name, all_values in zip(subplots, names, all_valuess):
plot_value(subplot, all_stamps, all_values, name)
# Set figure title
fig.suptitle('{} messages, {}'.format(len(self._msgs), self._filename))
# Write the PDF file
plt.savefig(self._filename)
# Close the figure to reclaim the memory
plt.close(fig)
# Write some stats as well
means = [statistics.mean(all_values) for all_values in all_valuess]
stdevs = [
statistics.stdev(all_values, mean)
for all_values, mean in zip(all_valuess, means)
]
mean_stdev = statistics.mean(stdevs)
print('means =', means)
print('stdevs =', stdevs)
print('mean stdev =', mean_stdev)
stop_time = time.process_time()
print('finished {}, elapsed time {:.2f}s'.format(
self._filename, stop_time - start_time))
print()
def process(self, msg):
s = seconds(msg.header.stamp)
# Update first, last
if self._first_time is None or s < self._first_time:
self._first_time = s
if self._last_time is None or s > self._last_time:
self._last_time = s
# Plot messages?
if self._last_time - self._first_time > self._queue_for:
self.plot_msgs()
self.reset()
def process(self, msg):
s = seconds(msg.header.stamp)
# Update 1st, last
if self._first_time is None or s < self._first_time:
self._first_time = s
if self._last_time is None or s > self._last_time:
self._last_time = s
# Plot messages?
if self._last_time - self._first_time > QUEUE_FOR:
self.plot_msgs()
# Reset
self._first_time = None
self._last_time = None
self._depth_msgs: List[Depth] = []
self._pre_msgs: List[PoseWithCovarianceStamped] = []
self._post_msgs: List[Odometry] = []
self._gt_msgs: List[Odometry] = []
def plot(self):
if len(self._control_msgs) < 2:
print('WARNING {} has too few messages! skipping'.format(
self._filename))
return
print('plotting {}'.format(self._filename))
start_time = time.process_time()
# Create a figure and 16 subplots:
# 4 for velocity
# 4 for plan vs est
# 4 for error in
# 4 for efforts
fig, ((axvx, axvy, axvz, axvw), (axpx, axpy, axpz, axpw),
(axex, axey, axez, axew), (axff, axfs, axfv,
axfw)) = plt.subplots(4, 4)
# x axis for all plots == time
# For most plots all all messages will be plotted
all_stamps = [seconds(msg.header.stamp) for msg in self._control_msgs]
# Warn on a large gap in timestamps -- this shouldn't happen with threading!
gaps = [
second - first
for first, second in zip(all_stamps[:-1], all_stamps[1:])
]
largest_gap = max(gaps)
if largest_gap > 0.1:
print('WARNING large time gap {:.2f}s'.format(largest_gap))
# For pose-based segments plot x, y, z, yaw
# For observation-based segments plot distance, z and bearing
if self._control_msgs[0].mission.segment_type == MissionState.OBS_RTM or \
self._control_msgs[0].mission.segment_type == MissionState.OBS_MTM:
# We expect 1 planned observation -- the marker we're tracking
# The number of observations in estimate may vary from 0 to the total number of markers
plan_obs_sizes = [
len(msg.mission.pose.fp.observations.observations)
for msg in self._control_msgs
]
est_obs_sizes = [
len(msg.estimate.observations.observations)
for msg in self._control_msgs
]
fewest_plan = min(plan_obs_sizes)
most_plan = max(plan_obs_sizes)
fewest_est = min(est_obs_sizes)
most_est = max(est_obs_sizes)
if fewest_plan != 1 or most_plan != 1:
print(
'ERROR expected 1 planned observation, found min {}, max {}'
.format(fewest_plan, most_plan))
return
print('len(estimate_observations) varies from {} to {}'.format(
fewest_est, most_est))
# The marker we're tracking
marker_id = self._control_msgs[
0].mission.pose.fp.observations.observations[0].id
# We may not find the marker in msg.estimate.observations,
# so the # of data points may be smaller
est_obs_stamps = []
est_obs_distance_values = []
est_obs_bearing_values = []
error_distance_values = []
error_bearing_values = []
for msg in self._control_msgs:
# Get distance and bearing to the planned observation
plan_distance = msg.mission.pose.fp.observations.polar_observations[
0].distance
plan_bearing = msg.mission.pose.fp.observations.polar_observations[
0].bearing
# Try to find the marker in the estimate
polar_obs: PolarObservation # Type hint
for polar_obs in msg.estimate.observations.polar_observations:
if polar_obs.id == marker_id:
est_obs_stamps.append(seconds(msg.header.stamp))
est_obs_distance_values.append(polar_obs.distance)
est_obs_bearing_values.append(polar_obs.bearing)
error_distance_values.append(polar_obs.distance -
plan_distance)
error_bearing_values.append(polar_obs.bearing -
plan_bearing)
# Plot velocity data
# No planned velocity for observations
plot_velo(
axvz, 'velo z', all_stamps,
[msg.mission.twist.linear.z for msg in self._control_msgs])
# Plot pose data
plot_pose(axpx, 'obs distance', 'plan distance', 'est distance',
all_stamps, est_obs_stamps, [
msg.mission.pose.fp.observations.
polar_observations[0].distance
for msg in self._control_msgs
], est_obs_distance_values)
plot_pose(
axpz, 'pose z', 'plan z', 'est z', all_stamps, all_stamps,
[msg.mission.pose.position.z for msg in self._control_msgs],
[msg.estimate.pose.position.z for msg in self._control_msgs])
plot_pose(axpw, 'obs bearing', 'plan bearing', 'est bearing',
all_stamps, est_obs_stamps, [
msg.mission.pose.fp.observations.
polar_observations[0].bearing
for msg in self._control_msgs
], est_obs_bearing_values)
# Plot error data
plot_error(axex, 'error distance', est_obs_stamps,
error_distance_values)
plot_error(axez, 'error z', all_stamps, [
msg.estimate.pose.position.z - msg.mission.pose.position.z
for msg in self._control_msgs
])
plot_error(axew, 'error bearing', est_obs_stamps,
error_bearing_values)
else:
# Plot velocity data
plot_velo(
axvx, 'velo x', all_stamps,
[msg.mission.twist.linear.x for msg in self._control_msgs])
plot_velo(
axvy, 'velo y', all_stamps,
[msg.mission.twist.linear.y for msg in self._control_msgs])
plot_velo(
axvz, 'velo z', all_stamps,
[msg.mission.twist.linear.z for msg in self._control_msgs])
plot_velo(
axvw, 'velo yaw', all_stamps,
[msg.mission.twist.angular.z for msg in self._control_msgs])
# Plot pose data
plot_pose(axpx, 'pose x', 'plan x', 'est x', all_stamps,
all_stamps, [
msg.mission.pose.fp.pose.pose.position.x
for msg in self._control_msgs
], [
msg.estimate.pose.pose.position.x
for msg in self._control_msgs
])
plot_pose(axpy, 'pose y', 'plan y', 'est y', all_stamps,
all_stamps, [
msg.mission.pose.fp.pose.pose.position.y
for msg in self._control_msgs
], [
msg.estimate.pose.pose.position.y
for msg in self._control_msgs
])
plot_pose(axpz, 'pose z', 'plan z', 'est z', all_stamps,
all_stamps, [
msg.mission.pose.fp.pose.pose.position.z
for msg in self._control_msgs
], [
msg.estimate.pose.pose.position.z
for msg in self._control_msgs
])
plot_pose(axpw, 'pose yaw', 'plan yaw', 'est yaw', all_stamps,
all_stamps, [
get_yaw(msg.mission.pose.fp.pose.pose.orientation)
for msg in self._control_msgs
], [
get_yaw(msg.estimate.pose.pose.orientation)
for msg in self._control_msgs
])
# Plot error data
plot_error(axex, 'error x', all_stamps, [
msg.estimate.pose.pose.position.x -
msg.mission.pose.fp.pose.pose.position.x
for msg in self._control_msgs
])
plot_error(axey, 'error y', all_stamps, [
msg.estimate.pose.pose.position.x -
msg.mission.pose.fp.pose.pose.position.y
for msg in self._control_msgs
])
plot_error(axez, 'error z', all_stamps, [
msg.estimate.pose.pose.position.x -
msg.mission.pose.fp.pose.pose.position.z
for msg in self._control_msgs
])
plot_error(axew, 'error yaw', all_stamps, [
norm_angle(
get_yaw(msg.estimate.pose.pose.orientation) -
get_yaw(msg.mission.pose.fp.pose.pose.orientation))
for msg in self._control_msgs
])
# Plot effort data
plot_effort(axff, 'forward', all_stamps,
[msg.efforts.forward for msg in self._control_msgs])
plot_effort(axfs, 'strafe', all_stamps,
[msg.efforts.strafe for msg in self._control_msgs])
plot_effort(axfv, 'vertical', all_stamps,
[msg.efforts.vertical for msg in self._control_msgs])
plot_effort(axfw, 'yaw', all_stamps,
[msg.efforts.yaw for msg in self._control_msgs])
# Set figure title
fig.suptitle('{} messages, {}'.format(len(self._control_msgs),
self._filename))
# Write the PDF file twice:
# -- once with a unique name for later analysis
# -- again with a repeated name so I can watch the plots live
plt.savefig(self._filename)
plt.savefig('plot_auv_segments.pdf')
# Close the figure to reclaim the memory
plt.close(fig)
stop_time = time.process_time()
print('finished {}, elapsed time {:.2f}s'.format(
self._filename, stop_time - start_time))
def plot_msgs(self):
"""Plot queued messages."""
# Convert quaternions to Euler angles
pre_pose_rpys = [
q_to_rpy(pre.pose.pose.orientation) for pre in self._pre_msgs
]
post_pose_rpys = [
q_to_rpy(post.pose.pose.orientation) for post in self._post_msgs
]
gt_pose_rpys = [
q_to_rpy(gt.pose.pose.orientation) for gt in self._gt_msgs
]
# Create a figure and 12 subplots, 6 for the pose and 6 for the twist
fig, ((axpx, axpy, axpz), (axtx, axty, axtz),
(axproll, axppitch, axpyaw), (axtroll, axtpitch,
axtyaw)) = plt.subplots(4, 3)
# Build lists of items to plot
depth_xs = [seconds(msg.header.stamp)
for msg in self._depth_msgs] # Depth messages
pre_xs = [seconds(msg.header.stamp)
for msg in self._pre_msgs] # Pre-filter pose messages
post_xs = [seconds(msg.header.stamp)
for msg in self._post_msgs] # Pose-filter pose messages
gt_xs = [seconds(msg.header.stamp)
for msg in self._gt_msgs] # Ground truth messages
subplots = [
axpx, axpy, axpz, axtx, axty, axtz, axproll, axppitch, axpyaw,
axtroll, axtpitch, axtyaw
]
names = [
'x', 'y', 'z', 'vx', 'vy', 'vz', 'roll', 'pitch', 'yaw', 'v roll',
'v pitch', 'v yaw'
]
depth_valuess = [
None, None, [msg.z for msg in self._depth_msgs], None, None, None,
None, None, None, None, None, None
]
depth_sdss = [
None, None,
[math.sqrt(msg.z_variance) for msg in self._depth_msgs], None,
None, None, None, None, None, None, None, None
]
pre_valuess = [[msg.pose.pose.position.x for msg in self._pre_msgs],
[msg.pose.pose.position.y for msg in self._pre_msgs],
[msg.pose.pose.position.z for msg in self._pre_msgs],
None, None, None, [rpy[0] for rpy in pre_pose_rpys],
[rpy[1] for rpy in pre_pose_rpys],
[rpy[2] for rpy in pre_pose_rpys], None, None, None]
pre_sdss = [[
math.sqrt(msg.pose.covariance[diag_index(0)])
for msg in self._pre_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(1)])
for msg in self._pre_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(2)])
for msg in self._pre_msgs
], None, None, None,
[
math.sqrt(msg.pose.covariance[diag_index(3)])
for msg in self._pre_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(4)])
for msg in self._pre_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(5)])
for msg in self._pre_msgs
], None, None, None]
post_valuess = [[msg.pose.pose.position.x for msg in self._post_msgs],
[msg.pose.pose.position.y for msg in self._post_msgs],
[msg.pose.pose.position.z for msg in self._post_msgs],
[msg.twist.twist.linear.x for msg in self._post_msgs],
[msg.twist.twist.linear.y for msg in self._post_msgs],
[msg.twist.twist.linear.z for msg in self._post_msgs],
[rpy[0] for rpy in post_pose_rpys],
[rpy[1] for rpy in post_pose_rpys],
[rpy[2] for rpy in post_pose_rpys],
[msg.twist.twist.angular.x for msg in self._post_msgs],
[msg.twist.twist.angular.y for msg in self._post_msgs],
[msg.twist.twist.angular.z for msg in self._post_msgs]]
post_sdss = [[
math.sqrt(msg.pose.covariance[diag_index(0)])
for msg in self._post_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(1)])
for msg in self._post_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(2)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(0)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(1)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(2)])
for msg in self._post_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(3)])
for msg in self._post_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(4)])
for msg in self._post_msgs
],
[
math.sqrt(msg.pose.covariance[diag_index(5)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(3)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(4)])
for msg in self._post_msgs
],
[
math.sqrt(msg.twist.covariance[diag_index(5)])
for msg in self._post_msgs
]]
gt_valuess = [[msg.pose.pose.position.x for msg in self._gt_msgs],
[msg.pose.pose.position.y for msg in self._gt_msgs],
[msg.pose.pose.position.z for msg in self._gt_msgs],
[msg.twist.twist.linear.x for msg in self._gt_msgs],
[msg.twist.twist.linear.y for msg in self._gt_msgs],
[msg.twist.twist.linear.z for msg in self._gt_msgs],
[rpy[0] for rpy in gt_pose_rpys],
[rpy[1] for rpy in gt_pose_rpys],
[rpy[2] for rpy in gt_pose_rpys],
[msg.twist.twist.angular.x for msg in self._gt_msgs],
[msg.twist.twist.angular.y for msg in self._gt_msgs],
[msg.twist.twist.angular.z for msg in self._gt_msgs]]
# Plot everything
for subplot, name, depth_values, depth_sds, pre_values, pre_sds, post_values, post_sds, \
gt_values in zip(subplots, names, depth_valuess, depth_sdss, pre_valuess, pre_sdss,
post_valuess, post_sdss, gt_valuess):
plot_subplot(subplot, name, depth_xs, depth_values, depth_sds,
pre_xs, pre_values, pre_sds, post_xs, post_values,
post_sds, gt_xs, gt_values)
# Calc average NEES
ave_nees = self.calc_nees()
nees_str = ''
if ave_nees < 0:
nees_str = 'no estimates'
elif ave_nees < 12:
nees_str = 'ave NEES {:.3f}, SUCCESS'.format(ave_nees)
else:
nees_str = 'ave NEES {:.3f}, FAILURE'.format(ave_nees)
# Set figure title
fig.suptitle(
'UKF status, {} second(s), with (mean, stddev), {}'.format(
QUEUE_FOR, nees_str))
# [Over]write PDF to disk
plt.savefig('plot_filter.pdf')
# Close the figure to reclaim the memory
plt.close(fig)
def plot_msgs(self):
"""Plot queued messages."""
print('plotting plot_filter.pdf')
start_time = time.process_time()
# Convert quaternions to Euler angles
pre_pose_rpys = [q_to_rpy(msg.fp.pose.pose.orientation) for msg in self._pre_msgs]
post_pose_rpys = [q_to_rpy(msg.fp.pose.pose.orientation) for msg in self._post_msgs]
gt_pose_rpys = [q_to_rpy(msg.pose.pose.orientation) for msg in self._gt_msgs]
plan_pose_rpys = [q_to_rpy(msg.mission.pose.fp.pose.pose.orientation)
for msg in self._control_msgs]
# Create a figure and 6 subplots
fig, ((axpx, axpy, axpz), (axproll, axppitch, axpyaw)) = plt.subplots(2, 3)
# X values are seconds from the first message
depth_xs = [seconds(msg.header.stamp) - self._first_time for msg in self._depth_msgs]
pre_xs = [seconds(msg.header.stamp) - self._first_time for msg in self._pre_msgs]
post_xs = [seconds(msg.header.stamp) - self._first_time for msg in self._post_msgs]
gt_xs = [seconds(msg.header.stamp) - self._first_time for msg in self._gt_msgs]
plan_xs = [seconds(msg.header.stamp) - self._first_time for msg in self._control_msgs]
subplots = [axpx, axpy, axpz, axproll, axppitch, axpyaw]
names = ['x', 'y', 'z', 'roll', 'pitch', 'yaw']
# Build 6 lists of y values, 1 for each subplot
depth_valuess = [None, None, [msg.z for msg in self._depth_msgs],
None, None, None]
depth_sdss = [None, None, [math.sqrt(msg.z_variance) for msg in self._depth_msgs],
None, None, None]
pre_valuess = [[msg.fp.pose.pose.position.x for msg in self._pre_msgs],
[msg.fp.pose.pose.position.y for msg in self._pre_msgs],
[msg.fp.pose.pose.position.z for msg in self._pre_msgs],
[rpy[0] for rpy in pre_pose_rpys],
[rpy[1] for rpy in pre_pose_rpys],
[rpy[2] for rpy in pre_pose_rpys]]
pre_sdss = [[math.sqrt(msg.fp.pose.covariance[diag_index(0)]) for msg in self._pre_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(1)]) for msg in self._pre_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(2)]) for msg in self._pre_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(3)]) for msg in self._pre_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(4)]) for msg in self._pre_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(5)]) for msg in self._pre_msgs]]
post_valuess = [[msg.fp.pose.pose.position.x for msg in self._post_msgs],
[msg.fp.pose.pose.position.y for msg in self._post_msgs],
[msg.fp.pose.pose.position.z for msg in self._post_msgs],
[rpy[0] for rpy in post_pose_rpys],
[rpy[1] for rpy in post_pose_rpys],
[rpy[2] for rpy in post_pose_rpys]]
post_sdss = [[math.sqrt(msg.fp.pose.covariance[diag_index(0)]) for msg in self._post_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(1)]) for msg in self._post_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(2)]) for msg in self._post_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(3)]) for msg in self._post_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(4)]) for msg in self._post_msgs],
[math.sqrt(msg.fp.pose.covariance[diag_index(5)]) for msg in self._post_msgs]]
gt_valuess = [[msg.pose.pose.position.x for msg in self._gt_msgs],
[msg.pose.pose.position.y for msg in self._gt_msgs],
[msg.pose.pose.position.z for msg in self._gt_msgs],
[rpy[0] for rpy in gt_pose_rpys],
[rpy[1] for rpy in gt_pose_rpys],
[rpy[2] for rpy in gt_pose_rpys]]
plan_valuess = [[msg.mission.pose.fp.pose.pose.position.x for msg in self._control_msgs],
[msg.mission.pose.fp.pose.pose.position.y for msg in self._control_msgs],
[msg.mission.pose.fp.pose.pose.position.z for msg in self._control_msgs],
None,
None,
[rpy[2] for rpy in plan_pose_rpys]]
# Plot everything
for subplot, name, depth_values, depth_sds, pre_values, pre_sds, post_values, post_sds, \
gt_values, plan_values in zip(subplots, names, depth_valuess, depth_sdss, pre_valuess,
pre_sdss, post_valuess, post_sdss, gt_valuess,
plan_valuess):
plot_subplot(subplot, name,
depth_xs, depth_values, depth_sds,
pre_xs, pre_values, pre_sds,
post_xs, post_values, post_sds,
gt_xs, gt_values, plan_xs, plan_values)
# Calc average NEES
ave_nees = self.calc_nees()
if ave_nees < 0:
nees_str = 'no estimates'
elif ave_nees < 12:
nees_str = 'ave NEES {:.3f}, SUCCESS'.format(ave_nees)
else:
nees_str = 'ave NEES {:.3f}, FAILURE'.format(ave_nees)
# Set figure title
fig.suptitle(
'UKF status, {} second(s), with (mean, stddev), {}'.format(self._queue_for, nees_str))
# [Over]write PDF to disk
plt.savefig('plot_filter.pdf')
# Close the figure to reclaim the memory
plt.close(fig)
# If elapsed time > 0.3s we're may be dropping messages
# Add a plotting thread to fix
stop_time = time.process_time()
print('elapsed time {:.2f}s'.format(stop_time - start_time))
def process_messages(ex: MissionExperiment, logger):
if len(ex.co_msgs) < 2:
logger.info('too few control messages, wrong topic?')
return
logger.info(
'{} recorded {} control messages spanning {:.2f} seconds'.format(
ex.mission_info, len(ex.co_msgs),
seconds(ex.co_msgs[-1].header.stamp) -
seconds(ex.co_msgs[0].header.stamp)))
if ex.mission_info == EXPERIMENT_F:
plan_run_v = auv_xy_velo
plan_drag_coef = drag_coef_f
elif ex.mission_info == EXPERIMENT_S:
plan_run_v = auv_xy_velo
plan_drag_coef = drag_coef_s
elif ex.mission_info == EXPERIMENT_Z:
plan_run_v = auv_z_velo
plan_drag_coef = drag_coef_z
else:
plan_run_v = auv_yaw_velo
plan_drag_coef = drag_coef_yaw
msg: Control
msg_start: Optional[Control] = None
current_phase = MissionState.PHASE_NONE
for msg in ex.co_msgs:
if msg.mission.phase != current_phase:
# Phase changed
current_phase = msg.mission.phase
if msg.mission.phase == MissionState.PHASE_TRAP_CONSTANT_V:
# Starting run (constant velocity) phase
msg_start = msg
elif msg.mission.phase == MissionState.PHASE_TRAP_DECEL:
# Starting deceleration phase, which means the run phase is over, or didn't happen
if msg_start is None:
# There was no run phase
logger.info('no run phase, skipping segment')
else:
# Run time
run_dt = seconds(msg.header.stamp) - seconds(
msg_start.header.stamp)
# Run distance
if ex.mission_info == EXPERIMENT_F:
actual_run_d = abs(
msg.estimate.pose.pose.position.x -
msg_start.estimate.pose.pose.position.x)
elif ex.mission_info == EXPERIMENT_S:
actual_run_d = abs(
msg.estimate.pose.pose.position.y -
msg_start.estimate.pose.pose.position.y)
elif ex.mission_info == EXPERIMENT_Z:
actual_run_d = abs(
msg.estimate.pose.pose.position.z -
msg_start.estimate.pose.pose.position.z)
else:
actual_run_d = abs(
norm_angle(
get_yaw(msg.estimate.pose.pose.orientation) -
get_yaw(
msg_start.estimate.pose.pose.orientation)))
# Run velocity
actual_run_v = actual_run_d / run_dt
# The plan depends on the thrust force parameters and the drag coefficient
# parameters. If we assume that the thrust force parameters are accurate, we
# can use the difference between planned & actual velocity to calculate new
# drag coefficients.
actual_drag_coef = plan_drag_coef * pow(
plan_run_v / actual_run_v, 2)
logger.info((
'run phase dt {:.2f}, plan v {:.2f}, actual v {:.2f}, '
'actual d {:.3f}, plan drag {:.2f}, actual drag {:.2f}'
).format(run_dt, plan_run_v, actual_run_v, actual_run_d,
plan_drag_coef, actual_drag_coef))
# Reset
msg_start = None