def find_real_peak(series):
p = np.argmax(series)
min_idx = p - 3
if (min_idx < 0):
min_idx = 0
max_idx = p + 3
if (max_idx >= len(series)):
max_idx = len(series) - 1
fit = np.polyfit(range(min_idx, max_idx+1), series[min_idx:max_idx+1], 2)
peak = fit[1] / -fit[0] / 2
return fit[1] / -fit[0] / 2
# For now, we just want to figure the parameters that existing autotune
# uses, but in a more accurate and more robust way. In the future we'll
# do much fancier things.
# So, for each of the three axes, let's perform fourier analysis in order to
# correlate the differentiated, filtered gyros vs. the actuator command.
# This will tell us the delay, or tau. From
def compute_axis(desired_series, diff_gyro_series, filter_order=3,
percentile_cutoff=2, plot_tau=True, series_cutoff=2,
axis_name=""):
desired_fft = fftpack.fft(desired_series)
diff_fft = fftpack.fft(diff_gyro_series)
desired_fft_r = -desired_fft.conjugate()
correl = fftpack.ifft(desired_fft_r * diff_fft)
tau_offset = find_real_peak(np.abs(correl[0:len(correl)/series_cutoff]))
if plot_tau:
p = figure(title="%s delay time vs. correlation"%(axis_name), y_axis_type = "log")
p.line(np.linspace(0,len(correl) / 2 * time_step, len(correl) / 2), np.abs(correl[0:len(correl)/2]))
tauspan = Span(location=tau_offset * time_step, dimension='height', line_color='red', line_width=1)
#bogospan = Span(location=.0168, dimension='height', line_color='green', line_width=1)
#p.renderers.extend([bogospan])
p.renderers.extend([tauspan])
show(p)
# half cycles per sample.e
filter_edge = filter_order / tau_offset / 3.14159 / 1.22
# A nth order filter doesn't result in n* the delay with bessel
# design technique. 1.22 is a fudge factor for order 3. Not sure where
# 1.22 comes from :D
db, da = signal.iirfilter(filter_order, [filter_edge], btype='lowpass', ftype='bessel')
#w, gd = signal.group_delay((db, da))
#p = figure()
#p.line(w, gd)
#show(p)
desired_series = pandas.concat([desired_series, desired_series], ignore_index = True)
delayedact = signal.lfilter(db, da, desired_series)
delayedact = delayedact[len(delayedact)/2:]
# Now compare the response magnitude of the filtered waveform vs. the
# differentiated gyro series, to compute beta and then the bias.
span_desired = np.percentile(delayedact, [percentile_cutoff, 100-percentile_cutoff])
span_gyrodiff = np.percentile(diff_gyro_series, [percentile_cutoff, 100-percentile_cutoff])
lin_gain = (span_gyrodiff[1]-span_gyrodiff[0]) / (span_desired[1]-span_desired[0])
bias = delayedact.mean() * lin_gain - diff_gyro_series.mean()
return (tau_offset, lin_gain, bias, delayedact * lin_gain - bias)
def compute_axis_by_name(series_name, do_plots=True, series_cutoff=2):
desired = df['desired'+series_name]
deriv_filtered = df['derivfiltered'+series_name]
tau_offset, lin_gain, bias, adjact = compute_axis(desired, deriv_filtered, plot_tau=do_plots,
series_cutoff=series_cutoff,
axis_name=series_name)
df['adjdesired' + series_name] = pandas.Series(adjact)
print "Axis %s: tau=%.1f samples (%0.2f ms / %0.3f)"%(series_name, tau_offset, tau_offset*time_step*1000, math.log(tau_offset*time_step))
print "\tgain=%0.1f (beta=%0.3f), bias=%0.1f"%(lin_gain, math.log(lin_gain / time_step), bias)
if (do_plots):
p = figure(title="%s shifted to line up"%(series_name))
p.line(df.time, deriv_filtered)
p.line(df.time, adjact, color="green")
show(p)
p = figure(title="residual %s"%(series_name))
p.line(df.time, deriv_filtered - adjact)
show(p)
compute_axis_by_name("X", series_cutoff=5)
compute_axis_by_name("Y", series_cutoff=5)
compute_axis_by_name("Z", series_cutoff=5)
print np.correlate(df.derivfilteredX, df.derivfilteredX)
print np.correlate(df.derivfilteredX, df.derivfilteredY)
print np.correlate(df.derivfilteredX, df.derivfilteredZ)
print np.correlate(df.derivfilteredY, df.derivfilteredY)
print np.correlate(df.derivfilteredY, df.derivfilteredZ)
print np.correlate(df.derivfilteredZ, df.derivfilteredZ)