bayes_estimate 0.20.0

#![allow(non_snake_case)]

//! Bayesian estimation noise models.
//!
//! Linear noise models are represented as structs.

use nalgebra::{allocator::Allocator, Const, DefaultAllocator, Dim, OMatrix, OVector, RealField};

use crate::cholesky::UDU;
use crate::matrix;
use crate::linalg::rcond::check_non_negative;

/// Additive noise.
///
/// Noise represented as a noise variance vector.
pub struct UncorrelatedNoise<N: RealField, QD: Dim>
where
    DefaultAllocator: Allocator<QD>,
{
    /// Noise variance
    pub q: OVector<N, QD>,
}

/// Additive noise.
///
/// Noise represented as a noise covariance matrix.
pub struct CorrelatedNoise<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D>,
{
    /// Noise covariance
    pub Q: OMatrix<N, D, D>,
}

/// Additive noise.
///
/// Noise represented as a noise variance vector and a noise coupling matrix.
/// The noise covariance is G.q.G'.
pub struct CoupledNoise<N: RealField, D: Dim, QD: Dim>
where
    DefaultAllocator: Allocator<D, QD> + Allocator<QD>,
{
    /// Noise variance
    pub q: OVector<N, QD>,
    /// Noise coupling
    pub G: OMatrix<N, D, QD>,
}

impl<'a, N: Copy + RealField, D: Dim> CorrelatedNoise<N, D>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// Creates a CorrelatedNoise from an CoupledNoise.
    pub fn from_coupled<QD: Dim>(coupled: &'a CoupledNoise<N, D, QD>) -> Self
    where
        DefaultAllocator: Allocator<QD, QD> + Allocator<D, QD> + Allocator<QD>,
    {
        let mut Q = OMatrix::zeros_generic(coupled.G.shape_generic().0, coupled.G.shape_generic().0);
        matrix::quadform_tr(&mut Q, N::one(), &coupled.G, &coupled.q, N::one());
        CorrelatedNoise { Q }
    }

    /// Creates a CorrelatedNoise from an UncorrelatedNoise.
    pub fn from_uncorrelated(uncorrelated: &'a UncorrelatedNoise<N, D>) -> Self
    {
        CorrelatedNoise{ Q: OMatrix::from_diagonal(&uncorrelated.q) }
    }
}

impl<N: Copy + RealField, D: Dim> CoupledNoise<N, D, D>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// Creates a CoupledNoise from an UncorrelatedNoise.
    /// The resulting 'G' is an identity matrix.
    pub fn from_uncorrelated(uncorrelated: UncorrelatedNoise<N, D>) -> Self {
        let nrows = uncorrelated.q.shape_generic().0;
        CoupledNoise {
            q: uncorrelated.q,
            G: OMatrix::identity_generic(nrows, nrows),
        }
    }

    /// Creates a CoupledNoise from an CorrelatedNoise.
    /// The CorrelatedNoise must be PSD.
    /// The resulting 'q' is always a vector of 1s.
    pub fn from_correlated(correlated: &CorrelatedNoise<N, D>) -> Result<Self, &'static str> {
        // Factorise the correlated noise
        let mut uc = correlated.Q.clone();
        let udu = UDU::new();
        let rcond = udu.UCfactor_n(&mut uc, correlated.Q.nrows());
        check_non_negative(rcond, "Q not PSD")?;
        uc.fill_lower_triangle(N::zero(), 1);

        Ok(CoupledNoise {
            q: OVector::repeat_generic(uc.shape_generic().0, Const::<1>, N::one()),
            G: uc,
        })
    }
}

#[cfg(test)]
mod tests {
    use nalgebra::Matrix2;

    use super::*;

    /// Simply make sure semi-definite correlated noise can be factorised
    #[test]
    fn correlated_zero_diagonal() {
        CoupledNoise::from_correlated(&CorrelatedNoise{ Q: Matrix2::<f32>::zeros() }).unwrap();
        CoupledNoise::from_correlated(&CorrelatedNoise{ Q: Matrix2::new(1., 0., 0., 0.) }).unwrap();
        CoupledNoise::from_correlated(&CorrelatedNoise{ Q: Matrix2::new(0., 0., 0., 1.) }).unwrap();
    }

    /// Simply make sure non semi-definite correlated noise cannot be factorised
    #[test]
    #[should_panic(expected = "Q not PSD")]
    fn correlated_not_semi_definite() {
        CoupledNoise::from_correlated(&CorrelatedNoise{ Q: Matrix2::new(0., 1., 1., 1.) }).unwrap();
    }
}