# Utils/Geolocation

Mathematical building blocks for geolocation solvers.

This module provides the low-level math that geolocation algorithms build on: a generic weighted least squares solver, analytical covariance prediction, and initial guess generation. Observable-specific locators (TDOA, AOA, FDOA, etc.) that wrap these primitives live in gri-geosim.

## Import and Use

```python
from gri_utils import geolocation

cov = geolocation.predicted_covariance(jacobian, R)
```

Or with a unique name:

```python
from gri_utils import geolocation as gri_geo

cov = gri_geo.predicted_covariance(jacobian, R)
```

## Module Overview

| Function | Purpose |
|----------|---------|
| `whitened_least_squares` | Generic Cholesky-whitened nonlinear least squares solver |
| `predicted_covariance` | Analytical solution covariance from Jacobian and noise (GDOP) |
| `default_initial_guess` | Project collector centroid onto Earth surface |

## Whitened Least Squares

Solves weighted nonlinear least squares problems by transforming the residual and Jacobian through Cholesky whitening. This handles correlated measurements (off-diagonal covariance terms) and produces a solution covariance and chi-squared fit statistic.

The solver minimizes:

    chi^2(x) = r(x).T @ R^{-1} @ r(x)

by whitening with the Cholesky factor L of R (where R = L @ L.T):

    r_white = L^{-1} @ r(x)
    J_white = L^{-1} @ J(x)

```python
from gri_utils.geolocation import whitened_least_squares

def my_residual_and_jacobian(x):
    # Compute residual vector r(x) and Jacobian J(x) at candidate x
    return residual, jacobian

# Full measurement covariance (can have off-diagonal correlation)
R = np.array([[1e-18, 0.5e-18, 0.0],
              [0.5e-18, 1e-18, 0.0],
              [0.0, 0.0, 1e-18]])

solution, sol_cov, chi_sq = whitened_least_squares(
    my_residual_and_jacobian, x0, R
)
```

The user provides:

- A function that returns `(residual, jacobian)` for a given state vector
- An initial guess `x0`
- The measurement covariance matrix `R`

Returns `(solution, solution_covariance, chi_squared)`. Additional keyword arguments are passed through to `scipy.optimize.least_squares`.

## Predicted Covariance

Analytically predicts the solution covariance from geometry and measurement uncertainty without running a solver. This is the GDOP (Geometric Dilution of Precision) computation:

    C = (J^T R^{-1} J)^{-1}

where J is the measurement Jacobian and R is the measurement covariance.

```python
from gri_utils.observables import tdoa_with_gradient
from gri_utils.geolocation import predicted_covariance
import numpy as np

# Evaluate Jacobian at candidate geometry
target = np.array([6378137.0, 100.0, 100.0])
c1 = np.array([[6378137.0, 1000.0, 0.0],
               [6378137.0, 0.0, 1000.0],
               [6378137.0, -1000.0, 0.0]])
c2 = np.array([[6378137.0, -1000.0, 0.0],
               [6378137.0, 0.0, -1000.0],
               [6378137.0, 1000.0, 0.0]])

jacobian = np.empty((3, 3))
for i in range(3):
    _, jacobian[i] = tdoa_with_gradient(target, c1[i], c2[i])

# 10 ns timing uncertainty, independent measurements
R = np.diag(np.full(3, (10e-9)**2))

# Predicted position covariance (3x3, ECEF)
cov = predicted_covariance(jacobian, R)
```

Use this to:

- Compare sensor configurations without Monte Carlo simulation
- Compute GDOP/PDOP/HDOP/VDOP from the diagonal of the solution covariance
- Evaluate whether a system design meets accuracy requirements
- Perform sensitivity analysis by sweeping geometry or noise parameters

## Default Initial Guess

Provides a starting point for iterative solvers when no prior target estimate is available. Computes the centroid of collector positions and projects it to the mean Earth radius along the radial direction.

```python
from gri_utils.geolocation import default_initial_guess

x0 = default_initial_guess(collector_xyz)  # (N, 3) -> (3,)
```

## Notes

- All positions are in ECEF (XYZ) coordinates in meters.
- The whitened least squares solver defaults to the Levenberg-Marquardt method (`method="lm"`) but accepts any method supported by `scipy.optimize.least_squares`.
- For observable-specific locators that build residual/Jacobian functions from TDOA, AOA, FDOA, or hybrid measurements, see gri-geosim.
