Extraction of a sine wave's parameters (especially phase) with a known frequency. A demonstration of linear least squares.

sine_fitting - known frequency.py

# 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()