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

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

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

    /// Creates a CorrelatedNoise from an UncorrelatedNoise.
    pub fn from_uncorrelated(uncorrelated: &'a UncorrelatedNoise<N, D>) -> 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 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 the Cholesky factor of a 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> {
        let d = correlated.Q.shape_generic().0;
        let cholesky = correlated.Q.clone().cholesky().ok_or("Q not PSD")?;

        Ok(CoupledNoise {
            q: OVector::repeat_generic(d, Const::<1>, N::one()),
            G: cholesky.l(),
        })
    }
}