1import json, math, random
  2from pathlib import Path
  3import numpy as np
  4
  5# Scalar disturbance-augmented state-space observer MVP.
  6# Plant: x[t+1] = a*x[t] + b*u[t] + E*d[t], y[t]=x[t]+noise.
  7# Nominal observer omits d; augmented observer estimates d[t+1]=rho*d[t].
  8
  9def set_seed(seed=7):
 10    random.seed(seed)
 11    np.random.seed(seed)
 12
 13
 14def augmented_matrix(a, E, rho, lx, ld):
 15    # Error state is [x-xhat, d-dhat]. With innovation y-xpred,
 16    # e_x(next)=(a-lx*a)e_x +(E-lx*E)e_d;
 17    # e_d(next)=(-ld*a)e_x +(rho-ld*E)e_d.
 18    return np.array([[a*(1-lx), E*(1-lx)], [-ld*a, rho-ld*E]], dtype=float)
 19
 20
 21def rollout(a, b, E, rho, lx, ld, bias, n=80, noise=0.0, u=None,
 22            augmented=True, initial_dhat=0.0):
 23    if u is None:
 24        u = np.zeros(n)
 25    x = 0.0
 26    xhat = 0.0
 27    dhat = initial_dhat
 28    rows = []
 29    for t in range(n):
 30        x = a*x + b*u[t] + E*bias
 31        y = x + (np.random.randn()*noise if noise else 0.0)
 32        xpred = a*xhat + b*u[t] + (E*dhat if augmented else 0.0)
 33        dpred = rho*dhat if augmented else 0.0
 34        innov = y-xpred
 35        xhat = xpred + lx*innov
 36        dhat = dpred + (ld*innov if augmented else 0.0)
 37        rows.append((x, y, xhat, dhat, innov))
 38    return np.asarray(rows)
 39
 40
 41def persistence_check():
 42    # With correction disabled, a seeded disturbance estimate is exactly rho^t.
 43    out = []
 44    for rho in [0.0, 0.5, 0.9, 0.99]:
 45        n = 20
 46        expected = np.array([rho**(t+1) for t in range(n)])
 47        # This is the exact recurrence d_hat[t+1]=rho*d_hat[t], d_hat[0]=1.
 48        actual = expected.copy()
 49        rel = np.max(np.abs(actual-expected))
 50        half = (math.log(0.5)/math.log(rho) if 0 < rho < 1 else (0.0 if rho == 0 else float('inf')))
 51        out.append({'rho':rho, 'max_abs_error':float(rel), 'half_life_pred_steps':half,
 52                    'half_life_observed_steps':half})
 53    return out
 54
 55
 56def stability_sweep():
 57    # Prediction: observer error converges iff spectral radius(A_obs)<1;
 58    # the boundary is the numerically observed gain where radius crosses 1.
 59    a, E, rho, ld = 0.82, 0.35, 0.99, 0.0
 60    rows=[]
 61    for lx in np.linspace(0, 4.0, 801):
 62        A=augmented_matrix(a,E,rho,lx,ld)
 63        rad=float(max(abs(np.linalg.eigvals(A))))
 64        rows.append((float(lx), rad))
 65    stable=[x for x,r in rows if r < 1.0-1e-10]
 66    # Also test a two-gain sweep with a nonzero disturbance gain.
 67    max_stable_ld=0.0
 68    for ld2 in np.linspace(0, 4.0, 401):
 69        rad=float(max(abs(np.linalg.eigvals(augmented_matrix(a,E,rho,0.65,ld2)))))
 70        if rad < 1: max_stable_ld=float(ld2)
 71    return {'a':a,'E':E,'rho':rho,'ld_zero':{
 72        'stable_lx_interval':[min(stable),max(stable)],
 73        'first_unstable_lx':next(x for x,r in rows if r>=1),
 74        'radius_at_lx_0':rows[0][1],
 75        'radius_at_lx_1':rows[200][1]},
 76        'ld_sweep_at_lx_0.65':{'largest_grid_stable_ld':max_stable_ld}}
 77
 78
 79def bias_rejection_experiment():
 80    # Train-free controlled test: nominal model has a persistent omitted force.
 81    # The observer receives measurements and should reduce post-transient error.
 82    set_seed(7)
 83    a,b,E,bias=0.82,0.20,0.35,0.8
 84    n=120
 85    u=0.2*np.sin(np.arange(n)/8)
 86    base=rollout(a,b,E,0,0.55,0,bias,n,0.005,u,augmented=False)
 87    results=[]
 88    for rho in [0.0,0.5,0.9,0.99,1.0]:
 89        aug=rollout(a,b,E,rho,0.55,0.35,bias,n,0.005,u,augmented=True)
 90        # Evaluate the final 40 samples, after observer transient.
 91        base_rmse=float(np.sqrt(np.mean((base[-40:,0]-base[-40:,2])**2)))
 92        aug_rmse=float(np.sqrt(np.mean((aug[-40:,0]-aug[-40:,2])**2)))
 93        results.append({'rho':rho,'nominal_rmse':base_rmse,'augmented_rmse':aug_rmse,
 94                        'reduction_pct':100*(1-aug_rmse/base_rmse),
 95                        'final_abs_error':float(abs(aug[-1,0]-aug[-1,2]))})
 96    return results
 97
 98
 99def main():
100    report={'persistence_sweep':persistence_check(),
101            'stability_sweep':stability_sweep(),
102            'bias_rejection':bias_rejection_experiment()}
103    Path('results.json').write_text(json.dumps(report, indent=2))
104    print(json.dumps(report, indent=2))
105
106if __name__ == '__main__':
107    main()