|
|
@@ -173,30 +173,30 @@ def sfit4(data, fs): |
|
|
|
# Convert to Hz |
|
|
|
f0 = i * fs / N |
|
|
|
|
|
|
|
## Fit it |
|
|
|
# first guess for A0, B0 using 3-parameter fit (step c) |
|
|
|
w = 2*pi*f0 |
|
|
|
D = c_[cos(w*t), sin(w*t), ones(N)] |
|
|
|
s = linalg.lstsq(D, data)[0] |
|
|
|
|
|
|
|
# Now iterate 6 times (step i) |
|
|
|
for idx in range(6): |
|
|
|
D = c_[cos(w*t), sin(w*t), ones(N), |
|
|
|
-s[0] * t * sin(w*t) + s[1] * t * cos(w*t) ] # eqn B.16 |
|
|
|
s = linalg.lstsq(D, data)[0] # eqn B.18 |
|
|
|
w = w + s[3] # update frequency estimate |
|
|
|
|
|
|
|
## Extract results |
|
|
|
A = sqrt(s[0]*s[0] + s[1]*s[1]) # eqn B.21 |
|
|
|
f0 = w / (2*pi) |
|
|
|
# Fit it. We'll catch exceptions here and just returns zeros |
|
|
|
# if something fails with the least squares fit, etc. |
|
|
|
try: |
|
|
|
# first guess for A0, B0 using 3-parameter fit (step c) |
|
|
|
w = 2*pi*f0 |
|
|
|
D = c_[cos(w*t), sin(w*t), ones(N)] |
|
|
|
s = linalg.lstsq(D, data)[0] |
|
|
|
|
|
|
|
# Now iterate 6 times (step i) |
|
|
|
for idx in range(6): |
|
|
|
D = c_[cos(w*t), sin(w*t), ones(N), |
|
|
|
-s[0] * t * sin(w*t) + s[1] * t * cos(w*t) ] # eqn B.16 |
|
|
|
s = linalg.lstsq(D, data)[0] # eqn B.18 |
|
|
|
w = w + s[3] # update frequency estimate |
|
|
|
|
|
|
|
## Extract results |
|
|
|
A = sqrt(s[0]*s[0] + s[1]*s[1]) # eqn B.21 |
|
|
|
f0 = w / (2*pi) |
|
|
|
phi = -arctan2(s[1], s[0]) # eqn B.22 |
|
|
|
except TypeError: |
|
|
|
C = s[2] |
|
|
|
return (A, f0, phi, C) |
|
|
|
except Exception as e: |
|
|
|
# something broke down, just return zeros |
|
|
|
return (0, 0, 0, 0) |
|
|
|
C = s[2] |
|
|
|
|
|
|
|
return (A, f0, phi, C) |
|
|
|
|
|
|
|
if __name__ == "__main__": |
|
|
|
main() |