nyx_space/od/kalman/
initializers.rs1use nalgebra::DimName;
20
21pub use crate::errors::NyxError;
22use crate::linalg::allocator::Allocator;
23use crate::linalg::{DefaultAllocator, U3};
24pub use crate::od::estimate::{Estimate, KfEstimate, Residual};
25pub use crate::od::snc::ProcessNoise;
26use crate::od::State;
27pub use crate::time::{Epoch, Unit};
28
29use super::{KalmanFilter, KalmanVariant};
30
31impl<T> KalmanFilter<T, U3>
32where
33 T: State,
34 DefaultAllocator: Allocator<<T as State>::Size>
35 + Allocator<<T as State>::VecLength>
36 + Allocator<U3>
37 + Allocator<<T as State>::Size, <T as State>::Size>
38 + Allocator<U3, U3>
39 + Allocator<<T as State>::Size, U3>
40 + Allocator<U3, <T as State>::Size>,
41 <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
42 <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
43{
44 pub fn new(initial_estimate: KfEstimate<T>, variant: KalmanVariant) -> Self {
46 Self {
47 prev_estimate: initial_estimate,
48 process_noise: vec![],
49 variant,
50 prev_used_snc: 0,
51 }
52 }
53}
54
55impl<T, A> KalmanFilter<T, A>
56where
57 T: State,
58 A: DimName,
59 DefaultAllocator: Allocator<<T as State>::Size>
60 + Allocator<<T as State>::VecLength>
61 + Allocator<A>
62 + Allocator<<T as State>::Size, <T as State>::Size>
63 + Allocator<A, A>
64 + Allocator<<T as State>::Size, A>
65 + Allocator<A, <T as State>::Size>,
66 <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
67 <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
68{
69 pub fn from_process_noise(
71 initial_estimate: KfEstimate<T>,
72 variant: KalmanVariant,
73 mut process_noise: ProcessNoise<A>,
74 ) -> Self {
75 process_noise.init_epoch = Some(initial_estimate.epoch());
76 Self {
77 prev_estimate: initial_estimate,
78 process_noise: vec![process_noise],
79 variant,
80 prev_used_snc: 0,
81 }
82 }
83
84 pub fn with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
86 process_noise.init_epoch = Some(self.prev_estimate.epoch());
87 self.process_noise.clear();
88 self.process_noise.push(process_noise);
89 self
90 }
91
92 pub fn and_with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
94 process_noise.init_epoch = Some(self.prev_estimate.epoch());
95 self.process_noise.push(process_noise);
96 self
97 }
98
99 pub(crate) fn initialize_process_noises(&mut self) {
100 for process_noise in &mut self.process_noise {
101 process_noise.init_epoch = Some(self.prev_estimate.epoch());
102 }
103 }
104}