Linear least squares phase estimation of a sine wave

This snippet extracts the phase out of a noisy sinusoidal signal. The algorithm is a clever linear least squares (LLS) scheme, courtesy of the IEEE waveform measurement standard for characterizing ADCs and DACs. It works in spite of the fact that the sine wave is non-linear -- which just underscores the point that the linear bit of LLS is linearity in the parameters, not the fit function. It works quite a bit faster than the Levenberg-Marquardt nonlinear least squares routine.

 

	# Least squares fitting
	# IEEE algorithm version for known frequency
	from __future__ import division
	import numpy as np
	from numpy import pi
	from numpy import fft
	from numpy import linalg as lg
	import pylab as plt
	import matplotlib.gridspec as gridspec
	## 3-parameter sine fit, with linear least squares
	dt = 1e-2
	T = np.random.uniform(3,15) # record length
	A = 1.0 # sine amplitude
	noise_amplitude = np.random.uniform(0,20)
	f = np.random.uniform(50,80) # sine frequency
	N = int(T/dt) + 1
	t = np.arange(0,T,dt)
	phase = np.random.uniform(0,2*pi)
	noise = np.random.normal(0,noise_amplitude,N) # noise vector
	y = A * np.sin(2*pi*f*t + phase) + noise
	y = np.matrix(y).T
	D = np.matrix( [np.cos(2*pi*f*t), np.sin(2*pi*f*t), np.ones(N)] ).T
	x_opt = lg.inv(D.T * D) * D.T * y # parameter vector
	a,b,c = x_opt # = A * np.sin(phase), A * np.cos(phase), np.average(y)
	residuals = y - D * x_opt
	S = lg.norm(residuals)**2
	x_cov = np.asscalar( S/(N - len(x_opt)) ) * lg.inv(D.T * D) # covariance matrix
	phi = np.arctan2(a,b)%pi # phase estimate
	delta_a, delta_b = np.sqrt(x_cov[0,0]), np.sqrt(x_cov[1,1])
	delta_phi = np.cos(phi)**2 * np.abs(a/b) * np.sqrt( (delta_a/a)**2 + (delta_b/b)**2 )
	def corr(x,y): return np.dot(x,y)/np.sqrt( np.dot(x,x) * np.dot(y,y) )
	def correlation_length(x):
	l = 0
	while abs(corr( x,np.roll(x,l) )) > 0.5: l = l+1
	return l
	N_eff = N/correlation_length(noise)
	delta_phi_estimated = np.sqrt(2) * np.std(residuals)/np.sqrt( (a**2 + b**2) * N_eff )
	phase_error = phi%pi - phase%pi
	print "SNR = ", round( lg.norm(x_opt[:-1])/np.std(residuals) ,3)
	print
	print "phase error = ",round(phase_error,3), "("+`round(delta_phi,3)`+")", "(fit)"
	print "predicted uncertainty = ", "("+`round(delta_phi_estimated,3)`+")"
	residuals, y, y_reconstructed = np.array(residuals), np.array(y), np.array( D * x_opt )
	## Plotting
	fig = plt.figure()
	gs = gridspec.GridSpec(3,6)
	ax1 = plt.subplot(gs[0:2,:-1])
	ax2 = plt.subplot(gs[2:,:-1])
	ax3 = plt.subplot(gs[2:,-1])
	ax3.xaxis.set_visible(False)
	ax3.yaxis.set_visible(False)
	ax1.plot(t,y,'b',lw=2)
	ax1.plot(t,y_reconstructed,'g',lw=2)
	ax1.set_ylabel("y")
	ax1.grid(axis='y')
	ax2.plot(t,residuals, 'g')
	ax2.set_ylabel("residuals")
	ax2.set_xlabel("Time, t [s]")
	n, bins, patches = ax3.hist(residuals, 50, normed=True, \
	orientation='horizontal', facecolor='green', \
	histtype='stepfilled')
	plt.show()

view raw sine_fitting - known frequency.py hosted with ❤ by GitHub