Commit 1248407b authored by Thomas Bruns's avatar Thomas Bruns
Browse files

added atan-demodulation function

parent e63a845f
......@@ -9,8 +9,10 @@ auxiliary functions related to sine-approximation
import numpy as np
from scipy import linalg as la
from scipy.sparse import coo_matrix, hstack
from scipy.sparse import coo_matrix
from scipy.sparse.linalg import lsqr as sp_lsqr
from scipy.signal import firwin, lfilter
from scipy.stats import linregress
import matplotlib.pyplot as mp
import warnings
......@@ -1122,4 +1124,89 @@ def seq_multi_bias(f_ab_c):
return f_ab_c[-1][1]
def atan_demodulate(samples, fc=0.0, fs=0.0, fc_correct=True, lamb=1.0):
"""
Arc-tangent demodulation of a frequency modulated signal (heterodyne Interferometer raw data)
with auto-estimate of carrier frequency and carrier frequency adjustment if needed
Parameters
----------
samples : numpyp.array of floats
raw data samples of the FM-signal
fc : float, optional
if fc>0 given it is the assumed carrier frequency for the (first) demodulation.
The default is 0.0 which means auto-estimation.
fs : TYPE, (not really) optional
sample rate of the measured samples. The default is 0.0.
fc_correct : Boolean, optional
if True an extra adjustment of the carrier frequency is performed.
The default is True.
lamb : float, optional
The wavelength of the interferometer which is used to scale phase to
displacement. The default is 1.0, which returns the raw phase.
Returns
-------
ti : numpyp.array of floats
timestamps of results
phase : numpyp.array of floats
phase or displacement i.e. demodulation results
fc : float
the carrier frequency ultimately used for the result..
"""
assert fs>0 , "sample rate fs must be given with fs>0"
assert fc >= 0 , "carrier frequency fc must be zero (auto-mode) or >0"
def filter_fir(data,fc, fs, order):
# auxilliary filter function
nyq = 0.5*fs
normal_cutoff = fc / nyq
b = firwin(order,normal_cutoff, width=None, window='hamming', pass_zero=True, scale=True, nyq=1.0)
y = lfilter(b,[1],data)
return y
def filter_fir2(data,fc, fs, order):
# auxilliarry filter function (bi-directional, forward-backward)
l = 4*order
data = np.hstack((np.flipud(data[:l]),data))
y = filter_fir(data,fc,fs,order)
y = np.flipud(y[l:])
y = np.hstack((np.flipud(y[:l]),y))
y = filter_fir(y,fc,fs,order)
y = np.flipud(y[l:])
return y
if fc == 0: # estimate the carrier frequency from the signal
pm = np.sign(samples) # +/- 1
zer = len(np.where(np.diff(pm)<0)[0]) # number of falling edge zero crossings
fc_est = zer/len(samples)*fs # estimate of carrier frequency
else:
fc_est = fc # take the given carrier frequency
om_fc = 2*np.pi*fc_est # angular frequency for sine/cosine
ti = np.arange(samples.shape[0])/fs # calculate a time_scale
si = filter_fir2(samples*np.sin(ti*om_fc),0.8*fc_est,fs,11) # sine-quadrature
co = filter_fir2(samples*np.cos(ti*om_fc),0.8*fc_est,fs,11) # cosine-quadrature
dis = np.arctan2(si,co) # retreive wrapped phase
phase = np.unwrap(dis) # unwrap phase
if fc_correct :
delta_f = linregress(ti,phase).slope/(2*np.pi)
fc_est -= delta_f
om_fc = 2*np.pi*fc_est
ti = np.arange(samples.shape[0])/fs # calculate a time_scale
si = filter_fir2(samples*np.sin(ti*om_fc),0.8*fc_est,fs,11) # sine-quadrature
co = filter_fir2(samples*np.cos(ti*om_fc),0.8*fc_est,fs,11) # cosine-quadrature
dis = np.arctan2(si,co) # wrapped phase
phase = np.unwrap(dis)
ti = 1/fs*np.arange(dis.shape[0]) #
if lamb != 1.0: # calculate displacement if wavelength given
phase = lamb*phase/(4*np.pi)
return ti,phase, fc_est
Supports Markdown
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