astrodynamics 0.11.0

//! Robust M-estimation primitives for iteratively reweighted least squares.
//!
//! These are the per-outer-iteration reweighting pieces of a Huber IRLS loop:
//! a median-absolute-deviation scale estimate and the Huber weight function.
//! They are deliberately pure `f64` arithmetic (abs, compare, divide, sort by
//! [`f64::total_cmp`]) with no fused-multiply-add and no contraction, so the
//! per-iteration weight vector is bit-reproducible against an explicit
//! outer-loop reference recipe. The trust-region linear-algebra step that
//! consumes the weights is BLAS-bound and is NOT a 0-ULP target.

/// The default Huber tuning constant. Residuals scaled below this (in units of
/// the robust scale) keep full weight; larger ones are down-weighted as
/// `k / |u|`. `1.345` gives ~95% efficiency at the Gaussian model.
pub const HUBER_K: f64 = 1.345;

/// The MAD-to-sigma consistency constant for a normal distribution,
/// `1 / Phi^-1(3/4)`. Multiplying the median absolute deviation by this makes
/// it a consistent estimator of the standard deviation under normality.
pub const MAD_NORMAL_CONST: f64 = 1.4826;

/// The median of `values`, computed on a `total_cmp` sort so the order (and
/// thus the result for an even count, which averages the two central values) is
/// deterministic and NaN-safe. An empty slice yields `0.0`. The averaging of
/// the two central elements is a single `(a + b) / 2.0`, no FMA.
pub fn median(values: &[f64]) -> f64 {
    if values.is_empty() {
        return 0.0;
    }
    let mut v: Vec<f64> = values.to_vec();
    v.sort_by(|a, b| a.total_cmp(b));
    let n = v.len();
    if n % 2 == 1 {
        v[n / 2]
    } else {
        (v[n / 2 - 1] + v[n / 2]) / 2.0
    }
}

/// The median-absolute-deviation scale of `residuals`, scaled to a normal-sigma
/// estimate and floored at `scale_floor`.
///
/// `s = max(scale_floor, MAD_NORMAL_CONST * median(|r_i - median(r)|))`. The
/// floor prevents a near-perfect fit (MAD approaching zero) from blowing up the
/// scaled residuals `u_i = r_i / s` and spuriously down-weighting every
/// observation. Both medians use [`median`]'s `total_cmp` sort.
pub fn mad_scale(residuals: &[f64], scale_floor: f64) -> f64 {
    let med = median(residuals);
    let abs_dev: Vec<f64> = residuals.iter().map(|r| (r - med).abs()).collect();
    let mad = median(&abs_dev);
    let scaled = MAD_NORMAL_CONST * mad;
    if scaled > scale_floor {
        scaled
    } else {
        scale_floor
    }
}

/// The Huber weight for a scaled residual `u = r / s`.
///
/// `w(u) = 1` for `|u| <= k` and `w(u) = k / |u|` otherwise (the Huber
/// `psi(u) / u` form, always in `(0, 1]`). At `u == 0` the weight is `1`. This
/// is the multiplier applied on top of any base (elevation) weight to obtain the
/// effective per-observation weight of the current outer iteration.
pub fn huber_weight(u: f64, k: f64) -> f64 {
    let a = u.abs();
    if a <= k {
        1.0
    } else {
        k / a
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn median_odd_even() {
        assert_eq!(median(&[3.0, 1.0, 2.0]), 2.0);
        assert_eq!(median(&[1.0, 2.0, 3.0, 4.0]), 2.5);
        assert_eq!(median(&[]), 0.0);
    }

    #[test]
    fn huber_weight_breaks_at_k() {
        assert_eq!(huber_weight(0.0, HUBER_K), 1.0);
        assert_eq!(huber_weight(HUBER_K, HUBER_K), 1.0);
        let w = huber_weight(2.0 * HUBER_K, HUBER_K);
        assert!((w - 0.5).abs() < 1e-15);
    }

    #[test]
    fn mad_scale_floored() {
        // All-equal residuals give MAD 0, so the floor governs.
        assert_eq!(mad_scale(&[5.0, 5.0, 5.0], 0.25), 0.25);
    }
}