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