1"""Small, differentiable Fisher-observability utilities."""
 2import torch
 3
 4def fisher_information(jacobians, noise_vars, prior_cov=None, eps=1e-8):
 5    """Accumulate J^T R^-1 J for a sequence of (observation_dim,state_dim) J."""
 6    if not torch.is_tensor(noise_vars):
 7        noise_vars = torch.as_tensor(noise_vars, dtype=jacobians[0].dtype, device=jacobians[0].device)
 8    d = jacobians[0].shape[-1]
 9    I = torch.zeros((d, d), dtype=jacobians[0].dtype, device=jacobians[0].device)
10    inv = torch.diag(1.0 / noise_vars)
11    for J in jacobians:
12        I = I + J.T @ inv @ J
13    if prior_cov is not None:
14        I = I + torch.linalg.inv(prior_cov)
15    return I
16
17def observability_regularizer(I, alpha=1.0, beta=0.0, eps=1e-6):
18    """The proposed -logdet plus optional smooth condition-number penalty."""
19    eig = torch.linalg.eigvalsh(I)
20    logdet = torch.logdet(I + eps * torch.eye(I.shape[-1], device=I.device, dtype=I.dtype))
21    cond = eig[-1] / (eig[0] + eps)
22    return alpha * (-logdet) + beta * cond
23
24def bearing_brightness_jacobian(state, brightness=True, c=1.0):
25    x, y = state[0], state[1]
26    r2 = x*x + y*y
27    Hb = torch.stack((-y/r2, x/r2, torch.zeros_like(x), torch.zeros_like(x))).reshape(1,4)
28    if not brightness:
29        return Hb
30    r4 = r2*r2
31    Hl = torch.stack((-2*c*x/r4, -2*c*y/r4, torch.zeros_like(x), torch.zeros_like(x))).reshape(1,4)
32    return torch.cat((Hb, Hl), dim=0)