def find_timeshift(niAcc, vnAcc, sampleRate, speed, plotError=False):
'''Returns the timeshift, tau, of the VectorNav [VN] data relative to the
National Instruments [NI] data.
Parameters
----------
niAcc : ndarray, shape(n, )
The acceleration of the NI accelerometer in its local Y direction.
vnAcc : ndarray, shape(n, )
The acceleration of the VN-100 in its local Z direction. Should be the
same length as NIacc and contains the same signal albiet time shifted.
The VectorNav signal should be leading the NI signal.
sampleRate : integer or float
Sample rate of the signals. This should be the same for each signal.
speed : float
The approximate forward speed of the bicycle.
Returns
-------
tau : float
The timeshift.
Notes
-----
The Z direction for `VNacc` is assumed to be aligned with the steer axis
and pointing down and the Y direction for the NI accelerometer should be
aligned with the steer axis and pointing up.
'''
# raise an error if the signals are not the same length
N = len(niAcc)
if N != len(vnAcc):
raise TimeShiftError('Signals are not the same length!')
# make a time vector
time = time_vector(N, sampleRate)
# the signals are opposite sign of each other, so fix that
niSig = -niAcc
vnSig = vnAcc
# some constants for find_bump
wheelbase = 1.02 # this is the wheelbase of the rigid rider bike
bumpLength = 1.
cutoff = 30.
# filter the NI Signal
filNiSig = butterworth(niSig, cutoff, sampleRate)
# find the bump in the filtered NI signal
niBump = find_bump(filNiSig, sampleRate, speed, wheelbase, bumpLength)
# remove the nan's in the VN signal and the corresponding time
v = vnSig[np.nonzero(np.isnan(vnSig) == False)]
t = time[np.nonzero(np.isnan(vnSig) == False)]
# fit a spline through the data
vn_spline = UnivariateSpline(t, v, k=3, s=0)
# and filter it
filVnSig = butterworth(vn_spline(time), cutoff, sampleRate)
# and find the bump in the filtered VN signal
vnBump = find_bump(filVnSig, sampleRate, speed, wheelbase, bumpLength)
if vnBump is None or niBump is None:
guess = 0.3
else:
# get an initial guess for the time shift based on the bump indice
guess = (niBump[1] - vnBump[1]) / float(sampleRate)
# Since vnSig may have nans we should only use contiguous data around
# around the bump. The first step is to split vnSig into sections bounded
# by the nans and then selected the section in which the bump falls. Then
# we select a similar area in niSig to run the time shift algorithm on.
if vnBump is None:
bumpLocation = 800 # just a random guess so things don't crash
else:
bumpLocation = vnBump[1]
indices, arrays = split_around_nan(vnSig)
for pair in indices:
if pair[0] <= bumpLocation < pair[1]:
bSec = pair
# subtract the mean and normalize both signals
niSig = normalize(subtract_mean(niSig, hasNans=True), hasNans=True)
vnSig = normalize(subtract_mean(vnSig, hasNans=True), hasNans=True)
niBumpSec = niSig[bSec[0]:bSec[1]]
vnBumpSec = vnSig[bSec[0]:bSec[1]]
timeBumpSec = time[bSec[0]:bSec[1]]
if len(niBumpSec) < 200:
warn('The bump section is only {} samples wide.'.format(str(len(niBumpSec))))
# set up the error landscape, error vs tau
# The NI lags the VectorNav and the time shift is typically between 0 and
# 1 seconds
tauRange = np.linspace(0., 2., num=500)
error = np.zeros_like(tauRange)
for i, val in enumerate(tauRange):
error[i] = sync_error(val, niBumpSec, vnBumpSec, timeBumpSec)
if plotError:
plt.figure()
plt.plot(tauRange, error)
plt.xlabel('tau')
plt.ylabel('error')
plt.show()
# find initial condition from landscape
tau0 = tauRange[np.argmin(error)]
print "The minimun of the error landscape is %f and the provided guess is %f" % (tau0, guess)
# Compute the minimum of the function using both the result from the error
# landscape and the bump find for initial guesses to the minimizer. Choose
# the best of the two.
tauBump, fvalBump = fmin(sync_error, guess, args=(niBumpSec,
vnBumpSec, timeBumpSec), full_output=True, disp=True)[0:2]
tauLandscape, fvalLandscape = fmin(sync_error, tau0, args=(niBumpSec, vnBumpSec,
timeBumpSec), full_output=True, disp=True)[0:2]
if fvalBump < fvalLandscape:
tau = tauBump
else:
tau = tauLandscape
#### if the minimization doesn't do a good job, just use the tau0
###if np.abs(tau - tau0) > 0.01:
###tau = tau0
###print "Bad minimizer!! Using the guess, %f, instead." % tau
print "This is what came out of the minimization:", tau
if not (0.05 < tau < 2.0):
raise TimeShiftError('This tau, {} s, is probably wrong'.format(str(tau)))
return tau
def find_bump(accelSignal, sampleRate, speed, wheelbase, bumpLength):
'''Returns the indices that surround the bump in the acceleration signal.
Parameters
----------
accelSignal : ndarray, shape(n,)
An acceleration signal with a single distinctive large acceleration
that signifies riding over the bump.
sampleRate : float
The sample rate of the signal.
speed : float
Speed of travel (or treadmill) in meters per second.
wheelbase : float
Wheelbase of the bicycle in meters.
bumpLength : float
Length of the bump in meters.
Returns
-------
indices : tuple
The first and last indice of the bump section.
'''
# Find the indice to the maximum absolute acceleration in the provided
# signal. This is mostly likely where the bump is. Skip the first few
# points in case there are some endpoint problems with filtered data.
n = len(accelSignal)
nSkip = 5
rectAccel = abs(subtract_mean(accelSignal[nSkip:n / 2.]))
indice = np.nanargmax(rectAccel) + nSkip
# This calculates how many time samples it takes for the bicycle to roll
# over the bump given the speed of the bicycle, it's wheelbase, the bump
# length and the sample rate.
bumpDuration = (wheelbase + bumpLength) / speed
bumpSamples = int(bumpDuration * sampleRate)
# make the number divisible by four
bumpSamples = int(bumpSamples / 4) * 4
# These indices try to capture the length of the bump based on the max
# acceleration indice.
indices = (indice - bumpSamples / 4, indice, indice + 3 * bumpSamples / 4)
# If the maximum acceleration is not greater than 0.5 m/s**2, then there was
# probably was no bump collected in the acceleration data.
maxChange = rectAccel[indice - nSkip]
if maxChange < 0.5:
warn('This run does not have a bump that is easily detectable. ' +
'The bump only gave a {:1.2f} m/s^2 change in nominal accerelation.\n'\
.format(maxChange) +
'The bump indice is {} and the bump time is {:1.2f} seconds.'\
.format(str(indice), indice / float(sampleRate)))
return None
else:
# If the bump isn't at the beginning of the run, give a warning.
if indice > n / 3.:
warn("This signal's max value is not in the first third of the data\n"
+ "It is at %1.2f seconds out of %1.2f seconds" %
(indice / float(sampleRate), n / float(sampleRate)))
# If there is a nan in the bump this maybe an issue down the line as the
# it is prefferable for the bump to be in the data when the fitting occurs,
# to get a better fit.
if np.isnan(accelSignal[indices[0]:indices[1]]).any():
warn('There is at least one NaN in this bump')
return indices
