You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

sinefit.py 6.6 KiB

8 years ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202
  1. #!/usr/bin/python
  2. # Sine wave fitting. This runs about 5x faster than realtime on raw data.
  3. import nilmtools.filter
  4. import nilmdb.client
  5. from numpy import *
  6. from scipy import *
  7. #import pylab as p
  8. import operator
  9. def main(argv = None):
  10. f = nilmtools.filter.Filter()
  11. parser = f.setup_parser("Sine wave fitting")
  12. group = parser.add_argument_group("Sine fit options")
  13. group.add_argument('-c', '--column', action='store', type=int,
  14. help='Column number (first data column is 1)')
  15. group.add_argument('-f', '--frequency', action='store', type=float,
  16. default=60.0,
  17. help='Approximate frequency (default: %(default)s)')
  18. group.add_argument('-m', '--min-freq', action='store', type=float,
  19. help='Minimum valid frequency '
  20. '(default: approximate frequency / 2))')
  21. group.add_argument('-M', '--max-freq', action='store', type=float,
  22. help='Maximum valid frequency '
  23. '(default: approximate frequency * 2))')
  24. # Parse arguments
  25. try:
  26. args = f.parse_args(argv)
  27. except nilmtools.filter.MissingDestination as e:
  28. rec = "float32_3"
  29. print "Source is %s (%s)" % (e.src.path, e.src.layout)
  30. print "Destination %s doesn't exist" % (e.dest.path)
  31. print "You could make it with a command like:"
  32. print " nilmtool -u %s create %s %s" % (e.dest.url, e.dest.path, rec)
  33. raise SystemExit(1)
  34. if args.column is None or args.column < 1:
  35. parser.error("need a column number >= 1")
  36. if args.frequency < 0.1:
  37. parser.error("frequency must be >= 0.1")
  38. if args.min_freq is None:
  39. args.min_freq = args.frequency / 2
  40. if args.max_freq is None:
  41. args.max_freq = args.frequency * 2
  42. if (args.min_freq > args.max_freq or
  43. args.min_freq > args.frequency or
  44. args.max_freq < args.frequency):
  45. parser.error("invalid min or max frequency")
  46. f.check_dest_metadata({ "sinefit_source": f.src.path,
  47. "sinefit_column": args.column })
  48. f.process_numpy(process, args = (args.column, args.frequency,
  49. args.min_freq, args.max_freq))
  50. def process(data, interval, args, insert_function, final):
  51. (column, f_expected, f_min, f_max) = args
  52. rows = data.shape[0]
  53. # Estimate sampling frequency from timestamps
  54. fs = 1e6 * (rows-1) / (data[-1][0] - data[0][0])
  55. # Pull out about 3.5 periods of data at once;
  56. # we'll expect to match 3 zero crossings in each window
  57. N = max(int(3.5 * fs / f_expected), 10)
  58. # If we don't have enough data, don't bother processing it
  59. if rows < N:
  60. return 0
  61. # Process overlapping windows
  62. start = 0
  63. num_zc = 0
  64. while start < (rows - N):
  65. this = data[start:start+N, column]
  66. t_min = data[start, 0]/1e6
  67. t_max = data[start+N-1, 0]/1e6
  68. # Do 4-parameter sine wave fit
  69. (A, f0, phi, C) = sfit4(this, fs)
  70. # Check bounds. If frequency is too crazy, ignore this window
  71. if f0 < f_min or f0 > f_max:
  72. print "frequency", f0, "outside valid range", f_min, "-", f_max
  73. start += N
  74. continue
  75. #p.plot(arange(N), this)
  76. #p.plot(arange(N), A * cos(f0/fs * 2 * pi * arange(N) + phi) + C, 'g')
  77. # Period starts when the argument of cosine is 3*pi/2 degrees,
  78. # so we're looking for sample number:
  79. # n = (3 * pi / 2 - phi) / (f0/fs * 2 * pi)
  80. zc_n = (3 * pi / 2 - phi) / (f0 / fs * 2 * pi)
  81. period_n = fs/f0
  82. # Add periods to make N positive
  83. while zc_n < 0:
  84. zc_n += period_n
  85. last_zc = None
  86. # Mark the zero crossings until we're a half period away
  87. # from the end of the window
  88. while zc_n < (N - period_n/2):
  89. #p.plot(zc_n, C, 'ro')
  90. t = t_min + zc_n / fs
  91. insert_function([[t * 1e6, f0, A, C]])
  92. num_zc += 1
  93. last_zc = zc_n
  94. zc_n += period_n
  95. # Advance the window one quarter period past the last marked
  96. # zero crossing, or advance the window by half its size if we
  97. # didn't mark any.
  98. if last_zc is not None:
  99. advance = min(last_zc + period_n/4, N)
  100. else:
  101. advance = N/2
  102. #p.plot(advance, C, 'go')
  103. #p.show()
  104. start = int(round(start + advance))
  105. # Return the number of rows we've processed
  106. print "Marked", num_zc, "zero-crossings in", start, "rows"
  107. return start
  108. def sfit4(data, fs):
  109. """(A, f0, phi, C) = sfit4(data, fs)
  110. Compute 4-parameter (unknown-frequency) least-squares fit to
  111. sine-wave data, according to IEEE Std 1241-2010 Annex B
  112. Input:
  113. data vector of input samples
  114. fs sampling rate (Hz)
  115. Output:
  116. Parameters [A, f0, phi, C] to fit the equation
  117. x[n] = A * cos(f0/fs * 2 * pi * n + phi) + C
  118. where n is sample number. Or, as a function of time:
  119. x(t) = A * cos(f0 * 2 * pi * t + phi) + C
  120. by Jim Paris
  121. (Verified to match sfit4.m)
  122. """
  123. N = len(data)
  124. t = linspace(0, (N-1) / fs, N)
  125. ## Estimate frequency using FFT (step b)
  126. Fc = fft(data)
  127. F = abs(Fc)
  128. F[0] = 0 # eliminate DC
  129. # Find pair of spectral lines with largest amplitude:
  130. # resulting values are in F(i) and F(i+1)
  131. i = argmax(F[0:int(N/2)] + F[1:int(N/2+1)])
  132. # Interpolate FFT to get a better result (from Markus [B37])
  133. U1 = real(Fc[i])
  134. U2 = real(Fc[i+1])
  135. V1 = imag(Fc[i])
  136. V2 = imag(Fc[i+1])
  137. n = 2 * pi / N
  138. ni1 = n * i
  139. ni2 = n * (i+1)
  140. K = ((V2-V1)*sin(ni1) + (U2-U1)*cos(ni1)) / (U2-U1)
  141. Z1 = V1 * (K - cos(ni1)) / sin(ni1) + U1
  142. Z2 = V2 * (K - cos(ni2)) / sin(ni2) + U2
  143. i = arccos((Z2*cos(ni2) - Z1*cos(ni1)) / (Z2-Z1)) / n
  144. # Convert to Hz
  145. f0 = i * fs / N
  146. ## Fit it
  147. # first guess for A0, B0 using 3-parameter fit (step c)
  148. w = 2*pi*f0
  149. D = c_[cos(w*t), sin(w*t), ones(N)]
  150. s = linalg.lstsq(D, data)[0]
  151. # Now iterate 6 times (step i)
  152. for idx in range(6):
  153. D = c_[cos(w*t), sin(w*t), ones(N),
  154. -s[0] * t * sin(w*t) + s[1] * t * cos(w*t) ] # eqn B.16
  155. s = linalg.lstsq(D, data)[0] # eqn B.18
  156. w = w + s[3] # update frequency estimate
  157. ## Extract results
  158. A = sqrt(s[0]*s[0] + s[1]*s[1]) # eqn B.21
  159. f0 = w / (2*pi)
  160. try:
  161. phi = -arctan2(s[1], s[0]) # eqn B.22
  162. except TypeError:
  163. # something broke down, just return zeros
  164. return (0, 0, 0, 0)
  165. C = s[2]
  166. return (A, f0, phi, C)
  167. if __name__ == "__main__":
  168. main()