diff --git a/dmi.py b/dmi.py
index 3e49b6d..5759c44 100644
--- a/dmi.py
+++ b/dmi.py
@@ -49,8 +49,11 @@ def main():
                             # Update the MEAS GUI at the same time so it's synchronized
                             gui.update_meas(samples_meas)
 
-                        #position, leakage = position_tracker.input(samples_ref, samples_meas)
-                        #print(np.sum(position)/len(position), leakage)
+                        if stabilizer.locked():
+                            position, leakage = position_tracker.input(samples_ref, samples_meas)
+                            print(np.sum(position)/len(position), leakage)
+                        else:
+                            position_tracker.reset()
                     finally:
                         buf_sdr.dispose(buffers)
             finally:
diff --git a/noptica.py b/noptica.py
index 7c8921b..aab8ca2 100644
--- a/noptica.py
+++ b/noptica.py
@@ -170,9 +170,13 @@ def continuous_unwrap(last_phase, last_phase_unwrapped, p):
 
 class PositionTracker:
     def __init__(self, leakage_avg):
+        self.leakage_avg = leakage_avg
+        self.reset()
+
+    def reset(self):
         self.last_phase = 0.0
         self.last_position = 0.0
-        self.leakage = np.zeros(leakage_avg)
+        self.leakage = np.zeros(self.leakage_avg)
         self.leakage_ptr = 0
 
     def input(self, ref, meas):