yakf 0.1.12

Yet Another Kalman Filter Implementation, as well as, Lie Theory (Lie group, algebra, vector) on SO(2), SO(3), SE(2) and SE(3). [no_std] is supported by default.
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

use crate::time::{Duration, Epoch};

use crate::linalg::allocator::Allocator;
use crate::linalg::{DefaultAllocator, DimName, OVector};

/// `State` trait.
///
/// `T`: state vector dimension. e.g., `Const<3>` stands for 3-D
///
/// `U`: external vector dimension (You should assign any DimName to it, though it is not in-use currently)
pub trait State<T, U>
where
    T: DimName,
    U: DimName,
    DefaultAllocator: Allocator<f64, U> + Allocator<f64, T>,
{
    /// get the state vec
    fn state(&self) -> &OVector<f64, T> {
        unimplemented!()
    }

    /// set the state vec
    fn set_state(&mut self, _state: OVector<f64, T>) {
        unimplemented!()
    }

    /// get the epoch
    fn epoch(&self) -> Epoch {
        unimplemented!()
    }

    /// set the epoch
    fn set_epoch(&mut self, _epoch: Epoch) {
        unimplemented!()
    }

    /// propagate
    fn propagate(
        &mut self,
        dynamics: &dyn Fn(&OVector<f64, T>, &OVector<f64, U>, Duration) -> OVector<f64, T>,
        dt: Duration,
        external: OVector<f64, U>,
    ) {
        self.set_state(dynamics(&self.state(), &external, dt));
        self.set_epoch(self.epoch() + dt);
    }
}