astrodynamics-gnss 0.9.2

//! Dilution of precision (DOP) from a satellite geometry.
//!
//! Dilution of precision summarises how the receiver-to-satellite geometry maps
//! measurement noise into solution uncertainty. From a design (geometry) matrix
//! `H` whose rows are the unit line-of-sight vectors plus a clock column, and a
//! diagonal weight matrix `W`, the cofactor matrix is
//!
//! ```text
//! Q = (H^T W H)^-1
//! ```
//!
//! a 4x4 symmetric matrix ordered `[x, y, z, clock]` (the position block in
//! ECEF metres, the clock state in the same length unit as the ranges). The DOP
//! scalars are square roots of sums of diagonal cofactor entries. The
//! horizontal/vertical split is taken after rotating the 3x3 position block into
//! a local east-north-up (ENU) frame at the receiver's geodetic
//! latitude/longitude.
//!
//! # Reproducibility
//!
//! The normal matrix `H^T W H` is accumulated by a plain left-to-right sum over
//! the satellites, and the 4x4 inverse is an explicit cofactor (adjugate /
//! determinant) expansion with a fixed term order rather than a LAPACK
//! factorisation. That keeps the whole computation libm/arithmetic-bound and
//! independently reproducible to the bit (it does not depend on a BLAS backend),
//! unlike a general dense inverse routed through LAPACK. The ENU rotation uses
//! `sin`/`cos` and the final scalars use `sqrt`; there is no fused multiply-add.
//!
//! # Failure mode
//!
//! A geometry with fewer than four independent line-of-sight directions, or one
//! whose normal matrix is singular or ill-conditioned, has no finite DOP. Such
//! geometries are reported as [`DopError::Singular`] rather than returning a
//! NaN-flagged or clamped result. The predicate is deterministic: the
//! determinant is exactly zero, or one of the variance diagonals that a DOP
//! scalar takes the square root of is negative or non-finite.

use crate::frame::Wgs84Geodetic;

/// A line-of-sight unit vector from the receiver toward a satellite, in ECEF.
///
/// The corresponding design-matrix row is `[-e_x, -e_y, -e_z, 1]`: the partial
/// derivative of the predicted range with respect to the receiver position is
/// `-e`, and the clock column is one (range increases one-for-one with the
/// receiver clock bias expressed as a length).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LineOfSight {
    /// ECEF X component of the unit line-of-sight vector.
    pub e_x: f64,
    /// ECEF Y component of the unit line-of-sight vector.
    pub e_y: f64,
    /// ECEF Z component of the unit line-of-sight vector.
    pub e_z: f64,
}

impl LineOfSight {
    /// Construct a line-of-sight unit vector from ECEF components.
    pub const fn new(e_x: f64, e_y: f64, e_z: f64) -> Self {
        Self { e_x, e_y, e_z }
    }

    /// The design-matrix row `[-e_x, -e_y, -e_z, 1]` for this direction.
    fn design_row(self) -> [f64; 4] {
        [-self.e_x, -self.e_y, -self.e_z, 1.0]
    }
}

/// The dilution-of-precision scalars for a geometry.
///
/// Each is dimensionless: the standard deviation of the solution component is
/// the corresponding DOP times the (range) measurement standard deviation. The
/// position split is in the local ENU frame at the receiver.
///
/// Produced by [`dop`] for a single receiver-clock state and by the positioning
/// pipeline's multi-clock geometry path for a multi-system state; the field
/// meanings below cover both.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Dop {
    /// Geometric DOP: the square root of the trace of the cofactor matrix over
    /// every state — the three position coordinates and every clock (one for a
    /// single-system solve, one per constellation for a multi-system solve).
    pub gdop: f64,
    /// Position DOP: `sqrt(qE + qN + qU)` over the ENU position block.
    pub pdop: f64,
    /// Horizontal DOP: `sqrt(qE + qN)`.
    pub hdop: f64,
    /// Vertical DOP: `sqrt(qU)`.
    pub vdop: f64,
    /// Time (clock) DOP: the square root of the reference clock's cofactor
    /// variance (`Q[3][3]`). With several clocks this is the first (reference)
    /// system's clock; the others enter `gdop` through the trace.
    pub tdop: f64,
}

/// Why a geometry has no finite DOP.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum DopError {
    /// Fewer line-of-sight directions than estimated parameters were supplied
    /// (four for a single clock, `3 + n_clocks` for several), so the normal
    /// matrix cannot be full rank.
    TooFewSatellites,
    /// The normal matrix `H^T W H` is singular or ill-conditioned: its
    /// determinant is zero, or a variance diagonal is negative or non-finite.
    Singular,
}

impl core::fmt::Display for DopError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            DopError::TooFewSatellites => {
                write!(
                    f,
                    "fewer satellites than parameters: geometry is rank-deficient"
                )
            }
            DopError::Singular => {
                write!(f, "singular or ill-conditioned geometry: no finite DOP")
            }
        }
    }
}

impl std::error::Error for DopError {}

// --- 4x4 / 3x3 linear algebra with a pinned operation order -----------------

/// Form the symmetric normal matrix `A = H^T W H` (4x4).
///
/// Each entry is accumulated left-to-right over the satellites in input order;
/// the term is `h[k][i] * w[k] * h[k][j]` with that exact grouping (the weight
/// applied between the two design entries), matching the reference recipe.
#[allow(clippy::needless_range_loop)] // explicit indices keep the H^T W H summation order legible and pinned
fn normal_matrix(rows: &[[f64; 4]], weights: &[f64]) -> [[f64; 4]; 4] {
    let mut a = [[0.0_f64; 4]; 4];
    for i in 0..4 {
        for j in 0..4 {
            let mut s = 0.0_f64;
            for k in 0..rows.len() {
                s += rows[k][i] * weights[k] * rows[k][j];
            }
            a[i][j] = s;
        }
    }
    a
}

/// Determinant of a 4x4 by cofactor expansion along the first row, with a fixed
/// term order matching the reference recipe.
fn det4(a: &[[f64; 4]; 4]) -> f64 {
    let m01 = a[2][0] * a[3][1] - a[2][1] * a[3][0];
    let m02 = a[2][0] * a[3][2] - a[2][2] * a[3][0];
    let m03 = a[2][0] * a[3][3] - a[2][3] * a[3][0];
    let m12 = a[2][1] * a[3][2] - a[2][2] * a[3][1];
    let m13 = a[2][1] * a[3][3] - a[2][3] * a[3][1];
    let m23 = a[2][2] * a[3][3] - a[2][3] * a[3][2];

    let c0 = a[1][1] * m23 - a[1][2] * m13 + a[1][3] * m12;
    let c1 = a[1][0] * m23 - a[1][2] * m03 + a[1][3] * m02;
    let c2 = a[1][0] * m13 - a[1][1] * m03 + a[1][3] * m01;
    let c3 = a[1][0] * m12 - a[1][1] * m02 + a[1][2] * m01;

    a[0][0] * c0 - a[0][1] * c1 + a[0][2] * c2 - a[0][3] * c3
}

/// Determinant of the 3x3 obtained by deleting row `skip_r` and column
/// `skip_c`, with the surviving rows/cols in ascending order and a fixed
/// first-row expansion.
fn minor3(a: &[[f64; 4]; 4], skip_r: usize, skip_c: usize) -> f64 {
    let mut rows = [0usize; 3];
    let mut ri = 0;
    for r in 0..4 {
        if r != skip_r {
            rows[ri] = r;
            ri += 1;
        }
    }
    let mut cols = [0usize; 3];
    let mut ci = 0;
    for c in 0..4 {
        if c != skip_c {
            cols[ci] = c;
            ci += 1;
        }
    }
    let b = [
        [
            a[rows[0]][cols[0]],
            a[rows[0]][cols[1]],
            a[rows[0]][cols[2]],
        ],
        [
            a[rows[1]][cols[0]],
            a[rows[1]][cols[1]],
            a[rows[1]][cols[2]],
        ],
        [
            a[rows[2]][cols[0]],
            a[rows[2]][cols[1]],
            a[rows[2]][cols[2]],
        ],
    ];
    b[0][0] * (b[1][1] * b[2][2] - b[1][2] * b[2][1])
        - b[0][1] * (b[1][0] * b[2][2] - b[1][2] * b[2][0])
        + b[0][2] * (b[1][0] * b[2][1] - b[1][1] * b[2][0])
}

/// Explicit cofactor (adjugate / determinant) inverse of a 4x4 matrix.
///
/// Returns `None` when the determinant is exactly zero. The cofactor of entry
/// `(i, j)` is `(-1)^(i+j)` times the `(i, j)` minor; the inverse is the
/// transpose of the cofactor matrix divided by the determinant, so
/// `inv[j][i] = cofactor(i, j) / det`.
#[allow(clippy::needless_range_loop)] // explicit (i, j) indices mirror the cofactor expansion
fn inv4(a: &[[f64; 4]; 4]) -> Option<[[f64; 4]; 4]> {
    let det = det4(a);
    if det == 0.0 {
        return None;
    }
    let mut inv = [[0.0_f64; 4]; 4];
    for i in 0..4 {
        for j in 0..4 {
            let minor = minor3(a, i, j);
            let sign = if (i + j) % 2 == 0 { 1.0 } else { -1.0 };
            let cof = sign * minor;
            inv[j][i] = cof / det;
        }
    }
    Some(inv)
}

/// ECEF -> ENU rotation matrix at geodetic latitude/longitude (radians).
fn ecef_to_enu_rotation(lat_rad: f64, lon_rad: f64) -> [[f64; 3]; 3] {
    let sphi = lat_rad.sin();
    let cphi = lat_rad.cos();
    let slam = lon_rad.sin();
    let clam = lon_rad.cos();
    [
        [-slam, clam, 0.0],
        [-sphi * clam, -sphi * slam, cphi],
        [cphi * clam, cphi * slam, sphi],
    ]
}

/// Rotate the 3x3 position cofactor block: `Q_enu = R Q_xyz R^T`, formed as
/// `(R Q) R^T` with plain left-to-right inner sums in a fixed order.
#[allow(clippy::needless_range_loop)] // explicit indices keep the R Q R^T product order pinned
fn rotate_pos_block(q: &[[f64; 4]; 4], r: &[[f64; 3]; 3]) -> [[f64; 3]; 3] {
    let mut rq = [[0.0_f64; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            let mut s = 0.0_f64;
            for k in 0..3 {
                s += r[i][k] * q[k][j];
            }
            rq[i][j] = s;
        }
    }
    let mut enu = [[0.0_f64; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            let mut s = 0.0_f64;
            for k in 0..3 {
                s += rq[i][k] * r[j][k];
            }
            enu[i][j] = s;
        }
    }
    enu
}

/// Compute the DOP scalars from line-of-sight directions, diagonal weights, and
/// the receiver geodetic position.
///
/// `los` and `weights` must have the same length, which must be at least four;
/// `weights` are the diagonal of `W`. Returns [`DopError::TooFewSatellites`] for
/// fewer than four directions and [`DopError::Singular`] for a singular or
/// ill-conditioned geometry (see the module docs for the exact predicate).
pub fn dop(los: &[LineOfSight], weights: &[f64], receiver: Wgs84Geodetic) -> Result<Dop, DopError> {
    assert_eq!(
        los.len(),
        weights.len(),
        "line-of-sight and weight counts differ"
    );
    if los.len() < 4 {
        return Err(DopError::TooFewSatellites);
    }

    let rows: Vec<[f64; 4]> = los.iter().map(|l| l.design_row()).collect();
    let a = normal_matrix(&rows, weights);
    let q = inv4(&a).ok_or(DopError::Singular)?;

    let r = ecef_to_enu_rotation(receiver.lat_rad, receiver.lon_rad);
    let enu = rotate_pos_block(&q, &r);

    let qe = enu[0][0];
    let qn = enu[1][1];
    let qu = enu[2][2];
    let qt = q[3][3];

    // The DOP scalars take the square root of cofactor variances. A
    // well-posed geometry yields a positive-definite Q with strictly positive
    // variance diagonals; a rank-deficient or ill-conditioned geometry can
    // leave a tiny nonzero determinant (so `inv4` succeeds) yet produce a
    // negative or non-finite variance. Reject that here rather than returning a
    // NaN-flagged DOP. The same deterministic predicate is applied by the
    // reference recipe so both agree on the success/failure boundary.
    let gdop_arg = q[0][0] + q[1][1] + q[2][2] + q[3][3];
    let pdop_arg = qe + qn + qu;
    let hdop_arg = qe + qn;
    let vdop_arg = qu;
    let tdop_arg = qt;
    for arg in [gdop_arg, pdop_arg, hdop_arg, vdop_arg, tdop_arg] {
        // `!(arg >= 0.0)` (not `arg < 0.0`) so a NaN variance is also rejected.
        #[allow(clippy::neg_cmp_op_on_partial_ord)]
        let nonpositive_or_nan = !(arg >= 0.0);
        if nonpositive_or_nan || !arg.is_finite() {
            return Err(DopError::Singular);
        }
    }

    Ok(Dop {
        gdop: gdop_arg.sqrt(),
        pdop: pdop_arg.sqrt(),
        hdop: hdop_arg.sqrt(),
        vdop: vdop_arg.sqrt(),
        tdop: tdop_arg.sqrt(),
    })
}

// --- multi-system DOP (general (3 + n_systems) x (3 + n_systems)) -----------

/// `R Q R^T` for a 3x3 position cofactor block, formed as `(R Q) R^T` with
/// fixed-order inner sums (the multi-system counterpart of [`rotate_pos_block`]).
#[allow(clippy::needless_range_loop)]
fn rotate3(q: &[[f64; 3]; 3], r: &[[f64; 3]; 3]) -> [[f64; 3]; 3] {
    let mut rq = [[0.0_f64; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            let mut s = 0.0_f64;
            for k in 0..3 {
                s += r[i][k] * q[k][j];
            }
            rq[i][j] = s;
        }
    }
    let mut enu = [[0.0_f64; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            let mut s = 0.0_f64;
            for k in 0..3 {
                s += rq[i][k] * r[j][k];
            }
            enu[i][j] = s;
        }
    }
    enu
}

/// Inverse of a symmetric positive-definite matrix by Cholesky factorisation,
/// for any size. Returns `None` when the matrix is not positive definite (a
/// rank-deficient geometry), which the caller maps to [`DopError::Singular`].
///
/// This is deterministic scalar arithmetic (no BLAS), but unlike the 4x4
/// cofactor path it is NOT pinned to a 0-ULP golden: it backs the multi-system
/// DOP, a geometry diagnostic, not a parity target.
///
/// The triangular factor/solve loops index by row and column deliberately
/// (the recurrences read against the matrix structure), so the index-based form
/// is kept rather than rewritten as iterators.
#[allow(clippy::needless_range_loop)]
fn invert_symmetric_pd(n: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let p = n.len();
    // Cholesky: N = L L^T, with L lower-triangular.
    let mut l = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..=i {
            let mut s = n[i][j];
            for k in 0..j {
                s -= l[i][k] * l[j][k];
            }
            if i == j {
                #[allow(clippy::neg_cmp_op_on_partial_ord)]
                let nonpositive_or_nan = !(s > 0.0);
                if nonpositive_or_nan || !s.is_finite() {
                    return None;
                }
                l[i][j] = s.sqrt();
            } else {
                l[i][j] = s / l[j][j];
            }
        }
    }
    // Invert the lower-triangular L by forward substitution.
    let mut li = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        li[i][i] = 1.0 / l[i][i];
        for j in 0..i {
            let mut s = 0.0_f64;
            for k in j..i {
                s -= l[i][k] * li[k][j];
            }
            li[i][j] = s / l[i][i];
        }
    }
    // N^-1 = (L^-1)^T (L^-1): inv[i][j] = sum_k li[k][i] * li[k][j].
    let mut inv = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..p {
            let mut s = 0.0_f64;
            for k in 0..p {
                s += li[k][i] * li[k][j];
            }
            inv[i][j] = s;
        }
    }
    Some(inv)
}

/// Multi-system dilution of precision: like [`dop`] but with one receiver-clock
/// column per GNSS rather than a single shared clock.
///
/// `clock_index[i]` is the clock column (`0..n_clocks`) for satellite `i` — its
/// system's index in the solve's clock ordering. The design row is therefore
/// `[-e_x, -e_y, -e_z, <one-hot over the n_clocks clock columns>]` and the
/// cofactor matrix is `(3 + n_clocks) x (3 + n_clocks)`. `PDOP`/`HDOP`/`VDOP` are
/// the position block (ENU-rotated, unambiguous); `GDOP` is the square root of
/// the full trace (all clocks); `TDOP` is the reference-system clock
/// (`Q[3][3]`). Returns [`DopError::Singular`] for a rank-deficient geometry.
///
/// This path uses a general symmetric inverse (see [`invert_symmetric_pd`]) and
/// is a deterministic geometry diagnostic, not a 0-ULP parity target; the
/// single-system [`dop`] retains the 0-ULP cofactor inverse.
///
/// Crate-internal: every `clock_index` must be in `0..n_clocks` (the solver
/// constructs them from the used satellites' system ordering, so this always
/// holds). It is not part of the public API because the index convention is
/// meaningless without the solver's clock ordering.
pub(crate) fn dop_multi(
    los: &[LineOfSight],
    clock_index: &[usize],
    n_clocks: usize,
    weights: &[f64],
    receiver: Wgs84Geodetic,
) -> Result<Dop, DopError> {
    assert_eq!(
        los.len(),
        weights.len(),
        "line-of-sight and weight counts differ"
    );
    assert_eq!(
        los.len(),
        clock_index.len(),
        "line-of-sight and clock-index counts differ"
    );
    assert!(n_clocks >= 1, "at least one clock");
    debug_assert!(
        clock_index.iter().all(|&c| c < n_clocks),
        "clock index out of range 0..n_clocks"
    );
    let p = 3 + n_clocks;
    if los.len() < p {
        return Err(DopError::TooFewSatellites);
    }

    let mut a = vec![vec![0.0_f64; p]; p];
    for k in 0..los.len() {
        let mut row = vec![0.0_f64; p];
        row[0] = -los[k].e_x;
        row[1] = -los[k].e_y;
        row[2] = -los[k].e_z;
        row[3 + clock_index[k]] = 1.0;
        let w = weights[k];
        #[allow(clippy::needless_range_loop)]
        for i in 0..p {
            for j in 0..p {
                a[i][j] += row[i] * w * row[j];
            }
        }
    }
    let q = invert_symmetric_pd(&a).ok_or(DopError::Singular)?;

    let r = ecef_to_enu_rotation(receiver.lat_rad, receiver.lon_rad);
    let qpos = [
        [q[0][0], q[0][1], q[0][2]],
        [q[1][0], q[1][1], q[1][2]],
        [q[2][0], q[2][1], q[2][2]],
    ];
    let enu = rotate3(&qpos, &r);

    let qe = enu[0][0];
    let qn = enu[1][1];
    let qu = enu[2][2];
    let qt = q[3][3];
    let trace: f64 = (0..p).map(|i| q[i][i]).sum();

    let gdop_arg = trace;
    let pdop_arg = qe + qn + qu;
    let hdop_arg = qe + qn;
    let vdop_arg = qu;
    let tdop_arg = qt;
    for arg in [gdop_arg, pdop_arg, hdop_arg, vdop_arg, tdop_arg] {
        #[allow(clippy::neg_cmp_op_on_partial_ord)]
        let nonpositive_or_nan = !(arg >= 0.0);
        if nonpositive_or_nan || !arg.is_finite() {
            return Err(DopError::Singular);
        }
    }

    Ok(Dop {
        gdop: gdop_arg.sqrt(),
        pdop: pdop_arg.sqrt(),
        hdop: hdop_arg.sqrt(),
        vdop: vdop_arg.sqrt(),
        tdop: tdop_arg.sqrt(),
    })
}

#[cfg(test)]
pub(crate) mod test_support {
    //! Internal accessors so the parity test can assert 0 ULP on the
    //! intermediates (normal matrix, cofactor matrix, ENU block) as well as the
    //! final scalars, without making them part of the public API.
    use super::*;

    pub(crate) fn normal_matrix_for(los: &[LineOfSight], weights: &[f64]) -> [[f64; 4]; 4] {
        let rows: Vec<[f64; 4]> = los.iter().map(|l| l.design_row()).collect();
        normal_matrix(&rows, weights)
    }

    pub(crate) fn det4_for(a: &[[f64; 4]; 4]) -> f64 {
        det4(a)
    }

    pub(crate) fn inv4_for(a: &[[f64; 4]; 4]) -> Option<[[f64; 4]; 4]> {
        inv4(a)
    }

    pub(crate) fn enu_block_for(q: &[[f64; 4]; 4], lat_rad: f64, lon_rad: f64) -> [[f64; 3]; 3] {
        let r = ecef_to_enu_rotation(lat_rad, lon_rad);
        rotate_pos_block(q, &r)
    }
}

#[cfg(test)]
mod tests;