From f530edd8a08e3f57e40aee7ed5b3719600263af9 Mon Sep 17 00:00:00 2001
From: Jim Paris <jim@jtan.com>
Date: Fri, 16 Aug 2013 15:36:39 -0400
Subject: [PATCH] sfit4: if interpolated DFT fails, use peak

---
 nilmtools/math.py | 26 +++++++++++++++-----------
 1 file changed, 15 insertions(+), 11 deletions(-)

diff --git a/nilmtools/math.py b/nilmtools/math.py
index 6bef097..a08358f 100644
--- a/nilmtools/math.py
+++ b/nilmtools/math.py
@@ -37,17 +37,21 @@ def sfit4(data, fs):
     i = argmax(F[0:int(N/2)] + F[1:int(N/2+1)])
 
     # Interpolate FFT to get a better result (from Markus [B37])
-    U1 = real(Fc[i])
-    U2 = real(Fc[i+1])
-    V1 = imag(Fc[i])
-    V2 = imag(Fc[i+1])
-    n = 2 * pi / N
-    ni1 = n * i
-    ni2 = n * (i+1)
-    K = ((V2-V1)*sin(ni1) + (U2-U1)*cos(ni1)) / (U2-U1)
-    Z1 = V1 * (K - cos(ni1)) / sin(ni1) + U1
-    Z2 = V2 * (K - cos(ni2)) / sin(ni2) + U2
-    i = arccos((Z2*cos(ni2) - Z1*cos(ni1)) / (Z2-Z1)) / n
+    try:
+        U1 = real(Fc[i])
+        U2 = real(Fc[i+1])
+        V1 = imag(Fc[i])
+        V2 = imag(Fc[i+1])
+        n = 2 * pi / N
+        ni1 = n * i
+        ni2 = n * (i+1)
+        K = ((V2-V1)*sin(ni1) + (U2-U1)*cos(ni1)) / (U2-U1)
+        Z1 = V1 * (K - cos(ni1)) / sin(ni1) + U1
+        Z2 = V2 * (K - cos(ni2)) / sin(ni2) + U2
+        i = arccos((Z2*cos(ni2) - Z1*cos(ni1)) / (Z2-Z1)) / n
+    except Exception:
+        # Just go with the biggest FFT peak
+        i = argmax(F[0:int(N/2)])
 
     # Convert to Hz
     f0 = i * float(fs) / N