1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104

#![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::matrix::check_non_negativ;

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

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

/// Additive noise.
///
/// Noise represented as a the 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<N, D, QD> + Allocator<N, 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<N, D, D> + Allocator<N, D>
{
    /// Creates a CorrelatedNoise from an CoupledNoise.
    pub fn from_coupled<QD: Dim>(coupled: &'a CoupledNoise<N, D, QD>) -> Self
    where
        DefaultAllocator: Allocator<N, QD, QD> + Allocator<N, D, QD> + Allocator<N, 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
        }
    }
}

impl<'a, N: Copy + RealField, QD: Dim> CorrelatedNoise<N, QD>
where
    DefaultAllocator: Allocator<N, QD, QD> + Allocator<N, QD>
{
    /// Creates a CorrelatedNoise from an UncorrelatedNoise.
    pub fn from_uncorrelated(uncorrelated: &'a UncorrelatedNoise<N, QD>) -> Self {
        let z_size = uncorrelated.q.shape_generic().0;
        let mut correlated = CorrelatedNoise {
            Q: OMatrix::zeros_generic(z_size, z_size)
        };
        for i in 0..uncorrelated.q.nrows() {
            correlated.Q[(i,i)] = uncorrelated.q[i];
        }

        correlated
    }
}

impl<N: Copy + RealField, D: Dim> CoupledNoise<N, D, D>
    where
        DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
    /// 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_negativ(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
        })
    }
}