sidereon_core/dop/

mod.rs

//! 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::astro::math::linear::{
    invert_4x4_cofactor, invert_symmetric_pd, normal_matrix_4_weighted_column_outer, LinearError,
};

use crate::frame::Wgs84Geodetic;

const DEG_TO_RAD: f64 = std::f64::consts::PI / 180.0;
const LOS_UNIT_TOLERANCE: f64 = 1.0e-3;

/// 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]
    }
}

/// Construct an ECEF line-of-sight unit vector from topocentric azimuth and
/// elevation in degrees.
///
/// Azimuth is clockwise from geodetic north, elevation is positive above the
/// local horizon, and the receiver latitude/longitude define the local ENU
/// frame. The returned vector points from receiver to satellite in ECEF.
pub fn line_of_sight_from_az_el_deg(
    azimuth_deg: f64,
    elevation_deg: f64,
    receiver: Wgs84Geodetic,
) -> Result<LineOfSight, DopError> {
    validate_az_el_receiver(azimuth_deg, elevation_deg, receiver)?;
    let az = azimuth_deg * DEG_TO_RAD;
    let el = elevation_deg * DEG_TO_RAD;
    let cos_el = el.cos();
    let east = cos_el * az.sin();
    let north = cos_el * az.cos();
    let up = el.sin();

    let r = ecef_to_enu_rotation(receiver.lat_rad, receiver.lon_rad);
    let e_x = r[0][0] * east + r[1][0] * north + r[2][0] * up;
    let e_y = r[0][1] * east + r[1][1] * north + r[2][1] * up;
    let e_z = r[0][2] * east + r[1][2] * north + r[2][2] * up;
    let los = LineOfSight::new(e_x, e_y, e_z);
    validate_los(core::slice::from_ref(&los))?;
    Ok(los)
}

/// 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 {
    /// A boundary input was malformed before DOP could be evaluated.
    InvalidInput {
        /// Name of the malformed field.
        field: &'static str,
        /// Stable validation reason.
        reason: &'static str,
    },
    /// 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::InvalidInput { field, reason } => {
                write!(f, "invalid DOP input {field}: {reason}")
            }
            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 {}

/// 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 non-negative 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> {
    validate_dop_inputs(los, weights, receiver)?;
    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_4_weighted_column_outer(&rows, weights).map_err(map_linear_error)?;
    let q = invert_4x4_cofactor(&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(),
    })
}

fn validate_dop_inputs(
    los: &[LineOfSight],
    weights: &[f64],
    receiver: Wgs84Geodetic,
) -> Result<(), DopError> {
    if los.len() != weights.len() {
        return Err(invalid_input("weights", "length must match los"));
    }
    validate_los(los)?;
    validate_weights(weights)?;
    validate_receiver(receiver)
}

fn validate_los(los: &[LineOfSight]) -> Result<(), DopError> {
    for line in los {
        if !(line.e_x.is_finite() && line.e_y.is_finite() && line.e_z.is_finite()) {
            return Err(invalid_input("los", "not finite"));
        }
        let norm = (line.e_x * line.e_x + line.e_y * line.e_y + line.e_z * line.e_z).sqrt();
        if !norm.is_finite() {
            return Err(invalid_input("los", "not finite"));
        }
        if (norm - 1.0).abs() > LOS_UNIT_TOLERANCE {
            return Err(invalid_input("los", "not unit length"));
        }
    }
    Ok(())
}

fn validate_weights(weights: &[f64]) -> Result<(), DopError> {
    if weights.iter().any(|weight| !weight.is_finite()) {
        return Err(invalid_input("weights", "not finite"));
    }
    if weights.iter().any(|&weight| weight < 0.0) {
        return Err(invalid_input("weights", "negative"));
    }
    Ok(())
}

fn validate_receiver(receiver: Wgs84Geodetic) -> Result<(), DopError> {
    if !(receiver.lat_rad.is_finite()
        && receiver.lon_rad.is_finite()
        && receiver.height_m.is_finite())
    {
        return Err(invalid_input("receiver", "not finite"));
    }
    if !(-core::f64::consts::FRAC_PI_2..=core::f64::consts::FRAC_PI_2).contains(&receiver.lat_rad) {
        return Err(invalid_input("receiver.lat_rad", "out of range"));
    }
    if !(-core::f64::consts::PI..=core::f64::consts::PI).contains(&receiver.lon_rad) {
        return Err(invalid_input("receiver.lon_rad", "out of range"));
    }
    Ok(())
}

fn validate_az_el_receiver(
    azimuth_deg: f64,
    elevation_deg: f64,
    receiver: Wgs84Geodetic,
) -> Result<(), DopError> {
    if !azimuth_deg.is_finite() {
        return Err(invalid_input("azimuth_deg", "not finite"));
    }
    if !elevation_deg.is_finite() {
        return Err(invalid_input("elevation_deg", "not finite"));
    }
    if !(-90.0..=90.0).contains(&elevation_deg) {
        return Err(invalid_input("elevation_deg", "out of range"));
    }
    validate_receiver(receiver)
}

fn invalid_input(field: &'static str, reason: &'static str) -> DopError {
    DopError::InvalidInput { field, reason }
}

fn map_linear_error(error: LinearError) -> DopError {
    match error {
        LinearError::InvalidInput { field, reason } => invalid_input(field, reason),
    }
}

// --- 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
}

/// 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> {
    validate_dop_multi_inputs(los, clock_index, n_clocks, weights, receiver)?;
    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(),
    })
}

fn validate_dop_multi_inputs(
    los: &[LineOfSight],
    clock_index: &[usize],
    n_clocks: usize,
    weights: &[f64],
    receiver: Wgs84Geodetic,
) -> Result<(), DopError> {
    if los.len() != weights.len() {
        return Err(invalid_input("weights", "length must match los"));
    }
    if los.len() != clock_index.len() {
        return Err(invalid_input("clock_index", "length must match los"));
    }
    if n_clocks == 0 {
        return Err(invalid_input("n_clocks", "not positive"));
    }
    if clock_index.iter().any(|&idx| idx >= n_clocks) {
        return Err(invalid_input("clock_index", "out of range"));
    }
    validate_los(los)?;
    validate_weights(weights)?;
    validate_receiver(receiver)
}

#[cfg(all(test, sidereon_repo_tests))]
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_4_weighted_column_outer(&rows, weights).expect("valid DOP test inputs")
    }

    pub(crate) fn det4_for(a: &[[f64; 4]; 4]) -> f64 {
        crate::astro::math::linear::det4_cofactor(a)
    }

    pub(crate) fn inv4_for(a: &[[f64; 4]; 4]) -> Option<[[f64; 4]; 4]> {
        invert_4x4_cofactor(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(all(test, sidereon_repo_tests))]
mod tests;