Commit 3cd64966 authored by Thomas Bruns's avatar Thomas Bruns
Browse files

added seq_multi_threeparsinefit() and helpers

parent 453a9724
......@@ -9,6 +9,8 @@ auxiliary functions related to sine-approximation
import numpy as np
from scipy import linalg as la
from scipy.sparse import coo_matrix
from scipy.sparse.linalg import lsqr as sp_lsqr
import matplotlib.pyplot as mp
import warnings
......@@ -19,7 +21,7 @@ def sampletimes(Fs, T): #
sample rate Fs \n
from 0 to T
"""
num = np.ceil(T * Fs)
num = int(np.ceil(T * Fs))
return np.linspace(0, T, num, dtype=np.float64)
......@@ -236,7 +238,6 @@ def fourparsinefit(y, t, f0, tol=1.0e-7, nmax=1000):
(-1.0) * abcd[0] * t * np.sin(w * t) + abcd[1] * t * np.cos(w * t),
]
)
print(dw)
abcd = (la.lstsq(D.transpose(), y))[0]
dw = abcd[3]
w = w + 0.9 * dw
......@@ -705,51 +706,167 @@ def PR_MultiSine(
) # frequency series, sample rate, sample timestamps, waveform
###################################
# MultiSine for "quick calibration" based on displacement and velocity without
# explicit time-information (used for Linearmotor-control)
def XV_MultiSine(freq, phase, x0, tau, N=1000, deg=True):
'''
Calculation of a multisine trajectory in terms of position and velocity based
on given frequency, amplitude and phase
def seq_multi_threeparam_Dmatrix(f,t,periods=1):
"""
Fit a multi-sinus-signal in slices in one go.
Parameters
----------
freq : numpy.array float
Vector of frequencies of the sine components
phase : numpy array float
Vector of initial phases of the sine components
x0 : TYPE
Vector of amplitudes of the sine components
tau : float
duration for the whole motion
N : int, optional
number of samples. The default is 1000.
deg : boolean, optional
If True phases are in degree, if False in rad. The default is True/Deg.
f : numpy.array of floats
list of frequencies in the signal
t : numpy.array of floats
timestamps of y (typically seconds)
periods : float, optional
the number of periods of each frequncy used for each fit. The default is 1.
Returns
-------
multi_x : numpy.array float
Calculated position vector
multi_v : numpy.array
Calculated velocity vector
fr, : 1-d numpy.array of floats
frequencies related to slices
D : 2-d numpy.array of floats
Design matrix for the fit
"""
T = t[-1]-t[0]
col = 0
data = np.array([])
ci = []
ri = []
fr = []
# Designmatrix for sin/cos
for fi, omi in zip(f,2*np.pi*f):
Nri = 0 # counter for the current row index
tau = 1/fi*periods # slice length in seconds
t_sl = np.array_split(t,np.ceil(T/tau)) # array of slices of sample times
fr = fr + [fi]*len(t_sl) # len(t_sl) times frequency fi in Design matrix
for ti in t_sl: # loop over slices fo one frequncy
sin = np.sin(omi * ti)
cos = np.cos(omi * ti)
data = np.hstack((data,sin)) # data vector
ci = ci+[col]*len(ti) # column index vector
col = col+1
ri = ri+ [Nri+i for i in range(len(ti))] # row index vector
data = np.hstack((data,cos)) # data vector
ci = ci+[col]*len(ti) # column index vector
col = col+1
ri = ri+ [Nri+i for i in range(len(ti))] # row index vector
Nri = Nri+len(ti)
# Designmatrix for Bias
data = np.hstack((data,np.ones((len(t)))))
ci = ci = ci+[col]*len(t)
ri = ri + [i for i in range(len(t))]
col = col+1
# build sparse matrix, init as coo, map to csr
D = coo_matrix((data,(ri,ci)),shape=(len(t),col)).tocsr()
return np.array(fr), D
def seq_multi_threeparsinefit(f,y,t,periods=1):
"""
performs a simultanius, sliced three-parameter fit on a multisine signal y
'''
assert (
len(freq) == len(x0) == len(phase)
), "XV_MultiSine: Unequal length of frequency and amplitude arrays!"
Parameters
----------
f : 1-d numpy.array of floats
frequencies in the signal
y : 1-d numpy.array
samples of the signal
t : 1-d numpy.array
vector of sample times
periods : float
(fractional) number of periods in one slice. The default is 1.
if deg: # rad -> deg
phase = phase * np.pi / 180.0
Returns
-------
f_ab_c : 2-d numpy.array of floats
frequencies, a/b-coefficients and bias related to given frequencies [f1,f1,f1,f2,f2, ...fn, fn, f=0=bias]
y0 : 1-d numpy.array of floats
samples of the optimal fit
resid : 1-d numpy.array of floats
Residuals = Difference =(y-y0)
"""
fr, D = seq_multi_threeparam_Dmatrix(f,t,periods) # calculate the design matrix (as sparse matrix)
abc = sp_lsqr(D,y)
y0 = D.dot(abc[0])
#print(abc)
f_ab_c =[]
k=0
for fi in fr:
f_ab_c = f_ab_c+ [fi, abc[0][k],abc[0][k+1]]
k=k+2
f_ab_c = f_ab_c + [0.0,abc[0][-1],0.0]
f_ab_c = np.array(f_ab_c).reshape((len(f_ab_c)//3,3))
print(f_ab_c)
return f_ab_c, y0, y-y0 # coefficients, fit signal, residuals
def seq_multi_amplitude(f_ab_c):
"""
return the amplitude(s) of a sequentially fit multi-sine signal,
i.e. amplitudes encoded in the return value of seq_multi_threeparsinefit.
Parameters
----------
f_ab_c : 2-d numpy array of floats (Nx3)
f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
Returns
-------
2d-numpy-array of floats (Nx2)
frequency and associated amplitude.
ti = np.arange(0, tau, tau / N)
"""
amp = [np.abs(f_ab_c[i,1]+1j*f_ab_c[i][2]) for i in range(f_ab_c.shape[0]-1)]
return np.vstack((f_ab_c[:-1,0],np.array(amp))).T
def seq_multi_phase(f_ab_c, deg=True):
"""
Calculates the initial phase of a sequentially fit multi-sine signal,
i.e. initial phases encoded in the return value of seq_multi_threeparsinefit.
result is either in degrees (deg=True) or rad (deg=False).
Parameters
----------
f_ab_c : 2-d numpy array of floats (Nx3)
f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
deg : Boolean, optional
Flag whether result is in degrees or rad. The default is True (Degrees).
Returns
-------
2d-numpy-array of floats (Nx2)
frequency and associated initial phase.
"""
amp = [np.angle(1j*f_ab_c[i,1]+f_ab_c[i][2],deg=deg) for i in range(f_ab_c.shape[0]-1)]
return np.vstack((f_ab_c[:-1,0],np.array(amp))).T
multi_x = np.zeros(len(ti), dtype=np.float64)
multi_v = np.zeros(len(ti), dtype=np.float64)
for f, x, p in zip(freq, x0, phase):
om = 2 * np.pi * f
multi_x = multi_x + x * np.sin(om * ti + p)
multi_v = multi_v + x * om * np.cos(om * ti + p)
def seq_multi_bias(f_ab_c):
"""
Returns the single bias of a sequentially fit multi-sine signal,
i.e. bias encoded in the return value of seq_multi_threeparsinefit.
Parameters
----------
f_ab_c : 2-d numpy array of floats (Nx3)
f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
return ti, multi_x, multi_v
Returns
-------
float
Bias of the signal.
"""
return f_ab_c[-1][1]
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