From 1cc80e1a7a08d1d669f386ba636ad17194e692e1 Mon Sep 17 00:00:00 2001
From: infiniteCable <75014343+infiniteCable@users.noreply.github.com>
Date: Fri, 11 Apr 2025 19:30:56 +0200
Subject: [PATCH] Create pt2.py

---
 common/pt2.py | 112 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 common/pt2.py

diff --git a/common/pt2.py b/common/pt2.py
new file mode 100644
index 000000000..14270619f
--- /dev/null
+++ b/common/pt2.py
@@ -0,0 +1,112 @@
+import math
+
+
+class PT2Filter:
+  """
+  Diskreter PT2-Filter mittels Tustin (bilinear transform)
+  Übertragungsfunktion (kontinuierlich):
+      H(s) =  (w0^2) / (s^2 + 2*zeta*w0*s + w0^2)
+  Abtastzeit dt.
+  """
+  def __init__(self, w0: float, zeta: float, dt: float):
+    """
+    w0:   Eigenkreisfrequenz [rad/s], bestimmt u.a. die Anstiegszeit
+    zeta: Dämpfungsgrad (zeta=1 => kritisch gedämpft)
+    dt:   Abtastzeit [s]
+    """
+    self.w0   = w0
+    self.zeta = zeta
+    self.dt   = dt
+    self.a1, self.a2, self.b0, self.b1, self.b2 = self._design_pt2(self.w0, self.zeta, self.dt)
+    self.y1 = 0.0
+    self.y2 = 0.0
+    self.u1 = 0.0
+    self.u2 = 0.0
+
+  def _design_pt2(self, w0, zeta, dt):
+    """
+    Erzeuge (a1, a2, b0, b1, b2) aus:
+       H(s) = w0^2 / [s^2 + 2*zeta*w0 s + w0^2]
+    via Tustin (s = (2/dt)*(1-z^-1)/(1+z^-1)).
+
+    Wir bringen G(z) in die Normalform:
+        Y(z)/U(z) = (b0 + b1 z^-1 + b2 z^-2) / (1 + a1 z^-1 + a2 z^-2).
+
+    => Zeitbereich: y[k] = -a1*y[k-1] - a2*y[k-2] + b0*u[k] + b1*u[k-1] + b2*u[k-2].
+    """
+        
+    Ts = dt
+    wd = w0
+    alpha = 2.0 / Ts
+    b2_ = w0**2
+    b1_ = 2.0 * w0**2
+    b0_ = w0**2
+    A2_f1 = alpha**2
+    A1_f1 = -2.0 * alpha**2
+    A0_f1 = alpha**2
+    factor2 = 2.0*zeta*wd*alpha
+    A2_f2 = factor2
+    A1_f2 = 0.0
+    A0_f2 = -factor2
+    A2_f3 = wd**2
+    A1_f3 = 2.0*(wd**2)
+    A0_f3 = wd**2
+    A2 = A2_f1 + A2_f2 + A2_f3
+    A1 = A1_f1 + A1_f2 + A1_f3
+    A0 = A0_f1 + A0_f2 + A0_f3
+    B2 = b2_
+    B1 = b1_
+    B0 = b0_
+    b2d = B2 / A2
+    b1d = B1 / A2
+    b0d = B0 / A2
+    a2d = A0 / A2
+    a1d = A1 / A2
+
+    return (a1d, a2d, b0d, b1d, b2d)
+
+  def sync(self, target: float):
+    steps = compute_saturation_steps(self.w0, self.zeta, self.dt)
+    for i in range(1, steps + 1):
+      update(target)
+
+  def compute_saturation_steps(self, w0: float, zeta: float, dt: float) -> int:
+    """
+    Berechnet eine Abschätzung der Schritte, bis der Filter (95% des Endwerts) erreicht ist.
+    
+    Wir nutzen hier die Abschätzung:
+        T_s = 4 / (zeta * w0)
+    und setzen N = T_s / dt.
+    """
+    Ts = 4.0 / (zeta * w0)
+    N = Ts / dt
+    return math.ceil(N)
+
+  def reset(self):
+    """
+    Setzt interne Zustände zurück
+    """
+    self.y1 = 0.0
+    self.y2 = 0.0
+    self.u1 = 0.0
+    self.u2 = 0.0
+
+  def update(self, u: float) -> float:
+    """
+    Ein Schritt des Filters mit aktuellem Eingang u.
+    Gibt Ausgang y zurück.
+    """
+    y = (
+        - self.a1 * self.y1
+        - self.a2 * self.y2
+        + self.b0 * u
+        + self.b1 * self.u1
+        + self.b2 * self.u2
+    )
+        
+    self.y2 = self.y1
+    self.y1 = y
+    self.u2 = self.u1
+    self.u1 = u
+        
+    return y