1import json, math, random
  2import numpy as np
  3import torch
  4
  5SEED = 7
  6np.random.seed(SEED); random.seed(SEED); torch.manual_seed(SEED)
  7torch.set_default_dtype(torch.float64)
  8
  9
 10def rot(a):
 11    c, s = torch.cos(a), torch.sin(a)
 12    z = torch.zeros_like(a)
 13    return torch.stack([torch.stack([c, -s], -1), torch.stack([s, c], -1)], -2)
 14
 15
 16def residual(theta, lv, lt, q, y):
 17    # theta = [p_x,p_y,x_x,x_y,psi]
 18    p, x, psi = theta[:2], theta[2:4], theta[4]
 19    R = rot(psi)
 20    r = x + torch.einsum('ij,tj->ti', R, lv) - q
 21    s = p - x - torch.einsum('ij,tj->ti', R, lt) - y
 22    return torch.cat([r.reshape(-1), s.reshape(-1)])
 23
 24
 25def jacobian(theta, lv, lt, q, y):
 26    return torch.autograd.functional.jacobian(
 27        lambda z: residual(z, lv, lt, q, y), theta
 28    ).detach().numpy()
 29
 30
 31def make_window(d, theta_true, T=2, noise=0.0, rng=None):
 32    rng = np.random.default_rng(SEED) if rng is None else rng
 33    lv = np.zeros((T, 2)); lv[:, 0] = np.linspace(0.0, d, T)
 34    lt = np.tile(np.array([0.4, -0.25]), (T, 1))
 35    p, x, psi = theta_true[:2], theta_true[2:4], theta_true[4]
 36    c, s = np.cos(psi), np.sin(psi)
 37    R = np.array([[c, -s], [s, c]])
 38    q = x + lv @ R.T
 39    y = p - x - lt @ R.T
 40    if noise:
 41        q += rng.normal(0, noise, q.shape)
 42        y += rng.normal(0, noise, y.shape)
 43    return tuple(torch.tensor(a) for a in (lv, lt, q, y))
 44
 45
 46def sigma_for(d, T=2):
 47    true = np.array([.7, -.2, .15, .1, .35])
 48    lv, lt, q, y = make_window(d, true, T=T)
 49    th = torch.tensor(true, requires_grad=True)
 50    sv = np.linalg.svd(jacobian(th, lv, lt, q, y), compute_uv=False)
 51    return float(sv[-1]), float(sv[0]), float(np.linalg.matrix_rank(jacobian(th, lv, lt, q, y), tol=1e-9))
 52
 53
 54def optimize(gated, d, T=4, steps=220, noise=.025, seed=0):
 55    rng = np.random.default_rng(seed)
 56    true = np.array([.7, -.2, .15, .1, .35])
 57    lv, lt, q, y = make_window(d, true, T=T, noise=noise, rng=rng)
 58    # Start with a substantially wrong calibration. Optimize only theta, isolating the gate mechanism.
 59    theta = torch.tensor([-.5, .6, .8, -.7, -1.0], requires_grad=True)
 60    opt = torch.optim.Adam([theta], lr=.035)
 61    # threshold calibrated from the well-excited d=1 window
 62    tau = sigma_for(1.0, T=T)[0] * .65
 63    for _ in range(steps):
 64        rr = residual(theta, lv, lt, q, y)
 65        consistency = (rr * rr).mean()
 66        if gated:
 67            J = torch.autograd.functional.jacobian(lambda z: residual(z,lv,lt,q,y), theta, create_graph=False)
 68            sv = torch.linalg.svdvals(J)
 69            sig = sv[-1].detach()
 70            gate = torch.clamp(sig / tau, max=1.0)
 71            # A small hinge penalty follows the proposed objective but does not dominate fitting.
 72            loss = gate * consistency + .02 * torch.relu(torch.tensor(tau) - sv[-1].detach())**2
 73        else:
 74            loss = consistency
 75        opt.zero_grad(); loss.backward(); opt.step()
 76    err = float(torch.linalg.norm(theta.detach() - torch.tensor(true)))
 77    return err, float(theta.detach()[4]), float(consistency.detach())
 78
 79
 80def main():
 81    true = np.array([.7, -.2, .15, .1, .35])
 82    # Prediction 1: d=0 leaves yaw unidentifiable (rank deficient).
 83    rank0 = sigma_for(0.0, T=2)[2]
 84    rank1 = sigma_for(1.0, T=2)[2]
 85    # Prediction 2: for small motion, sigma_min is linear in d.
 86    ds = np.geomspace(.005, 1.0, 9)
 87    sigmas = np.array([sigma_for(float(d), T=2)[0] for d in ds])
 88    slope = float(np.polyfit(np.log(ds), np.log(sigmas), 1)[0])
 89    ratio = float(np.median(sigmas[:5] / ds[:5]))
 90    # Prediction 3: longer windows improve excitation, with diminishing/no worse trend.
 91    lengths = [2, 3, 4, 6, 8]
 92    sigT = [sigma_for(.8, T=T)[0] for T in lengths]
 93    monotone = all(sigT[i+1] >= sigT[i] - 1e-10 for i in range(len(sigT)-1))
 94
 95    # Secondary mini-experiment: difficult low-excitation windows versus ordinary training.
 96    train_ds = [.03, .1, .3, 1.0]
 97    results = {}
 98    for d in train_ds:
 99        b = [optimize(False, d, seed=20+i)[0] for i in range(3)]
100        g = [optimize(True, d, seed=20+i)[0] for i in range(3)]
101        results[str(d)] = {'sigma_min': sigma_for(d,4)[0],
102                           'baseline_error_mean': float(np.mean(b)),
103                           'gated_error_mean': float(np.mean(g))}
104
105    report = {
106      'seed': SEED,
107      'predictions': {
108        'zero_motion_rank_deficiency': {'predicted': 'rank drops below 5 at d=0', 'observed_rank_d0': rank0, 'observed_rank_d1': rank1, 'confirmed': rank0 < 5 and rank1 == 5},
109        'small_motion_scaling': {'predicted': 'sigma_min proportional to d, log-log slope 1', 'observed_loglog_slope': slope, 'median_sigma_over_d': ratio, 'd_values': ds.tolist(), 'sigma_values': sigmas.tolist(), 'confirmed': abs(slope-1) < .08},
110        'window_length': {'predicted': 'sigma_min nondecreasing with T', 'T_values': lengths, 'sigma_values': sigT, 'confirmed': monotone}
111      },
112      'mini_experiment': results,
113      'notes': 'The residual parameterization includes both vehicle and target views. At exactly zero motion, yaw is ambiguous; nonzero motion restores full local rank.'
114    }
115    with open('results.json','w') as f: json.dump(report,f,indent=2)
116    print(json.dumps(report, indent=2))
117
118if __name__ == '__main__': main()