def _calculate_brake_event_fit(self,
window_size=55,
braking_threshold=0.1,
min_size=75):
t = self.bicycle['time']
v = self.bicycle['speed']
filtered_velocity = ff.moving_average(v, window_size, window_size / 2)
filtered_acceleration = ff.moving_average(
self.bicycle['accelerometer x'], window_size, window_size / 2)
braking_slice = braking.get_trial_braking_indices(
filtered_acceleration, braking_threshold, min_size)[0]
# determine if wheel lockup occurs
lockup_mask = ((v[braking_slice] < 0.2) &
(filtered_acceleration[braking_slice] > 2.5))
# best-fit line metrics
slope, intercept, r_value, p_value, stderr = scipy.stats.linregress(
t[braking_slice][~lockup_mask], v[braking_slice][~lockup_mask])
fitparams = BrakeEventLinearFitParameters(
window_size, braking_threshold, min_size, t, filtered_velocity,
filtered_acceleration, braking_slice, lockup_mask, slope,
intercept, r_value, p_value, stderr)
return fitparams
def get_metrics(rec, window_size=55):
t = rec['time']
steer = rec['steer angle']
filt_steer, _, lowcut, _, _ = filtered_steer(rec)
filt_steer -= steer.mean()
mod_steer = steer - steer.mean()
error = filt_steer - mod_steer
event_groups = get_steer_event_indices(filt_steer)
first_turn = True
for event_range in reversed(event_groups):
for r0, r1 in event_range:
sum_error = sum(error[r0:r1])
sum_filt = sum(filt_steer[r0:r1])
# if error ratio is too high, discard steering event
if sum_error / sum_filt < ERROR_RATIO:
if first_turn and len(event_range) > 1:
first_turn = False
amplitude = np.abs(filt_steer[r0:r1]).max()
period = 2 * (t[r1] - t[r0])
vf = ff.moving_average(rec['speed'], window_size,
window_size / 2)
v0 = vf[r0]
assert v0 > 1.0, 'velocity is too low'
break
if first_turn:
break
assert first_turn, 'turn not detected'
return np.array([(amplitude, period, v0, (r0, r1), lowcut, 0, 0)],
dtype=metrics_dtype)
def plot_rider_velocities(recs, rider_id, **kwargs):
fig, axes = plt.subplots(2, 2, **kwargs)
axes = axes.ravel()
colors = sns.color_palette('Paired', 8)
for rid, tid, r in recs:
if rider_id != rid:
continue
t = r['time']
ws = 55 # window size of samples -> 0.44 seconds @ 125 Hz
vf = ff.moving_average(r['speed'], ws, ws/2)
af = ff.moving_average(r['accelerometer x'], ws, ws/2)
vc = colors[1]
ac = colors[3]
ax = axes[tid - 1]
ax.plot(t, vf, label='velocity, gaussian weighted moving average',
color=vc)
ax.plot(t, af, label='acceleration, gaussian weighted moving average',
color=ac)
ax.legend()
ylim = ax.get_ylim()
ax.plot(t, r['speed'], color=vc, alpha=0.3)
ax.plot(t, r['accelerometer x'], color=ac, alpha=0.3)
ax.set_ylim(ylim)
ax.set_title('rider {} trial {}'.format(rid, tid))
ax.set_ylabel('m/s, -m/s^2')
ax.set_xlabel('time [s]')
ax.axhline(0, color=sns.xkcd_palette(['charcoal'])[0],
linewidth=1,zorder=1)
largest_range, all_ranges = get_trial_braking_indices(af)
for clump in all_ranges:
ax.axvspan(t[clump.start],
t[clump.stop - 1],
color=colors[5], alpha=0.2)
if largest_range is not None:
ax.axvspan(t[largest_range.start],
t[largest_range.stop - 1],
color=colors[5], alpha=0.4)
return fig, axes
def get_metrics(trial, window_size=55, braking_threshold=0.3, min_size=15,
use_kalman=False):
""" window size in samples
ws = 55 # window size of samples -> 0.44 seconds @ 125 Hz
use_kalman: bool, use smoothed kalman filter speed estimate
"""
t = trial.data['time']
if use_kalman:
v = np.squeeze(trial.kalman_smoothed_result.state_estimate[:, 3])
filtered_velocity = v
else:
v = trial.data['speed']
filtered_velocity = ff.moving_average(v,
window_size, window_size/2)
filtered_acceleration = ff.moving_average(trial.data['accelerometer x'],
window_size, window_size/2)
braking_range = get_trial_braking_indices(filtered_acceleration,
braking_threshold,
min_size)[0]
try:
b0, b1 = braking_range = get_trial_braking_indices(filtered_acceleration,
braking_threshold,
min_size)[0]
except TypeError:
# handle 'slice' object
b0, b1 = braking_range = (braking_range.start, braking_range.stop)
b1 -= 1
# determine if wheel lockup occurs
# look at raw speed and filtered acceleration
lockup_indices = np.where((v < 0.2) &
(filtered_acceleration > 2.5))[0]
lockup_ranges = util.get_contiguous_numbers(lockup_indices)
# calculate braking indices by removing lockup ranges
br = set(range(b0, b1))
for l0, l1 in lockup_ranges:
br -= set(range(l0, l1))
braking_indices = list(br)
# best-fit line metrics
slope, intercept, r_value, p_value, stderr = scipy.stats.linregress(
t[braking_indices], v[braking_indices])
# braking metrics
start_velocity = filtered_velocity[b0]
braking_duration = t[b1] - t[b0]
# TODO what if best-fit line crosses zero?
braking_distance = (np.polyval([slope, intercept], [t[b0], t[b1]]).mean() *
braking_duration)
braking_starttime = t[b0]
braking_endtime = t[b1]
# TODO do filtering in another function and pass filtered signals to this
# function to calculate metrics
return (np.array([(slope,
intercept,
r_value,
p_value,
stderr,
start_velocity,
braking_duration,
braking_distance,
braking_starttime,
braking_endtime,
window_size,
braking_range,
len(lockup_ranges),
0,
0)], dtype=metrics_dtype),
filtered_velocity,
filtered_acceleration,
lockup_ranges)
def plot_trajectory(self, ax=None, plot_vel=True, bbmask=None, **fig_kw):
if plot_vel:
num_plots = 2
else:
num_plots = 1
if ax is None:
fig, ax = plt.subplots(num_plots, 1, sharex=False, **fig_kw)
else:
assert len(ax) == num_plots
fig = ax[0].get_figure()
if not plot_vel:
ax = [ax]
colors = sns.color_palette('Paired', 12)
# stationary points
ax[0].scatter(self.x.data[self.stationary_mask & ~self.bb_mask],
self.y.data[self.stationary_mask & ~self.bb_mask],
s=5,
marker='x',
color='black',
label='stationary points')
x = self.x.copy()
y = self.y.copy()
x.mask = self.stationary_mask | self.bb_mask
y.mask = self.stationary_mask | self.bb_mask
# non-stationary points
if bbmask is None:
ax[0].scatter(x,
y,
s=3,
marker='.',
color=colors[1],
label='non-stationary points')
else:
mask = _apply_bbmask(bbmask, x, y, apply_mask=False)
ax[0].scatter(x[~mask],
y[~mask],
s=3,
marker='.',
color=colors[1],
label='non-stationary points')
ax[0].scatter(x[mask],
y[mask],
s=3,
marker='.',
color=colors[0],
label='non-stationary points (masked)')
# trajectory points
xm, ym = self.trajectory(mode='raw', bbmask=bbmask)
ax[0].scatter(xm,
ym,
s=5,
edgecolor=colors[5],
label='NSP centroid (per frame)')
# interpolated trajectory
xm, ym = self.trajectory(mode='interp', bbmask=bbmask)
ax[0].plot(xm,
ym,
color=colors[4],
label='NSP centroid (interpolated)')
# filtered trajectory
order = 4
fc = 1.5
fs = 20
wn = fc / (0.5 * fs)
b, a = scipy.signal.butter(order, wn, btype='lowpass')
butterf = lambda x: scipy.signal.filtfilt(b, a, x)
ax[0].plot(butterf(xm),
butterf(ym),
color=colors[3],
label='NSP centroid (filtered, low pass butterworth)')
ax[0].legend()
if not plot_vel:
return fig, ax[0]
f = lambda x: np.square(np.diff(x))
v = lambda x, y: np.sqrt(f(x) + f(y)) / 0.05
ax[1].plot(self.bicycle.time,
ff.moving_average(self.bicycle.speed, 55),
color=colors[1],
zorder=2,
label='measured speed (filtered, moving average)')
ylim = ax[1].get_ylim()
ax[1].plot(self.bicycle.time,
self.bicycle.speed,
color=colors[0],
zorder=0,
label='measured speed')
ax[1].plot(self.lidar.time[1:],
v(xm, ym),
color=colors[4],
zorder=1,
label='estimated speed (centroid)')
ax[1].plot(
self.lidar.time[1:],
v(butterf(xm), butterf(ym)),
color=colors[3],
zorder=2,
label='estimated speed (centroid, filtered, low pass butter)')
ax[1].set_ylim(ylim)
handles, labels = ax[1].get_legend_handles_labels()
# swap first two elements
handles[0], handles[1] = handles[1], handles[0]
labels[0], labels[1] = labels[1], labels[0]
ax[1].legend(handles, labels)
return fig, ax
def plot_velocity(r):
charcoal_color = sns.xkcd_palette(['charcoal'])[0]
t = r['time']
v = r['speed']
dt = np.diff(t).mean()
# from visual inspection of plot
i0 = np.argwhere(t > 20)[0][0]
i1 = np.argwhere(t > 120)[0][0]
vx = v[i0:i1].mean()
sg_window_length = 255
sg_polyorder = 2
vf = scipy.signal.savgol_filter(v,
sg_window_length,
polyorder=sg_polyorder)
wiener_length = 256
h = wiener_taps(vf, v, wiener_length)
vf2 = scipy.signal.filtfilt(h, np.array([1]), v)
vf3 = scipy.signal.lfilter(h, np.array([1]), v)
vf4 = ff.moving_average(v, wiener_length - 1, wiener_length / 2)
colors = sns.color_palette('Paired', 10)
fig, axes = plt.subplots(2, 1)
axes = axes.ravel()
ax = axes[0]
ax.plot(t[i0:i1],
vx * np.ones(i1 - i0),
color=colors[1],
alpha=1,
zorder=1,
label='velocity, measurement mean')
ax.plot(t,
v,
color=colors[3],
alpha=1,
zorder=0,
label='velocity, measured')
ax.plot(
t,
vf,
color=colors[5],
alpha=1,
zorder=0,
label='velocity, savitzky-golay filter, length={}, order={}'.format(
sg_window_length, sg_polyorder))
ax.plot(t,
vf2,
color=colors[7],
alpha=1,
zorder=2,
label='velocity, wiener filter, length={}, filtfilt'.format(
wiener_length))
ax.plot(t,
vf3,
color=colors[6],
alpha=1,
zorder=2,
label='velocity, wiener filter, length={}, lfilter'.format(
wiener_length))
ax.plot(t,
vf3,
color=colors[8],
alpha=1,
zorder=2,
label='velocity, moving average, length={}'.format(wiener_length -
1))
ax.set_xlabel('time [s]')
ax.set_ylabel('velocity [m/s]')
ax.axhline(0, color=charcoal_color, linewidth=1, zorder=1)
ax.legend()
freq, xf0 = ff.fft(v, dt) # uses hamming window
freq, xf1 = ff.fft(vf, dt) # uses hamming window
freq, xf2 = ff.fft(vf2, dt) # uses hamming window
ax = axes[1]
ax.plot(freq, xf0, color=colors[3], alpha=1, zorder=0)
ax.plot(freq, xf1, color=colors[5], alpha=1, zorder=0)
ax.plot(freq, xf2, color=colors[7], alpha=1, zorder=1)
ax.set_yscale('log')
ax.set_xlabel('frequency [Hz]')
ax.set_ylabel('amplitude')
def plot_kalman_result(result,
event=None,
smoothed_result=None,
wheelbase=1.0,
ax_mask=None,
ax=None,
**fig_kw):
if ax_mask is None:
# create axes for all plots
ax_mask = np.ones((5, ), dtype=bool)
else:
ax_mask = np.asarray(ax_mask).astype(bool)
assert ax_mask.shape == (5, )
if ax is None:
n = np.count_nonzero(ax_mask)
if n == 5:
ax_shape = (3, 2)
colspan = 2
else:
ax_shape = (n, 1)
colspan = 1
fig, ax = plt.subplots(*ax_shape, sharex=True, **fig_kw)
ax = ax.ravel()
if ax_mask[0]: # plot trajectory
ax[0].axis('off')
if n == 5:
ax[1].axis('off')
ax = ax[1:]
ax[0] = plt.subplot2grid(ax_shape, (0, 0),
rowspan=1,
colspan=colspan,
fig=fig)
else:
fig = ax.get_figure()
ax = ax.ravel()
ax_index = np.cumsum(ax_mask, dtype=int) - ax_mask[0]
ax_index[~ax_mask] = ax_mask.size # set unused axes with invalid index
x = result.state_estimate
P = result.error_covariance
def correct_trajectory(state, wheelbase):
yaw = state[:, 2]
x = state[:, 0] + wheelbase / 2 * np.cos(yaw)
y = state[:, 1] + wheelbase / 2 * np.sin(yaw)
return x.squeeze(), y.squeeze()
if event is None:
event_time = np.arange(x.shape[0])
else:
event_time = event.bicycle.time
T = np.diff(event.bicycle.time).mean()
z = generate_measurement(event)
color = sns.color_palette('tab10', 10)
if ax_mask[0]: # plot trajectory
ax_ = ax[ax_index[0]]
_x, _y = correct_trajectory(x, wheelbase)
ax_.plot(_x, _y, color=color[0], alpha=0.5, label='KF trajectory')
ax_.fill_between(_x,
_y + np.sqrt(P[:, 1, 1]),
_y - np.sqrt(P[:, 1, 1]),
color=color[0],
alpha=0.2)
if event is not None:
index = ~z.mask.any(axis=1)
ax_.scatter(_x[index],
_y[index],
s=15,
marker='X',
color=color[0],
label='KF trajectory (valid measurement)')
if smoothed_result is not None:
xs = smoothed_result.state_estimate
Ps = smoothed_result.error_covariance
_x, _y = correct_trajectory(xs, wheelbase)
ax_.plot(_x, _y, color=color[2], alpha=0.5, label='KS trajectory')
ax_.fill_between(_x,
_y + np.sqrt(Ps[:, 1, 1]),
_y - np.sqrt(Ps[:, 1, 1]),
color=color[2],
alpha=0.2)
if event is not None:
ax_.scatter(_x[index],
_y[index],
s=15,
marker='X',
color=color[2],
label='KS trajectory (valid measurement)')
if event is not None:
ax_.scatter(z[:, 0].compressed(),
z[:, 1].compressed(),
s=15,
marker='X',
color=color[1],
alpha=0.5,
label='trajectory (butter)')
ax_.scatter(event.x,
event.y,
s=1,
marker='.',
zorder=0,
color=color[6],
alpha=0.5,
label='lidar point cloud')
ax_.legend()
if ax_mask[1]: # plot yaw angle
ax_ = ax[ax_index[1]]
ax_.plot(event_time, x[:, 2], color=color[0], label='KF yaw angle')
ax_.fill_between(event_time,
x[:, 2].squeeze() + np.sqrt(P[:, 2, 2]),
x[:, 2].squeeze() - np.sqrt(P[:, 2, 2]),
color=color[0],
alpha=0.2)
if smoothed_result is not None:
ax_.plot(event_time,
xs[:, 2],
color=color[2],
label='KS yaw angle')
ax_.fill_between(event_time,
xs[:, 2].squeeze() + np.sqrt(Ps[:, 2, 2]),
xs[:, 2].squeeze() - np.sqrt(Ps[:, 2, 2]),
color=color[2],
alpha=0.2)
if event is not None:
ax_.plot(event_time[1:],
scipy.integrate.cumtrapz(z[:, 2], dx=T) + np.pi,
color=color[3],
label='integrated yaw rate')
ax_.legend()
if ax_mask[2]: # plot yaw rate
ax_ = ax[ax_index[2]]
ax_.plot(event_time, x[:, 4], color=color[0], label='KF yaw rate')
ax_.fill_between(event_time,
x[:, 4].squeeze() + np.sqrt(P[:, 4, 4]),
x[:, 4].squeeze() - np.sqrt(P[:, 4, 4]),
color=color[0],
alpha=0.2)
if smoothed_result is not None:
ax_.plot(event_time, xs[:, 4], color=color[2], label='KS yaw rate')
ax_.fill_between(event_time,
xs[:, 4].squeeze() + np.sqrt(Ps[:, 4, 4]),
xs[:, 4].squeeze() - np.sqrt(Ps[:, 4, 4]),
color=color[2],
alpha=0.2)
if event is not None:
ax_.plot(event_time,
z[:, 2],
color=color[1],
alpha=0.5,
label='yaw rate')
ax_.legend()
if ax_mask[3]: # plot speed
ax_ = ax[ax_index[3]]
ax_.plot(event_time, x[:, 3], color=color[0], label='KF speed')
ax_.fill_between(event_time,
x[:, 3].squeeze() + np.sqrt(P[:, 3, 3]),
x[:, 3].squeeze() - np.sqrt(P[:, 3, 3]),
color=color[0],
alpha=0.2)
ax_.axhline(0, color='black')
if smoothed_result is not None:
ax_.plot(event_time, xs[:, 3], color=color[2], label='KS speed')
ax_.fill_between(event_time,
xs[:, 3].squeeze() + np.sqrt(Ps[:, 3, 3]),
xs[:, 3].squeeze() - np.sqrt(Ps[:, 3, 3]),
color=color[2],
alpha=0.2)
if event is not None:
ax_.plot(event_time,
ff.moving_average(event.bicycle.speed, 55),
color=color[1],
label='speed')
ax_.plot(event_time[1:],
scipy.integrate.cumtrapz(z[:, 3], dx=T) + x[0, 3],
color=color[3],
label='integrated accel')
ylim = ax_.get_ylim()
ax_.plot(event_time,
event.bicycle.speed,
color=color[1],
alpha=0.2,
label='speed (raw)')
ax_.set_ylim(ylim)
ax_.legend()
if ax_mask[4]: # plot acceleration
ax_ = ax[ax_index[4]]
ax_.plot(event_time,
x[:, 5],
zorder=2,
color=color[0],
label='KF accel')
ax_.fill_between(event_time,
x[:, 5].squeeze() + np.sqrt(P[:, 5, 5]),
x[:, 5].squeeze() - np.sqrt(P[:, 5, 5]),
color=color[0],
alpha=0.2)
if smoothed_result is not None:
ax_.plot(event_time,
xs[:, 5],
zorder=2,
color=color[2],
label='KS accel')
ax_.fill_between(event_time,
xs[:, 5].squeeze() + np.sqrt(Ps[:, 5, 5]),
xs[:, 5].squeeze() - np.sqrt(Ps[:, 5, 5]),
color=color[2],
alpha=0.2)
if event is not None:
ax_.plot(event_time,
z[:, 3],
zorder=1,
color=color[1],
label='acceleration')
ax_.legend()
return fig, ax