1import json
 2import math
 3from pathlib import Path
 4import numpy as np
 5
 6SEED = 2961
 7DT = 0.002
 8
 9
10def exp_filter(signal, a, dt=DT, z0=0.0):
11    """Exact zero-order-hold update for zdot=-a*z+a*q."""
12    beta = math.exp(-a * dt)
13    z = float(z0)
14    out = np.empty(len(signal))
15    for i, q in enumerate(signal):
16        z = beta * z + (1.0 - beta) * float(q)
17        out[i] = z
18    return out
19
20
21def frequency_check():
22    rows = []
23    for a in [0.3, 1.0, 3.0]:
24        for ratio in [2.0, 5.0, 10.0]:
25            w = ratio * a
26            dt = 0.001
27            n = int(max(20 * (2 * math.pi / w) / dt, 12000))
28            t = np.arange(n) * dt
29            z = exp_filter(np.sin(w * t), a, dt)
30            cut = n // 3
31            amp = 2 * abs(np.mean(z[cut:] * np.exp(-1j * w * t[cut:])))
32            theory = a / math.sqrt(a * a + w * w)
33            rows.append({"a": a, "omega": w, "omega_over_a": ratio,
34                         "measured": float(amp), "theory": float(theory),
35                         "relative_error": float(abs(amp - theory) / theory)})
36    return rows
37
38
39def run_controller(kind, a=3.0, noise_amp=0.08, omega=30.0, episodes=20):
40    # Scalar safe system xdot=u, h=x. Observation noise affects the estimated q.
41    # The direct h>=delta safeguard is implemented as a one-step viability clamp.
42    alpha, eps, umax, kappa, delta = 1.0, 0.015, 1.0, a / 2.0, 0.01
43    decay = math.exp(-a * DT)
44    all_tv, all_minx, all_viol, all_infeas = [], [], [], []
45    for ep in range(episodes):
46        phase = 0.37 * ep
47        x, z = 0.22, 0.0
48        actions, xs = [], []
49        for i in range(3000):
50            t = i * DT
51            obs = x + noise_amp * math.sin(omega * t + phase)
52            # Nominal policy is intentionally sensitive to the noisy observation.
53            u_pi = -0.42 - 0.75 * obs
54            qhat_pi = u_pi + alpha * obs
55            if kind == "memoryless":
56                lower = -alpha * obs + eps
57            else:
58                lower = -alpha * obs + (1.0 - kappa / a) * z + eps
59            infeasible = lower > umax
60            u = float(np.clip(max(u_pi, lower), -umax, umax))
61            # Explicit direct margin h>=delta, represented by a one-step clamp.
62            if x <= delta:
63                u = max(u, 0.0)
64            q_exec = u + alpha * x
65            if kind == "filtered":
66                # Exactly one state update, using the executed residual.
67                z = decay * z + (1.0 - decay) * q_exec
68            x += DT * u
69            actions.append(u)
70            xs.append(x)
71            all_infeas.append(float(infeasible))
72        all_tv.append(float(np.sum(np.abs(np.diff(actions)))))
73        all_minx.append(float(np.min(xs)))
74        all_viol.append(float(np.mean(np.asarray(xs) < 0.0)))
75    return {"tv_mean": float(np.mean(all_tv)), "tv_std": float(np.std(all_tv)),
76            "min_x_mean": float(np.mean(all_minx)), "violation_rate": float(np.mean(all_viol)),
77            "infeasible_rate": float(np.mean(all_infeas))}
78
79
80def main():
81    freq = frequency_check()
82    comparison = {}
83    for a in [0.3, 1.0, 3.0, 10.0]:
84        comparison[str(a)] = {
85            "memoryless": run_controller("memoryless", a=a, omega=30.0),
86            "filtered": run_controller("filtered", a=a, omega=30.0),
87        }
88    result = {"seed": SEED, "dt": DT, "frequency_response": freq,
89              "toy_comparison": comparison}
90    Path("results.json").write_text(json.dumps(result, indent=2))
91    print(json.dumps(result, indent=2))
92
93
94if __name__ == "__main__":
95    main()