adskalman/
state_and_covariance.rs

1use nalgebra::{
2    allocator::Allocator, base::storage::Owned, DefaultAllocator, Dim, Matrix, OMatrix, OVector,
3    RealField, Vector,
4};
5
6/// State and covariance pair for a given estimate
7#[derive(Debug, Clone)]
8pub struct StateAndCovariance<R, SS>
9where
10    R: RealField,
11    SS: Dim,
12    DefaultAllocator: Allocator<SS> + Allocator<SS, SS>,
13{
14    state: Vector<R, SS, Owned<R, SS>>,
15    covariance: Matrix<R, SS, SS, Owned<R, SS, SS>>,
16}
17
18impl<R, SS> StateAndCovariance<R, SS>
19where
20    R: RealField,
21    SS: Dim,
22    DefaultAllocator: Allocator<SS> + Allocator<SS, SS>,
23{
24    /// Create a new `StateAndCovariance`.
25    ///
26    /// It is assumed that the covariance matrix is symmetric and positive
27    /// semi-definite.
28    pub fn new(
29        state: Vector<R, SS, Owned<R, SS>>,
30        covariance: Matrix<R, SS, SS, Owned<R, SS, SS>>,
31    ) -> Self {
32        // In theory, checks could be run to ensure the covariance matrix is
33        // both symmetric and positive semi-definite. The Cholesky decomposition
34        // could be used to test if it is positive definite. However, matrices
35        // which are positive semi-definite but not positive definite are also
36        // valid covariance matrices. Thus, if the Cholesky decomposition fails,
37        // the eigenvalues could be computed and used to test semi-definiteness
38        // (e.g. https://scicomp.stackexchange.com/questions/12979).
39        //
40        // I have decided that the computational cost is not worth the marginal
41        // benefits such testing would bring. If your covariance matrices might
42        // not be symmetric and positive semi-definite, test them prior to this.
43        Self { state, covariance }
44    }
45    /// Get a reference to the state vector.
46    #[inline]
47    pub fn state(&self) -> &Vector<R, SS, Owned<R, SS>> {
48        &self.state
49    }
50    /// Get a mut reference to the state vector.
51    #[inline]
52    pub fn state_mut(&mut self) -> &mut Vector<R, SS, Owned<R, SS>> {
53        &mut self.state
54    }
55    /// Get a reference to the covariance matrix.
56    #[inline]
57    pub fn covariance(&self) -> &Matrix<R, SS, SS, Owned<R, SS, SS>> {
58        &self.covariance
59    }
60    /// Get a mutable reference to the covariance matrix.
61    #[inline]
62    pub fn covariance_mut(&mut self) -> &mut Matrix<R, SS, SS, Owned<R, SS, SS>> {
63        &mut self.covariance
64    }
65    /// Get the state vector and covariance matrix.
66    #[inline]
67    pub fn inner(self) -> (OVector<R, SS>, OMatrix<R, SS, SS>) {
68        (self.state, self.covariance)
69    }
70}
adskalman/state_and_covariance.rs

adskalman/
state_and_covariance.rs
