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.
 
 
 
 

259 lines
8.8 KiB

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