Commit 5a7907cd authored by Matthieu Cattin's avatar Matthieu Cattin

Working prototype of sine wave fit and boundaries check.

parent 20238014
......@@ -28,19 +28,9 @@ Sine wave fit test
def gen_sine(frequency=1, amplitude=1, noise=0.0, cycles=1, rate=10, phase=0.0, offset=0.0):
# Sample a noisy sine wave and return a list of sample amplitudes
#print("frequency : %f"%frequency)
#print("amplitude : %f"%amplitude)
#print("noise : %f"%noise)
#print("cycles : %f"%cycles)
#print("rate : %f"%rate)
#print("phase : %f"%phase)
#print("offset : %f"%offset)
# Calculated derived values
dt = 1/float(frequency*rate)
#print("dt : %f"%dt)
period = 1/float(frequency)
#print("period : %f"%period)
steps = arange(0, cycles*period, dt)
samples = [] # create an empty list
......@@ -49,35 +39,48 @@ def gen_sine(frequency=1, amplitude=1, noise=0.0, cycles=1, rate=10, phase=0.0,
# for given number of cycles
for t in steps:
angle = 2*pi*t/period + phase # in radians
a = amplitude*sin(angle) + noise*randn() + offset
a = amplitude*sin(angle) + noise*random() + offset
samples.append(a)
return samples
class Parameter:
def __init__(self, value):
self.value = value
def set(self, value):
self.value = value
def my_sine(x, o, a, f, p):
return o + a * sin(f*x + p)
def __call__(self):
return self.value
def fit(function, parameters, y, x = None):
def f(params):
i = 0
for p in parameters:
p.set(params[i])
i += 1
return y - function(x)
def fit_sine(x, y, f):
# Guessed amplitude is the max value of input array
guess_ampl = max(y)
# Guessed offset is the mean value of input array
guess_offset = mean(y)
# Guessed frequency is the highest peak in the input array fft
yhat = fftpack.rfft(y)
idx = (yhat**2).argmax()
freqs = fftpack.rfftfreq(len(y), d = (x[1]-x[0])/(2*pi))
guess_freq = freqs[idx]
# Guessed phase is zero (doesn't really matters), could make a fisrt rising_edge detection
#tmp = diff(sign(y))
#rising_edges = where(select([tmp>0],[tmp]))[0]
guess_phase = 0.0
if x is None: x = np.arange(y.shape[0])
p = [param() for param in parameters]
return optimize.leastsq(f, p, full_output=True, ftol=1e-6, xtol=1e-6)
guess_params = [guess_offset, guess_ampl, guess_freq, guess_phase]
print("Guessed params => Offset: %f, Amplitude: %f, Frequency: %f, Phase: %f"%(guess_offset, guess_ampl, guess_freq, guess_phase))
# Fit f to x,y input data
(fit_offset, fit_ampl, fit_freq, fit_phase), pcov = optimize.curve_fit(f, x, y, guess_params)
print("Fit params => Offset: %f, Amplitude: %f, Frequency: %f, Phase: %f"%(fit_offset, fit_ampl, fit_freq, fit_phase))
return fit_offset, fit_ampl, fit_freq, fit_phase
def check_bounds(data, upper, lower):
out_bounds = 0
for i in range(len(data)):
if data[i] > upper[i] or data[i] < lower[i]:
out_bounds += 1
return out_bounds
def my_sine(x, o, a, f, p):
return o + a * sin(f*x + p)
def plot_result(data, fit, sin_upper, sin_lower, ylimit, x_data):
clf()
......@@ -96,47 +99,36 @@ def plot_result(data, fit, sin_upper, sin_lower, ylimit, x_data):
def main (default_directory = '.'):
# Creates fake input data (noisy sine wave)
ampl = 2
ampl = 10*random()
freq = 1
rate = 500
phase = 0.5*pi
noise = 0.02
phase = 2*pi*random()
noise = 0.5
cycles = 2
offset = 0
yReal = gen_sine(freq, ampl, noise, cycles, rate, phase, offset)
xReal = arange(len(yReal))
print("Input sine params:\nOffset: %f, Amplitude: %f, Frequency: %f, Phase: %f"%(offset, ampl, freq, phase))
# rising edge detection
zero_cross = where(diff(sign(yReal)))[0]
print zero_cross
x = diff(sign(yReal))
rising_edge = where(select([x>0],[x]))[0]
print rising_edge
# give fit initial parameters
guess_ampl = max(yReal)
yhat = fftpack.rfft(yReal)
idx = (yhat**2).argmax()
freqs = fftpack.rfftfreq(len(yReal), d = (xReal[1]-xReal[0])/(2*pi))
guess_freq = freqs[idx]
guess_phase = 0.0
guess_offset = mean(yReal)
guess_params = [guess_offset, guess_ampl, guess_freq, guess_phase]
y = gen_sine(freq, ampl, noise, cycles, rate, phase, offset)
x = arange(len(y))
print("Input sine params => Offset: %f, Amplitude: %f, Frequency: %f, Phase: %f"%(offset, ampl, freq, phase))
print "Guessed params:"
print guess_params
fit_offset, fit_ampl, fit_freq, fit_phase = fit_sine(x, y, my_sine)
(fit_offset, fit_ampl, fit_freq, fit_phase), pcov = optimize.curve_fit(my_sine, xReal, yReal, guess_params)
# Create a sine wave from the fit params
fit = my_sine(x, fit_offset, fit_ampl, fit_freq, fit_phase)
print("Fit params:\nOffset: %f, Amplitude: %f, Frequency: %f, Phase: %f"%(fit_offset, fit_ampl, fit_freq, fit_phase))
# Create upper and lower bound sine waves
margin = 0.05
upper_margin = fit_offset + abs(fit_ampl) * margin
lower_margin = fit_offset - abs(fit_ampl) * margin
upper = my_sine(x, upper_margin, fit_ampl, fit_freq, fit_phase)
lower = my_sine(x, lower_margin, fit_ampl, fit_freq, fit_phase)
fit_sin = my_sine(xReal, fit_offset, fit_ampl, fit_freq, fit_phase)
# Check that input data are inside bounds
out_bounds = check_bounds(y, upper, lower)
if out_bounds != 0:
print("######## Input waveform outside bounds! ############ %d"%out_bounds)
upper = my_sine(xReal, fit_offset+0.1, fit_ampl, fit_freq, fit_phase)
lower = my_sine(xReal, fit_offset-0.1, fit_ampl, fit_freq, fit_phase)
plot_result(yReal, fit_sin, upper, lower, 2, xReal)
# Plot all waveforms
plot_result(y, fit, upper, lower, ampl, x)
if __name__ == '__main__' :
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment