1import numpy as np
 2
 3
 4class HurwitzLatentObserver:
 5    """Euler observer: z_dot = f(z) + G(y - C z)."""
 6    def __init__(self, f, C, G, dt, U_K=None):
 7        self.f = f
 8        self.C = np.asarray(C, dtype=float)
 9        self.G = np.asarray(G, dtype=float)
10        self.dt = float(dt)
11        if U_K is None:
12            _, s, vh = np.linalg.svd(self.C, full_matrices=True)
13            rank = int(np.sum(s > 1e-10))
14            U_K = vh[rank:].T
15        self.U_K = np.asarray(U_K, dtype=float)
16
17    def step(self, z, y):
18        z = np.asarray(z, dtype=float)
19        y = np.asarray(y, dtype=float)
20        return z + self.dt * (self.f(z) + self.G @ (y - self.C @ z))
21
22    def projected_jacobian(self, x, jacobian):
23        J = np.asarray(jacobian(x), dtype=float)
24        return self.U_K.T @ (J - self.G @ self.C) @ self.U_K
25
26    def lyapunov_penalty(self, x, jacobian, P=None, alpha=0.1):
27        H = self.projected_jacobian(x, jacobian)
28        if H.size == 0:
29            return 0.0
30        if P is None:
31            P = np.eye(H.shape[0])
32        P = np.asarray(P, dtype=float)
33        S = H.T @ P + P @ H + 2.0 * alpha * P
34        return float(max(0.0, np.max(np.linalg.eigvalsh((S + S.T) / 2.0))) )