From 2c9cec24ebf120ea52bd716bda1e2917fbfad7da Mon Sep 17 00:00:00 2001
From: Roman Zeyde <roman.zeyde@gmail.com>
Date: Fri, 4 Jul 2014 12:50:41 +0300
Subject: [PATCH] add loop module

---
 loop.py | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100644 loop.py

diff --git a/loop.py b/loop.py
new file mode 100644
index 0000000..3525383
--- /dev/null
+++ b/loop.py
@@ -0,0 +1,40 @@
+import numpy as np
+
+class Filter(object):
+    def __init__(self, b, a=()):
+        self.b = b
+        self.a = a
+        self.x = [0] * len(b)
+        self.y = [0] * len(a)
+
+    def __call__(self, x):
+        self.x = [x] + self.x
+        self.x = self.x[:len(self.b)]
+        self.y = self.y[:len(self.a)]
+        y = np.dot(self.x, self.b) + np.dot(self.y, self.a)
+        self.y = [y] + self.y
+        return y
+
+class Loop(object):
+    def __init__(self, x, A, k, h):
+        self.x = np.array(x)
+        self.A = np.array(A)
+        self.k = np.array(k)
+        self.h = np.array(h)
+
+    def __call__(self, y):
+        self.err = y - np.dot(self.h, self.x)
+        self.dx = self.k * self.err
+        self.x = self.x + self.dx
+        self.x = np.dot(self.A, self.x)
+
+class Integrator(Loop):
+    def __init__(self, phase, freq, k):
+        x = [phase, freq]  # state variable vector
+        # evolution matrix:
+        # | phase' = phase + freq
+        # | freq' = freq
+        A = [[1, 1], [0, 1]]
+        h = [1, 0]  # phase = dot(h, x)
+        Loop.__init__(self, x=x, A=A, k=k, h=h)
+