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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#![allow(non_snake_case)]
use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector, RealField};
use crate::noise::CorrelatedNoise;
pub trait Estimator<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D>,
{
fn state(&self) -> Result<OVector<N, D>, &'static str>;
}
pub trait FunctionalPredictor<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>
{
fn predict(
&mut self, f: impl Fn(&OVector<N, D>) -> OVector<N, D>,
noise: &CorrelatedNoise<N, D>)
-> Result<(), &'static str>;
}
pub trait FunctionalObserver<N: RealField, D: Dim, ZD: Dim>
where
DefaultAllocator:
Allocator<N, ZD, D> + Allocator<N, ZD, ZD> + Allocator<N, D> + Allocator<N, ZD>
{
fn observe(
&mut self,
z: &OVector<N, ZD>,
h: impl Fn(&OVector<N, D>) -> OVector<N, ZD>,
noise: &CorrelatedNoise<N, ZD>)
-> Result<(), &'static str>;
}
#[derive(PartialEq, Clone)]
pub struct KalmanState<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
pub x: OVector<N, D>,
pub X: OMatrix<N, D, D>,
}
#[derive(PartialEq, Clone)]
pub struct InformationState<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
pub i: OVector<N, D>,
pub I: OMatrix<N, D, D>,
}
pub trait KalmanEstimator<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
fn init(&mut self, state: &KalmanState<N, D>) -> Result<(), &'static str>;
fn kalman_state(&self) -> Result<KalmanState<N, D>, &'static str>;
}
pub trait ExtendedLinearPredictor<N: RealField, D: Dim>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>
{
fn predict(
&mut self,
x_pred: &OVector<N, D>,
Fx: &OMatrix<N, D, D>,
noise: &CorrelatedNoise<N, D>,
) -> Result<(), &'static str>;
}
pub trait ExtendedLinearObserver<N: RealField, D: Dim, ZD: Dim>
where
DefaultAllocator:
Allocator<N, ZD, D> + Allocator<N, ZD, ZD> + Allocator<N, ZD>
{
fn observe_innovation(
&mut self,
s: &OVector<N, ZD>,
Hx: &OMatrix<N, ZD, D>,
noise: &CorrelatedNoise<N, ZD>,
) -> Result<(), &'static str>;
}