numeris 0.5.2

//! State estimation: EKF, UKF, SR-UKF, CKF, RTS smoother, batch least-squares.
//!
//! Filters use closure-based dynamics and measurement models with const-generic
//! state (`N`) and measurement (`M`) dimensions. The EKF and `BatchLsq` are
//! fully no-std compatible; the sigma-point filters (UKF, SR-UKF, CKF) and
//! the RTS smoother require `alloc` for temporary storage.
//!
//! # Extended Kalman Filter
//!
//! ```
//! use numeris::estimate::Ekf;
//! use numeris::{Vector, Matrix};
//!
//! // 2-state constant-velocity, 1 measurement (position only)
//! let x0 = Vector::from_array([0.0_f64, 1.0]); // [pos, vel]
//! let p0 = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
//! let mut ekf = Ekf::<f64, 2, 1>::new(x0, p0);
//!
//! let dt = 0.1;
//! let q = Matrix::new([[0.01, 0.0], [0.0, 0.01]]);
//! let r = Matrix::new([[0.5]]);
//!
//! // Predict with explicit Jacobian
//! ekf.predict(
//!     |x| Vector::from_array([x[0] + dt * x[1], x[1]]),
//!     |_x| Matrix::new([[1.0, dt], [0.0, 1.0]]),
//!     Some(&q),
//! );
//!
//! // Update with measurement
//! ekf.update(
//!     &Vector::from_array([0.12]),
//!     |x| Vector::from_array([x[0]]),
//!     |_x| Matrix::new([[1.0, 0.0]]),
//!     &r,
//! ).unwrap();
//! ```
//!
//! # Unscented Kalman Filter
//!
//! ```
//! use numeris::estimate::Ukf;
//! use numeris::{Vector, Matrix};
//!
//! let x0 = Vector::from_array([0.0_f64, 1.0]);
//! let p0 = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
//! let mut ukf = Ukf::<f64, 2, 1>::new(x0, p0);
//!
//! let dt = 0.1;
//! let q = Matrix::new([[0.01, 0.0], [0.0, 0.01]]);
//! let r = Matrix::new([[0.5]]);
//!
//! ukf.predict(
//!     |x| Vector::from_array([x[0] + dt * x[1], x[1]]),
//!     Some(&q),
//! ).unwrap();
//!
//! ukf.update(
//!     &Vector::from_array([0.12]),
//!     |x| Vector::from_array([x[0]]),
//!     &r,
//! ).unwrap();
//! ```
//!
//! # Batch Least-Squares (no-std)
//!
//! ```
//! use numeris::estimate::BatchLsq;
//! use numeris::{Vector, Matrix};
//!
//! let mut lsq = BatchLsq::<f64, 1>::new();
//!
//! // Accumulate scalar observations: z = x + noise
//! let h = Matrix::new([[1.0_f64]]);
//! let r = Matrix::new([[0.1]]);
//! lsq.add_observation(&Vector::from_array([1.05]), &h, &r).unwrap();
//! lsq.add_observation(&Vector::from_array([0.95]), &h, &r).unwrap();
//!
//! let (x, p) = lsq.solve().unwrap();
//! assert!((x[0] - 1.0).abs() < 0.1);
//! ```

mod ekf;
#[cfg(feature = "alloc")]
mod ukf;
mod cholupdate;
#[cfg(feature = "alloc")]
mod srukf;
#[cfg(feature = "alloc")]
mod ckf;
#[cfg(feature = "alloc")]
mod rts;
mod batch;

#[cfg(test)]
mod tests;

pub use ekf::Ekf;
#[cfg(feature = "alloc")]
pub use ukf::Ukf;
#[cfg(feature = "alloc")]
pub use srukf::SrUkf;
#[cfg(feature = "alloc")]
pub use ckf::Ckf;
#[cfg(feature = "alloc")]
pub use rts::{EkfStep, rts_smooth};
pub use batch::BatchLsq;

use crate::linalg::CholeskyDecomposition;
use crate::matrix::vector::Vector;
use crate::traits::FloatScalar;
use crate::Matrix;

/// Errors from state estimation algorithms.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum EstimateError {
    /// Covariance matrix is not positive definite (Cholesky failed).
    CovarianceNotPD,
    /// Innovation covariance is singular (cannot compute Kalman gain).
    SingularInnovation,
    /// Cholesky downdate produced a non-positive-definite result.
    CholdowndateFailed,
}

impl core::fmt::Display for EstimateError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            EstimateError::CovarianceNotPD => {
                write!(f, "covariance matrix is not positive definite")
            }
            EstimateError::SingularInnovation => {
                write!(f, "innovation covariance is singular")
            }
            EstimateError::CholdowndateFailed => {
                write!(f, "Cholesky downdate failed: result not positive definite")
            }
        }
    }
}

/// Attempt Cholesky decomposition, retrying with increasing diagonal jitter if needed.
///
/// Accumulated floating-point errors can make a mathematically positive-definite matrix
/// appear non-PD by ~1e-14. Rather than failing immediately, this tries adding
/// `ε·I` for ε ∈ {1e-9, 1e-7, 1e-5} before giving up.
pub(crate) fn cholesky_with_jitter<T: FloatScalar, const N: usize>(
    p: &Matrix<T, N, N>,
) -> Result<CholeskyDecomposition<T, N>, EstimateError> {
    if let Ok(c) = CholeskyDecomposition::new(p) {
        return Ok(c);
    }
    for exp in [9u32, 7, 5] {
        let eps = T::from(10.0f64.powi(-(exp as i32))).unwrap();
        let mut pj = *p;
        for i in 0..N {
            pj[(i, i)] = pj[(i, i)] + eps;
        }
        if let Ok(c) = CholeskyDecomposition::new(&pj) {
            return Ok(c);
        }
    }
    Err(EstimateError::CovarianceNotPD)
}

/// Clamp the diagonal of `p` so that `p[i,i] >= min_var` for all `i`.
///
/// A no-op when `min_var <= 0`. Prevents the covariance from degenerating to
/// zero or going negative after many updates.
pub(crate) fn apply_var_floor<T: FloatScalar, const N: usize>(
    p: &mut Matrix<T, N, N>,
    min_var: T,
) {
    if min_var > T::zero() {
        for i in 0..N {
            if p[(i, i)] < min_var {
                p[(i, i)] = min_var;
            }
        }
    }
}

/// Forward-difference Jacobian of `f: Vector<T,N> → Vector<T,M>`.
///
/// Uses step size `h_j = sqrt(ε) * max(|x_j|, 1)` for each component.
pub(crate) fn fd_jacobian<T: FloatScalar, const N: usize, const M: usize>(
    f: &impl Fn(&Vector<T, N>) -> Vector<T, M>,
    x: &Vector<T, N>,
) -> Matrix<T, M, N> {
    let sqrt_eps = T::epsilon().sqrt();
    let f0 = f(x);
    let mut jac = Matrix::<T, M, N>::zeros();

    for j in 0..N {
        let xj = x[j];
        let h = sqrt_eps * xj.abs().max(T::one());
        let mut x_pert = *x;
        x_pert[j] = xj + h;
        let f_pert = f(&x_pert);

        for i in 0..M {
            jac[(i, j)] = (f_pert[i] - f0[i]) / h;
        }
    }

    jac
}