rudolf/
base.rs

1use nalgebra::{RealField, SMatrix, SVector};
2
3#[derive(Debug)]
4pub struct State<T, const S: usize>
5where
6    T: RealField,
7{
8    pub mean: SVector<T, S>,
9    pub cov: SMatrix<T, S, S>,
10}
11
12#[derive(Debug)]
13pub struct Noise<T, const S: usize, const O: usize>
14where
15    T: RealField,
16{
17    pub ctrl: SMatrix<T, S, S>,
18    pub obs: SMatrix<T, O, O>,
19}
20
21#[derive(Debug)]
22pub struct Model<T, const S: usize, const O: usize>
23where
24    T: RealField,
25{
26    pub state: SMatrix<T, S, S>,
27    pub ctrl: SMatrix<T, S, S>,
28    pub obs: SMatrix<T, O, S>,
29}
30
31#[derive(Debug)]
32pub struct NonLinearModel<T, const S: usize, const O: usize>
33where
34    T: RealField,
35{
36    pub state: fn(SVector<T, S>, T) -> SVector<T, S>,
37    pub ctrl: fn(SVector<T, S>, T) -> SVector<T, S>,
38    pub obs: fn(SVector<T, O>, T) -> SVector<T, O>,
39}
40
41pub trait Filter<T, const S: usize, const O: usize>
42where
43    T: RealField,
44{
45    fn predict(&mut self, state: &State<T, S>, ctrl: &SVector<T, S>) -> State<T, S>;
46    fn update(&mut self, state: &State<T, S>, obs: &SVector<T, O>) -> State<T, S>;
47}
48
49#[derive(Debug)]
50pub struct SigmaPoints<T, const S: usize, const N: usize>
51where
52    T: RealField,
53{
54    pub mean_weights: SVector<T, N>,
55    pub cov_weights: SVector<T, N>,
56    pub sigmas: SMatrix<T, N, S>,
57}
58
59pub trait SigmaPointGenerator<T, const S: usize, const N: usize>
60where
61    T: RealField,
62{
63    fn generate_sigmas(&self, mean: &SVector<T, S>, cov: &SMatrix<T, S, S>)
64        -> SigmaPoints<T, S, N>;
65}
rudolf/base.rs

rudolf/
base.rs
