adskalman 0.17.0

Kalman filter and Rauch-Tung-Striebel smoothing implementation using nalgebra, no_std
Documentation 
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

use nalgebra::{
    allocator::Allocator, base::storage::Owned, DefaultAllocator, Dim, Matrix, OMatrix, OVector,
    RealField, Vector,
};

/// State and covariance pair for a given estimate
#[derive(Debug, Clone)]
pub struct StateAndCovariance<R, SS>
where
    R: RealField,
    SS: Dim,
    DefaultAllocator: Allocator<SS> + Allocator<SS, SS>,
{
    state: Vector<R, SS, Owned<R, SS>>,
    covariance: Matrix<R, SS, SS, Owned<R, SS, SS>>,
}

impl<R, SS> StateAndCovariance<R, SS>
where
    R: RealField,
    SS: Dim,
    DefaultAllocator: Allocator<SS> + Allocator<SS, SS>,
{
    /// Create a new `StateAndCovariance`.
    ///
    /// It is assumed that the covariance matrix is symmetric and positive
    /// semi-definite.
    pub fn new(
        state: Vector<R, SS, Owned<R, SS>>,
        covariance: Matrix<R, SS, SS, Owned<R, SS, SS>>,
    ) -> Self {
        // In theory, checks could be run to ensure the covariance matrix is
        // both symmetric and positive semi-definite. The Cholesky decomposition
        // could be used to test if it is positive definite. However, matrices
        // which are positive semi-definite but not positive definite are also
        // valid covariance matrices. Thus, if the Cholesky decomposition fails,
        // the eigenvalues could be computed and used to test semi-definiteness
        // (e.g. https://scicomp.stackexchange.com/questions/12979).
        //
        // I have decided that the computational cost is not worth the marginal
        // benefits such testing would bring. If your covariance matrices might
        // not be symmetric and positive semi-definite, test them prior to this.
        Self { state, covariance }
    }
    /// Get a reference to the state vector.
    #[inline]
    pub fn state(&self) -> &Vector<R, SS, Owned<R, SS>> {
        &self.state
    }
    /// Get a mut reference to the state vector.
    #[inline]
    pub fn state_mut(&mut self) -> &mut Vector<R, SS, Owned<R, SS>> {
        &mut self.state
    }
    /// Get a reference to the covariance matrix.
    #[inline]
    pub fn covariance(&self) -> &Matrix<R, SS, SS, Owned<R, SS, SS>> {
        &self.covariance
    }
    /// Get a mutable reference to the covariance matrix.
    #[inline]
    pub fn covariance_mut(&mut self) -> &mut Matrix<R, SS, SS, Owned<R, SS, SS>> {
        &mut self.covariance
    }
    /// Get the state vector and covariance matrix.
    #[inline]
    pub fn inner(self) -> (OVector<R, SS>, OMatrix<R, SS, SS>) {
        (self.state, self.covariance)
    }
}