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.
 
 
 
 

164 lines
5.0 KiB

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